<commit_before>// Copyright 2018 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "cc/coord.h"

#include <utility>

#include "absl/strings/ascii.h"
#include "absl/strings/numbers.h"
#include "absl/strings/str_cat.h"

namespace minigo {

constexpr uint16_t Coord::kPass;
constexpr uint16_t Coord::kInvalid;
const char Coord::kKgsColumns[20] = "ABCDEFGHJKLMNOPQRST";

namespace {

Coord TryParseKgs(absl::string_view str) {
  auto upper = absl::AsciiStrToUpper(str);
  if (upper == "PASS") {
    return Coord::kPass;
  }
  if (upper == "RESIGN") {
    return Coord::kResign;
  }

  auto col_char = upper[0];
  if (col_char < 'A' || col_char > 'T' || col_char == 'I') {
    return Coord::kInvalid;
  }
  int col = col_char < 'I' ? col_char - 'A' : 8 + col_char - 'J';

  auto row_str = upper.substr(1);
  int row;
  if (!absl::SimpleAtoi(row_str, &row) || row <= 0 || row > kN) {
    return Coord::kInvalid;
  }
  return {kN - row, col};
}

Coord TryParseSgf(absl::string_view str) {
  if (str.empty()) {
    return Coord::kPass;
  }
  if (str.size() != 2) {
    return Coord::kInvalid;
  }

  int col = str[0] - 'a';
  int row = str[1] - 'a';
  if (row < 0 || row >= kN || col < 0 || col >= kN) {
    return Coord::kInvalid;
  }
  return {row, col};
}

Coord TryParseString(absl::string_view str) {
  auto result = TryParseKgs(str);
  if (result != Coord::kInvalid) {
    return result;
  }
  return TryParseSgf(str);
}

}  // namespace

Coord Coord::FromKgs(absl::string_view str, bool allow_invalid) {
  auto c = TryParseKgs(str);
  MG_CHECK(allow_invalid || c != Coord::kInvalid);
  return c;
}

Coord Coord::FromSgf(absl::string_view str, bool allow_invalid) {
  auto c = TryParseSgf(str);
  MG_CHECK(allow_invalid || c != Coord::kInvalid);
  return c;
}

Coord Coord::FromString(absl::string_view str, bool allow_invalid) {
  auto c = TryParseString(str);
  MG_CHECK(allow_invalid || c != Coord::kInvalid);
  return c;
}

std::string Coord::ToKgs() const {
  if (*this == kPass) {
    return "pass";
  } else if (*this == kResign) {
    return "resign";
  } else if (*this == kInvalid) {
    return "invalid";
  }
  int row = value_ / kN;
  int col = value_ % kN;
  absl::string_view col_str = kKgsColumns;
  return absl::StrCat(col_str.substr(col, 1), kN - row);
}

std::string Coord::ToSgf() const {
  if (*this == kPass) {
    return "";
  } else if (*this == kInvalid) {
    return "invalid";
  }

  // We should not be writing resign moves to SGF files.
  MG_CHECK(*this != kResign);

  int row = value_ / kN;
  int col = value_ % kN;
  char buffer[2];
  buffer[0] = 'a' + col;
  buffer[1] = 'a' + row;
  return {buffer, 2};
}

std::ostream& operator<<(std::ostream& os, Coord c) {
  if (c == Coord::kPass) {
    return os << "pass";
  } else if (c == Coord::kResign) {
    return os << "resign";
  } else if (c == Coord::kInvalid) {
    return os << "invalid";
  } else {
    uint16_t value = c;
    int row = value / kN;
    int col = value % kN;
    return os << Coord::kKgsColumns[col] << (kN - row);
  }
}

}  // namespace minigo
<commit_msg>Allow [tt] for pass (#588)<commit_after>// Copyright 2018 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "cc/coord.h"

#include <utility>

#include "absl/strings/ascii.h"
#include "absl/strings/numbers.h"
#include "absl/strings/str_cat.h"

namespace minigo {

constexpr uint16_t Coord::kPass;
constexpr uint16_t Coord::kInvalid;
const char Coord::kKgsColumns[20] = "ABCDEFGHJKLMNOPQRST";

namespace {

Coord TryParseKgs(absl::string_view str) {
  auto upper = absl::AsciiStrToUpper(str);
  if (upper == "PASS") {
    return Coord::kPass;
  }
  if (upper == "RESIGN") {
    return Coord::kResign;
  }

  auto col_char = upper[0];
  if (col_char < 'A' || col_char > 'T' || col_char == 'I') {
    return Coord::kInvalid;
  }
  int col = col_char < 'I' ? col_char - 'A' : 8 + col_char - 'J';

  auto row_str = upper.substr(1);
  int row;
  if (!absl::SimpleAtoi(row_str, &row) || row <= 0 || row > kN) {
    return Coord::kInvalid;
  }
  return {kN - row, col};
}

Coord TryParseSgf(absl::string_view str) {
  if (str.empty() || (kN == 19 && str == "tt")) {
    return Coord::kPass;
  }
  if (str.size() != 2) {
    return Coord::kInvalid;
  }

  int col = str[0] - 'a';
  int row = str[1] - 'a';
  if (row < 0 || row >= kN || col < 0 || col >= kN) {
    return Coord::kInvalid;
  }
  return {row, col};
}

Coord TryParseString(absl::string_view str) {
  auto result = TryParseKgs(str);
  if (result != Coord::kInvalid) {
    return result;
  }
  return TryParseSgf(str);
}

}  // namespace

Coord Coord::FromKgs(absl::string_view str, bool allow_invalid) {
  auto c = TryParseKgs(str);
  MG_CHECK(allow_invalid || c != Coord::kInvalid);
  return c;
}

Coord Coord::FromSgf(absl::string_view str, bool allow_invalid) {
  auto c = TryParseSgf(str);
  MG_CHECK(allow_invalid || c != Coord::kInvalid);
  return c;
}

Coord Coord::FromString(absl::string_view str, bool allow_invalid) {
  auto c = TryParseString(str);
  MG_CHECK(allow_invalid || c != Coord::kInvalid);
  return c;
}

std::string Coord::ToKgs() const {
  if (*this == kPass) {
    return "pass";
  } else if (*this == kResign) {
    return "resign";
  } else if (*this == kInvalid) {
    return "invalid";
  }
  int row = value_ / kN;
  int col = value_ % kN;
  absl::string_view col_str = kKgsColumns;
  return absl::StrCat(col_str.substr(col, 1), kN - row);
}

std::string Coord::ToSgf() const {
  if (*this == kPass) {
    return "";
  } else if (*this == kInvalid) {
    return "invalid";
  }

  // We should not be writing resign moves to SGF files.
  MG_CHECK(*this != kResign);

  int row = value_ / kN;
  int col = value_ % kN;
  char buffer[2];
  buffer[0] = 'a' + col;
  buffer[1] = 'a' + row;
  return {buffer, 2};
}

std::ostream& operator<<(std::ostream& os, Coord c) {
  if (c == Coord::kPass) {
    return os << "pass";
  } else if (c == Coord::kResign) {
    return os << "resign";
  } else if (c == Coord::kInvalid) {
    return os << "invalid";
  } else {
    uint16_t value = c;
    int row = value / kN;
    int col = value % kN;
    return os << Coord::kKgsColumns[col] << (kN - row);
  }
}

}  // namespace minigo
<|endoftext|>


<commit_before>#include "ContactPlugin.hh"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <gazebo/sensors/sensors.hh>
#include <string.h>
#include <stdio.h>

#include <sstream>

char word_sep = ' ';

using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(ContactPlugin)

/////////////////////////////////////////////////
ContactPlugin::ContactPlugin() : SensorPlugin()
{
}

/////////////////////////////////////////////////
ContactPlugin::~ContactPlugin()
{
}

ros::Publisher chatter_pub;
int argc;
char **argv;

/////////////////////////////////////////////////
void ContactPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr /*_sdf*/)
{
  // Get the parent sensor.
  this->parentSensor =
    boost::dynamic_pointer_cast<sensors::ContactSensor>(_sensor);

  // Make sure the parent sensor is valid.
  if (!this->parentSensor)
  {
    gzerr << "ContactPlugin requires a ContactSensor.\n";
    return;
  }

  // Connect to the sensor update event.
  this->updateConnection = this->parentSensor->ConnectUpdated(
      boost::bind(&ContactPlugin::OnUpdate, this));

  // Make sure the parent sensor is active.
  this->parentSensor->SetActive(true);

  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  chatter_pub = n.advertise<std_msgs::String>("contact", 1000);

}

/////////////////////////////////////////////////
void ContactPlugin::OnUpdate()
{
  // Get all the contacts.
  msgs::Contacts contacts;

    char g0,g1,g2,g3,g4,g5,g6,g7,g8,g9,g10,g11,g12,g13;
    g0 = g1 = g2 = g3 = g4 = g5 = g6 = g7 = g8 = g9 = g10 = g11 = g12 = g13 = '0';

  contacts = this->parentSensor->GetContacts();
  for (unsigned int i = 0; i < contacts.contact_size(); ++i)
  {
    //std_msgs::String msg1;

    std::stringstream ss2;
    ss2 << "Collision between[" << contacts.contact(i).collision1()
              << "] and [" << contacts.contact(i).collision2() << "]\n";

    //std::display = ss2.str();
    //ROS_INFO("%s",display);

    std::string tmp = contacts.contact(i).collision1();
    char str[32];
    strncpy(str, tmp.c_str(), sizeof(str));
    str[sizeof(str) - 1] = 0;
    char * pch;
    pch = strtok (str,":");
    //printf ("%s\n",pch);
    //ROS_INFO("%s",pch);
    std::string object1 = pch;

    tmp = contacts.contact(i).collision2();
    char str1[32];
    strncpy(str1, tmp.c_str(), sizeof(str));
    str[sizeof(str) - 1] = 0;
    char * pch1;
    pch1 = strtok (str1,":");
    std::string object2 = pch1;

    //ROS_INFO("%s",pch);

    //Collision detection
    if(object1 == "robot0" || object2 == "robot0")
    {
      g0 = '1';
    }
    if(object1 == "robot1" || object2 == "robot1")
    {
      g1 = '1';
    }
    if(object1 == "robot2" || object2 == "robot2")
    {
      g2 = '1';
    }
    if(object1 == "robot3" || object2 == "robot3")
    {
      g3 = '1';
    }

    if(object1 == "robot4" || object2 == "robot4")
    {
      g4 = '1';
    }

    if(object1 == "robot5" || object2 == "robot5")
    {
      g5 = '1';
    }
    if(object1 == "robot6" || object2 == "robot6")
    {
      g6 = '1';
    }
    if(object1 == "robot7" || object2 == "robot7")
    {
      g7 = '1';
    }
    if(object1 == "robot8" || object2 == "robot8")
    {
      g8 = '1';
    }
    if(object1 == "robot9" || object2 == "robot9")
    {
      g9 = '1';
    }
    if(object1 == "robot10" || object2 == "robot10")
    {
      g10 = '1';
    }
    if(object1 == "robot11" || object2 == "robot11")
    {
      g11 = '1';
    }
    if(object1 == "robot12" || object2 == "robot12")
    {
      g12 = '1';
    }
    if(object1 == "robot13" || object2 == "robot13")
    {
      g13 = '1';
    }

    /*for (unsigned int j = 0; j < contacts.contact(i).position_size(); ++j)
    {
      std::cout << j << "  Position:"
                << contacts.contact(i).position(j).x() << " "
                << contacts.contact(i).position(j).y() << " "
                << contacts.contact(i).position(j).z() << "\n";
      std::cout << "   Normal:"
                << contacts.contact(i).normal(j).x() << " "
                << contacts.contact(i).normal(j).y() << " "
                << contacts.contact(i).normal(j).z() << "\n";
      std::cout << "   Depth:" << contacts.contact(i).depth(j) << "\n";
    }*/
  }

    std_msgs::String msg;
    std::stringstream ss;
    ss << g0 << word_sep << g1 << word_sep << g2 << word_sep << g3 << word_sep << g4 << word_sep << g5 << word_sep << g6 << word_sep << g7 << word_sep << g8 << word_sep << g9 << word_sep << g10 << word_sep << g11 << word_sep << g12 << word_sep << g13;
    msg.data = ss.str();

    //ROS_INFO("%s",msg.data.str());

    chatter_pub.publish(msg);
    ros::spinOnce();
}
<commit_msg>Manage /contact max_update_rate<commit_after>#include "ContactPlugin.hh"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <gazebo/sensors/sensors.hh>
#include <string.h>
#include <stdio.h>

#include <sstream>

char word_sep = ' ';

using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(ContactPlugin)

/////////////////////////////////////////////////
ContactPlugin::ContactPlugin() : SensorPlugin()
{
}

/////////////////////////////////////////////////
ContactPlugin::~ContactPlugin()
{
}

ros::Publisher chatter_pub;
int argc;
char **argv;
float max_update_rate = 2.0;
common::Time updateRate = common::Time(0, common::Time::SecToNano((1.0/max_update_rate)));
common::Time prevUpdateTime = common::Time::GetWallTime();

/////////////////////////////////////////////////
void ContactPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr /*_sdf*/)
{
  // Get the parent sensor.
  this->parentSensor =
    boost::dynamic_pointer_cast<sensors::ContactSensor>(_sensor);

  // Make sure the parent sensor is valid.
  if (!this->parentSensor)
  {
    gzerr << "ContactPlugin requires a ContactSensor.\n";
    return;
  }

  // Connect to the sensor update event.
  this->updateConnection = this->parentSensor->ConnectUpdated(
      boost::bind(&ContactPlugin::OnUpdate, this));

  // Make sure the parent sensor is active.
  this->parentSensor->SetActive(true);

  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  chatter_pub = n.advertise<std_msgs::String>("contact", 1000);

}

/////////////////////////////////////////////////
void ContactPlugin::OnUpdate()
{
  if (common::Time::GetWallTime() - prevUpdateTime < updateRate)
    return;
  // Get all the contacts.
  msgs::Contacts contacts;

    char g0,g1,g2,g3,g4,g5,g6,g7,g8,g9,g10,g11,g12,g13;
    g0 = g1 = g2 = g3 = g4 = g5 = g6 = g7 = g8 = g9 = g10 = g11 = g12 = g13 = '0';

  contacts = this->parentSensor->GetContacts();
  for (unsigned int i = 0; i < contacts.contact_size(); ++i)
  {
    //std_msgs::String msg1;

    std::stringstream ss2;
    ss2 << "Collision between[" << contacts.contact(i).collision1()
              << "] and [" << contacts.contact(i).collision2() << "]\n";

    //std::display = ss2.str();
    //ROS_INFO("%s",display);

    std::string tmp = contacts.contact(i).collision1();
    char str[32];
    strncpy(str, tmp.c_str(), sizeof(str));
    str[sizeof(str) - 1] = 0;
    char * pch;
    pch = strtok (str,":");
    //printf ("%s\n",pch);
    //ROS_INFO("%s",pch);
    std::string object1 = pch;

    tmp = contacts.contact(i).collision2();
    char str1[32];
    strncpy(str1, tmp.c_str(), sizeof(str));
    str[sizeof(str) - 1] = 0;
    char * pch1;
    pch1 = strtok (str1,":");
    std::string object2 = pch1;

    //ROS_INFO("%s",pch);

    //Collision detection
    if(object1 == "robot0" || object2 == "robot0")
    {
      g0 = '1';
    }
    if(object1 == "robot1" || object2 == "robot1")
    {
      g1 = '1';
    }
    if(object1 == "robot2" || object2 == "robot2")
    {
      g2 = '1';
    }
    if(object1 == "robot3" || object2 == "robot3")
    {
      g3 = '1';
    }

    if(object1 == "robot4" || object2 == "robot4")
    {
      g4 = '1';
    }

    if(object1 == "robot5" || object2 == "robot5")
    {
      g5 = '1';
    }
    if(object1 == "robot6" || object2 == "robot6")
    {
      g6 = '1';
    }
    if(object1 == "robot7" || object2 == "robot7")
    {
      g7 = '1';
    }
    if(object1 == "robot8" || object2 == "robot8")
    {
      g8 = '1';
    }
    if(object1 == "robot9" || object2 == "robot9")
    {
      g9 = '1';
    }
    if(object1 == "robot10" || object2 == "robot10")
    {
      g10 = '1';
    }
    if(object1 == "robot11" || object2 == "robot11")
    {
      g11 = '1';
    }
    if(object1 == "robot12" || object2 == "robot12")
    {
      g12 = '1';
    }
    if(object1 == "robot13" || object2 == "robot13")
    {
      g13 = '1';
    }

    /*for (unsigned int j = 0; j < contacts.contact(i).position_size(); ++j)
    {
      std::cout << j << "  Position:"
                << contacts.contact(i).position(j).x() << " "
                << contacts.contact(i).position(j).y() << " "
                << contacts.contact(i).position(j).z() << "\n";
      std::cout << "   Normal:"
                << contacts.contact(i).normal(j).x() << " "
                << contacts.contact(i).normal(j).y() << " "
                << contacts.contact(i).normal(j).z() << "\n";
      std::cout << "   Depth:" << contacts.contact(i).depth(j) << "\n";
    }*/
  }

    std_msgs::String msg;
    std::stringstream ss;
    ss << g0 << word_sep << g1 << word_sep << g2 << word_sep << g3 << word_sep << g4 << word_sep << g5 << word_sep << g6 << word_sep << g7 << word_sep << g8 << word_sep << g9 << word_sep << g10 << word_sep << g11 << word_sep << g12 << word_sep << g13;
    msg.data = ss.str();

    //ROS_INFO("%s",msg.data.str());

    prevUpdateTime = common::Time::GetWallTime();
    chatter_pub.publish(msg);
    ros::spinOnce();
}
<|endoftext|>


<commit_before>/*
Copyright (C) 2017,2018 Rodrigo Jose Hernandez Cordoba

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#include "aeongames/ModelInstance.h"
#include "aeongames/Frustum.h"
#include "aeongames/AABB.h"
#include "aeongames/Scene.h"
#include "aeongames/Node.h"
#include "OpenGLWindow.h"
#include "OpenGLRenderer.h"
#include "OpenGLModel.h"
#include "OpenGLFunctions.h"
#include <sstream>
#include <iostream>
#include <algorithm>
#include <array>

namespace AeonGames
{
    OpenGLWindow::OpenGLWindow ( void* aWindowId, const std::shared_ptr<const OpenGLRenderer> aOpenGLRenderer ) :
        mOpenGLRenderer ( aOpenGLRenderer ), mWindowId ( aWindowId )
    {
        try
        {
            Initialize();
        }
        catch ( ... )
        {
            Finalize();
            throw;
        }
    }

    OpenGLWindow::~OpenGLWindow()
    {
        Finalize();
    }

    void* OpenGLWindow::GetWindowId() const
    {
        return mWindowId;
    }

    void OpenGLWindow::ResizeViewport ( uint32_t aWidth, uint32_t aHeight )
    {
        if ( aWidth && aHeight )
        {
#ifdef WIN32
            HDC hdc = GetDC ( reinterpret_cast<HWND> ( mWindowId ) );
            wglMakeCurrent ( hdc, reinterpret_cast<HGLRC> ( mOpenGLRenderer->GetOpenGLContext() ) );
            OPENGL_CHECK_ERROR_NO_THROW;
            ReleaseDC ( reinterpret_cast<HWND> ( mWindowId ), hdc );
#else
            glXMakeCurrent ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                             reinterpret_cast<::Window> ( mWindowId ),
                             static_cast<GLXContext> ( mOpenGLRenderer->GetOpenGLContext() ) );
            OPENGL_CHECK_ERROR_NO_THROW;
#endif
            glViewport ( 0, 0, aWidth, aHeight );
            OPENGL_CHECK_ERROR_NO_THROW;
        }
    }

    void OpenGLWindow::Render ( const std::shared_ptr<Scene>& aScene ) const
    {
#ifdef _WIN32
        HDC hdc = GetDC ( reinterpret_cast<HWND> ( mWindowId ) );
        wglMakeCurrent ( hdc, static_cast<HGLRC> ( mOpenGLRenderer->GetOpenGLContext() ) );
        glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        ReleaseDC ( reinterpret_cast<HWND> ( mWindowId ), hdc );
#else
        glXMakeCurrent ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                         reinterpret_cast<::Window> ( mWindowId ),
                         static_cast<GLXContext> ( mOpenGLRenderer->GetOpenGLContext() ) );
        glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
#endif

        Matrix4x4 view_matrix{ mViewTransform.GetInvertedMatrix() };
        Matrix4x4 projection_matrix =
            mProjectionMatrix * Matrix4x4
        {
            // Flip Matrix
            1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, -1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        };

        glBindBuffer ( GL_UNIFORM_BUFFER, mMatricesBuffer );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBufferSubData ( GL_UNIFORM_BUFFER, sizeof ( float ) * 16, sizeof ( float ) * 16, view_matrix.GetMatrix4x4() );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBufferSubData ( GL_UNIFORM_BUFFER, 0, sizeof ( float ) * 16, ( projection_matrix ).GetMatrix4x4() );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBindBufferBase ( GL_UNIFORM_BUFFER, 0, mMatricesBuffer );

        Frustum frustum ( projection_matrix * view_matrix );
        aScene->LoopTraverseDFSPreOrder ( [this, &frustum, &projection_matrix, &view_matrix] ( Node & aNode )
        {
            const ModelInstance* model_instance = reinterpret_cast<const ModelInstance*> ( aNode.GetComponent ( ModelInstance::TypeId ) );
            const OpenGLModel* opengl_model = reinterpret_cast<const OpenGLModel*> ( aNode.GetComponent ( OpenGLModel::TypeId ) );
            if ( opengl_model )
            {
                if ( frustum.Intersects ( aNode.GetGlobalAABB() ) )
                {
                    // We dont really need to pass the matrices here, but we already have them so why not.
                    opengl_model->Render ( model_instance, projection_matrix, view_matrix );
                }
            }
            else
            {
                /* This is lazy loading */
                aNode.AttachComponent ( OpenGLModel::TypeId, {ModelInstance::TypeId}, std::make_shared<OpenGLModel> ( model_instance->GetModel(), mOpenGLRenderer ) );
            }
        } );

#if _WIN32
        SwapBuffers ( hdc );
        ReleaseDC ( reinterpret_cast<HWND> ( mWindowId ), hdc );
#else
        glXSwapBuffers ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                         reinterpret_cast<::Window> ( mWindowId ) );
#endif
    }

    const GLuint OpenGLWindow::GetMatricesBuffer() const
    {
        return mMatricesBuffer;
    }

    void OpenGLWindow::Initialize()
    {
        if ( !mOpenGLRenderer )
        {
            throw std::runtime_error ( "Pointer to OpenGL Renderer is nullptr." );
        }
#if _WIN32
        HDC hdc = GetDC ( static_cast<HWND> ( mWindowId ) );
        PIXELFORMATDESCRIPTOR pfd{};
        pfd.nSize = sizeof ( PIXELFORMATDESCRIPTOR );
        pfd.nVersion = 1;
        pfd.dwFlags = PFD_DRAW_TO_WINDOW | PFD_SUPPORT_OPENGL | PFD_DOUBLEBUFFER;
        pfd.iPixelType = PFD_TYPE_RGBA;
        pfd.cColorBits = 32;
        pfd.cDepthBits = 32;
        pfd.iLayerType = PFD_MAIN_PLANE;
        int pf = ChoosePixelFormat ( hdc, &pfd );
        SetPixelFormat ( hdc, pf, &pfd );
        wglMakeCurrent ( hdc, static_cast<HGLRC> ( mOpenGLRenderer->GetOpenGLContext() ) );
        ReleaseDC ( static_cast<HWND> ( mWindowId ), hdc );
        RECT rect;
        GetClientRect ( static_cast<HWND> ( mWindowId ), &rect );
        glViewport ( 0, 0, rect.right, rect.bottom );
        OPENGL_CHECK_ERROR_THROW;
#else
        XWindowComponents x_window_attributes {};
        XGetWindowComponents ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                               reinterpret_cast<::Window> ( mWindowId ), &x_window_attributes );
        std::cout << "Visual " << x_window_attributes.visual <<
                  " Default " << DefaultVisual ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                          DefaultScreen ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ) ) ) << std::endl;
        XSetErrorHandler ( [] ( Display * display, XErrorEvent * error_event ) -> int
        {
            std::cout << "Error Code " << static_cast<int> ( error_event->error_code ) << std::endl;
            return 0;
        } );
        if ( !glXMakeCurrent (  static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                                reinterpret_cast<::Window> ( mWindowId ),
                                static_cast<GLXContext> ( mOpenGLRenderer->GetOpenGLContext() ) ) )
        {
            std::ostringstream stream;
            stream << "Failed to make OpenGL current to XWindow. Error: ";
            throw std::runtime_error ( stream.str().c_str() );
        }
        XSetErrorHandler ( nullptr );
#endif
        glClearColor ( 0.5f, 0.5f, 0.5f, 1.0f );
        OPENGL_CHECK_ERROR_NO_THROW;
        glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBlendFunc ( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
        OPENGL_CHECK_ERROR_NO_THROW;
        glEnable ( GL_BLEND );
        OPENGL_CHECK_ERROR_NO_THROW;
        glDepthFunc ( GL_LESS );
        OPENGL_CHECK_ERROR_NO_THROW;
        glEnable ( GL_DEPTH_TEST );
        OPENGL_CHECK_ERROR_NO_THROW;
        glCullFace ( GL_BACK );
        OPENGL_CHECK_ERROR_NO_THROW;
        glEnable ( GL_CULL_FACE );
        OPENGL_CHECK_ERROR_NO_THROW;
        glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        OPENGL_CHECK_ERROR_NO_THROW;

        // Initialize Matrix Buffer
        glGenBuffers ( 1, &mMatricesBuffer );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBindBuffer ( GL_UNIFORM_BUFFER, mMatricesBuffer );
        OPENGL_CHECK_ERROR_NO_THROW;
        float matrices[32] =
        {
            1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f,
            1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        };
        glBufferData ( GL_UNIFORM_BUFFER, sizeof ( float ) * 32,
                       matrices, GL_DYNAMIC_DRAW );
        OPENGL_CHECK_ERROR_NO_THROW;
    }

    void OpenGLWindow::Finalize()
    {
        if ( glIsBuffer ( mMatricesBuffer ) )
        {
            OPENGL_CHECK_ERROR_NO_THROW;
            glDeleteBuffers ( 1, &mMatricesBuffer );
            OPENGL_CHECK_ERROR_NO_THROW;
            mMatricesBuffer = 0;
        }
        OPENGL_CHECK_ERROR_NO_THROW;
    }
}
<commit_msg>Linux build fix.<commit_after>/*
Copyright (C) 2017,2018 Rodrigo Jose Hernandez Cordoba

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#include "aeongames/ModelInstance.h"
#include "aeongames/Frustum.h"
#include "aeongames/AABB.h"
#include "aeongames/Scene.h"
#include "aeongames/Node.h"
#include "OpenGLWindow.h"
#include "OpenGLRenderer.h"
#include "OpenGLModel.h"
#include "OpenGLFunctions.h"
#include <sstream>
#include <iostream>
#include <algorithm>
#include <array>

namespace AeonGames
{
    OpenGLWindow::OpenGLWindow ( void* aWindowId, const std::shared_ptr<const OpenGLRenderer> aOpenGLRenderer ) :
        mOpenGLRenderer ( aOpenGLRenderer ), mWindowId ( aWindowId )
    {
        try
        {
            Initialize();
        }
        catch ( ... )
        {
            Finalize();
            throw;
        }
    }

    OpenGLWindow::~OpenGLWindow()
    {
        Finalize();
    }

    void* OpenGLWindow::GetWindowId() const
    {
        return mWindowId;
    }

    void OpenGLWindow::ResizeViewport ( uint32_t aWidth, uint32_t aHeight )
    {
        if ( aWidth && aHeight )
        {
#ifdef WIN32
            HDC hdc = GetDC ( reinterpret_cast<HWND> ( mWindowId ) );
            wglMakeCurrent ( hdc, reinterpret_cast<HGLRC> ( mOpenGLRenderer->GetOpenGLContext() ) );
            OPENGL_CHECK_ERROR_NO_THROW;
            ReleaseDC ( reinterpret_cast<HWND> ( mWindowId ), hdc );
#else
            glXMakeCurrent ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                             reinterpret_cast<::Window> ( mWindowId ),
                             static_cast<GLXContext> ( mOpenGLRenderer->GetOpenGLContext() ) );
            OPENGL_CHECK_ERROR_NO_THROW;
#endif
            glViewport ( 0, 0, aWidth, aHeight );
            OPENGL_CHECK_ERROR_NO_THROW;
        }
    }

    void OpenGLWindow::Render ( const std::shared_ptr<Scene>& aScene ) const
    {
#ifdef _WIN32
        HDC hdc = GetDC ( reinterpret_cast<HWND> ( mWindowId ) );
        wglMakeCurrent ( hdc, static_cast<HGLRC> ( mOpenGLRenderer->GetOpenGLContext() ) );
        glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        ReleaseDC ( reinterpret_cast<HWND> ( mWindowId ), hdc );
#else
        glXMakeCurrent ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                         reinterpret_cast<::Window> ( mWindowId ),
                         static_cast<GLXContext> ( mOpenGLRenderer->GetOpenGLContext() ) );
        glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
#endif

        Matrix4x4 view_matrix{ mViewTransform.GetInvertedMatrix() };
        Matrix4x4 projection_matrix =
            mProjectionMatrix * Matrix4x4
        {
            // Flip Matrix
            1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, -1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        };

        glBindBuffer ( GL_UNIFORM_BUFFER, mMatricesBuffer );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBufferSubData ( GL_UNIFORM_BUFFER, sizeof ( float ) * 16, sizeof ( float ) * 16, view_matrix.GetMatrix4x4() );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBufferSubData ( GL_UNIFORM_BUFFER, 0, sizeof ( float ) * 16, ( projection_matrix ).GetMatrix4x4() );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBindBufferBase ( GL_UNIFORM_BUFFER, 0, mMatricesBuffer );

        Frustum frustum ( projection_matrix * view_matrix );
        aScene->LoopTraverseDFSPreOrder ( [this, &frustum, &projection_matrix, &view_matrix] ( Node & aNode )
        {
            const ModelInstance* model_instance = reinterpret_cast<const ModelInstance*> ( aNode.GetComponent ( ModelInstance::TypeId ) );
            const OpenGLModel* opengl_model = reinterpret_cast<const OpenGLModel*> ( aNode.GetComponent ( OpenGLModel::TypeId ) );
            if ( opengl_model )
            {
                if ( frustum.Intersects ( aNode.GetGlobalAABB() ) )
                {
                    // We dont really need to pass the matrices here, but we already have them so why not.
                    opengl_model->Render ( model_instance, projection_matrix, view_matrix );
                }
            }
            else
            {
                /* This is lazy loading */
                aNode.AttachComponent ( OpenGLModel::TypeId, {ModelInstance::TypeId}, std::make_shared<OpenGLModel> ( model_instance->GetModel(), mOpenGLRenderer ) );
            }
        } );

#if _WIN32
        SwapBuffers ( hdc );
        ReleaseDC ( reinterpret_cast<HWND> ( mWindowId ), hdc );
#else
        glXSwapBuffers ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                         reinterpret_cast<::Window> ( mWindowId ) );
#endif
    }

    const GLuint OpenGLWindow::GetMatricesBuffer() const
    {
        return mMatricesBuffer;
    }

    void OpenGLWindow::Initialize()
    {
        if ( !mOpenGLRenderer )
        {
            throw std::runtime_error ( "Pointer to OpenGL Renderer is nullptr." );
        }
#if _WIN32
        HDC hdc = GetDC ( static_cast<HWND> ( mWindowId ) );
        PIXELFORMATDESCRIPTOR pfd{};
        pfd.nSize = sizeof ( PIXELFORMATDESCRIPTOR );
        pfd.nVersion = 1;
        pfd.dwFlags = PFD_DRAW_TO_WINDOW | PFD_SUPPORT_OPENGL | PFD_DOUBLEBUFFER;
        pfd.iPixelType = PFD_TYPE_RGBA;
        pfd.cColorBits = 32;
        pfd.cDepthBits = 32;
        pfd.iLayerType = PFD_MAIN_PLANE;
        int pf = ChoosePixelFormat ( hdc, &pfd );
        SetPixelFormat ( hdc, pf, &pfd );
        wglMakeCurrent ( hdc, static_cast<HGLRC> ( mOpenGLRenderer->GetOpenGLContext() ) );
        ReleaseDC ( static_cast<HWND> ( mWindowId ), hdc );
        RECT rect;
        GetClientRect ( static_cast<HWND> ( mWindowId ), &rect );
        glViewport ( 0, 0, rect.right, rect.bottom );
        OPENGL_CHECK_ERROR_THROW;
#else
        XWindowAttributes x_window_attributes {};
        XGetWindowAttributes ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                               reinterpret_cast<::Window> ( mWindowId ), &x_window_attributes );
        std::cout << "Visual " << x_window_attributes.visual <<
                  " Default " << DefaultVisual ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                          DefaultScreen ( static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ) ) ) << std::endl;
        XSetErrorHandler ( [] ( Display * display, XErrorEvent * error_event ) -> int
        {
            std::cout << "Error Code " << static_cast<int> ( error_event->error_code ) << std::endl;
            return 0;
        } );
        if ( !glXMakeCurrent (  static_cast<Display*> ( mOpenGLRenderer->GetWindowId() ),
                                reinterpret_cast<::Window> ( mWindowId ),
                                static_cast<GLXContext> ( mOpenGLRenderer->GetOpenGLContext() ) ) )
        {
            std::ostringstream stream;
            stream << "Failed to make OpenGL current to XWindow. Error: ";
            throw std::runtime_error ( stream.str().c_str() );
        }
        XSetErrorHandler ( nullptr );
#endif
        glClearColor ( 0.5f, 0.5f, 0.5f, 1.0f );
        OPENGL_CHECK_ERROR_NO_THROW;
        glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBlendFunc ( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
        OPENGL_CHECK_ERROR_NO_THROW;
        glEnable ( GL_BLEND );
        OPENGL_CHECK_ERROR_NO_THROW;
        glDepthFunc ( GL_LESS );
        OPENGL_CHECK_ERROR_NO_THROW;
        glEnable ( GL_DEPTH_TEST );
        OPENGL_CHECK_ERROR_NO_THROW;
        glCullFace ( GL_BACK );
        OPENGL_CHECK_ERROR_NO_THROW;
        glEnable ( GL_CULL_FACE );
        OPENGL_CHECK_ERROR_NO_THROW;
        glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        OPENGL_CHECK_ERROR_NO_THROW;

        // Initialize Matrix Buffer
        glGenBuffers ( 1, &mMatricesBuffer );
        OPENGL_CHECK_ERROR_NO_THROW;
        glBindBuffer ( GL_UNIFORM_BUFFER, mMatricesBuffer );
        OPENGL_CHECK_ERROR_NO_THROW;
        float matrices[32] =
        {
            1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f,
            1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        };
        glBufferData ( GL_UNIFORM_BUFFER, sizeof ( float ) * 32,
                       matrices, GL_DYNAMIC_DRAW );
        OPENGL_CHECK_ERROR_NO_THROW;
    }

    void OpenGLWindow::Finalize()
    {
        if ( glIsBuffer ( mMatricesBuffer ) )
        {
            OPENGL_CHECK_ERROR_NO_THROW;
            glDeleteBuffers ( 1, &mMatricesBuffer );
            OPENGL_CHECK_ERROR_NO_THROW;
            mMatricesBuffer = 0;
        }
        OPENGL_CHECK_ERROR_NO_THROW;
    }
}
<|endoftext|>


<commit_before>#include "solver/group/single_group_solver.h"

#include <memory>

#include "system/system.h"
#include "system/solution/tests/mpi_group_angular_solution_mock.h"
#include "system/terms/tests/linear_term_mock.h"
#include "system/terms/tests/bilinear_term_mock.h"
#include "solver/tests/linear_mock.h"
#include "test_helpers/gmock_wrapper.h"

namespace {

using namespace bart;

using ::testing::DoDefault, ::testing::NiceMock, ::testing::Return;
using ::testing::ReturnRef, ::testing::_;

class SolverGroupSingleGroupSolverTest : public ::testing::Test {
 protected:

  using LinearSolver = solver::LinearMock;
  using LeftHandSide = system::terms::BilinearTermMock;
  using RightHandSide = system::terms::LinearTermMock;
  using GroupSolution = NiceMock<system::solution::MPIGroupAngularSolutionMock>;

  // SUpporting objects
  system::System test_system_;
  GroupSolution solution_;

  // Supporting mocks
  std::unique_ptr<LinearSolver> linear_solver_ptr_;

  // Mock Observing pointers
  LinearSolver* linear_solver_obs_ptr_;
  RightHandSide* rhs_obs_ptr_;
  LeftHandSide* lhs_obs_ptr_;

  // test parameters
  const int total_angles_ = 2;
  const int test_group_ = 2;

  void SetUp() override;
};

void SolverGroupSingleGroupSolverTest::SetUp() {
  linear_solver_ptr_ = std::make_unique<LinearSolver>();
  linear_solver_obs_ptr_ = linear_solver_ptr_.get();

  auto rhs_ptr_ = std::make_unique<RightHandSide>();
  auto lhs_ptr_ = std::make_unique<LeftHandSide>();

  rhs_obs_ptr_ = rhs_ptr_.get();
  lhs_obs_ptr_ = lhs_ptr_.get();

  test_system_.right_hand_side_ptr_ = std::move(rhs_ptr_);
  test_system_.left_hand_side_ptr_ = std::move(lhs_ptr_);

  ON_CALL(solution_, total_angles())
      .WillByDefault(Return(total_angles_));
}

TEST_F(SolverGroupSingleGroupSolverTest, Constructor) {
  solver::group::SingleGroupSolver test_solver(std::move(linear_solver_ptr_));

  auto test_ptr = dynamic_cast<LinearSolver*>(test_solver.linear_solver_ptr());

  EXPECT_NE(test_ptr, nullptr);
}

TEST_F(SolverGroupSingleGroupSolverTest, SolveGroupOperation) {
  solver::group::SingleGroupSolver test_solver(std::move(linear_solver_ptr_));

  /* Test objects. These are objects that will be returned by our mock functions
   * and used to verify correct mediation of all the classes. In most cases they
   * do not need to include any actual information, we will just verify they are
   * correctly passed by their reference.
   */
  std::vector<system::MPIVector> solution_vectors_(total_angles_);
  std::vector<std::shared_ptr<system::MPISparseMatrix>> lhs_matrices_(total_angles_);
  std::vector<std::shared_ptr<system::MPIVector>> rhs_vectors_(total_angles_);

  // Expect to retrieve total angles (to access all angles for the test group)
  EXPECT_CALL(solution_, total_angles())
      .WillOnce(Return(total_angles_));

  // Expectations for each angle:
  for (int angle = 0; angle < total_angles_; ++angle) {
    system::Index index{test_group_, angle};
    // Expect to retrieve each solution
    EXPECT_CALL(solution_, BracketOp(angle))
        .WillOnce(ReturnRef(solution_vectors_[angle]));
    // Expect to retrieve each LHS
    EXPECT_CALL(*lhs_obs_ptr_, GetFullTermPtr(index))
        .WillOnce(Return(lhs_matrices_[angle]));
    // Expect to retrieve each RHS
    EXPECT_CALL(*rhs_obs_ptr_, GetFullTermPtr(index))
        .WillOnce(Return(rhs_vectors_[angle]));

    EXPECT_CALL(*linear_solver_obs_ptr_, Solve(
        lhs_matrices_[angle].get(),
        &solution_vectors_[angle],
        rhs_vectors_[angle].get(),
        _));
  }

  test_solver.SolveGroup(test_group_, test_system_, solution_);
}

TEST_F(SolverGroupSingleGroupSolverTest, SolveGroupBadAngles) {
  solver::group::SingleGroupSolver test_solver(std::move(linear_solver_ptr_));

  std::array<int, 2> bad_angles = {-1, 0};
  for (const int angle : bad_angles) {
    EXPECT_CALL(solution_, total_angles())
        .WillOnce(Return(angle));
    EXPECT_ANY_THROW(test_solver.SolveGroup(0, test_system_, solution_));
  }
}

} // namespace
<commit_msg>fixed some expected calls for SolveGroup<commit_after>#include "solver/group/single_group_solver.h"

#include <memory>

#include "system/system.h"
#include "system/solution/tests/mpi_group_angular_solution_mock.h"
#include "system/terms/tests/linear_term_mock.h"
#include "system/terms/tests/bilinear_term_mock.h"
#include "solver/tests/linear_mock.h"
#include "test_helpers/dealii_test_domain.h"
#include "test_helpers/gmock_wrapper.h"

namespace {

using namespace bart;

using ::testing::DoDefault, ::testing::NiceMock, ::testing::Return;
using ::testing::ReturnRef, ::testing::_;
using ::testing::Pointee, ::testing::Ref;

class SolverGroupSingleGroupSolverTest :
    public ::testing::Test,
    public bart::testing::DealiiTestDomain<2> {
 protected:

  using LinearSolver = solver::LinearMock;
  using LeftHandSide = system::terms::BilinearTermMock;
  using RightHandSide = system::terms::LinearTermMock;
  using GroupSolution = NiceMock<system::solution::MPIGroupAngularSolutionMock>;

  // SUpporting objects
  system::System test_system_;
  GroupSolution solution_;

  // Supporting mocks
  std::unique_ptr<LinearSolver> linear_solver_ptr_;

  // Mock Observing pointers
  LinearSolver* linear_solver_obs_ptr_;
  RightHandSide* rhs_obs_ptr_;
  LeftHandSide* lhs_obs_ptr_;

  // test parameters
  const int total_angles_ = 2;
  const int test_group_ = 2;

  void SetUp() override;
};

void SolverGroupSingleGroupSolverTest::SetUp() {
  SetUpDealii();
  linear_solver_ptr_ = std::make_unique<LinearSolver>();
  linear_solver_obs_ptr_ = linear_solver_ptr_.get();

  auto rhs_ptr_ = std::make_unique<RightHandSide>();
  auto lhs_ptr_ = std::make_unique<LeftHandSide>();

  rhs_obs_ptr_ = rhs_ptr_.get();
  lhs_obs_ptr_ = lhs_ptr_.get();

  test_system_.right_hand_side_ptr_ = std::move(rhs_ptr_);
  test_system_.left_hand_side_ptr_ = std::move(lhs_ptr_);

  ON_CALL(solution_, total_angles())
      .WillByDefault(Return(total_angles_));
}

TEST_F(SolverGroupSingleGroupSolverTest, Constructor) {
  solver::group::SingleGroupSolver test_solver(std::move(linear_solver_ptr_));

  auto test_ptr = dynamic_cast<LinearSolver*>(test_solver.linear_solver_ptr());

  EXPECT_NE(test_ptr, nullptr);
}

TEST_F(SolverGroupSingleGroupSolverTest, SolveGroupOperation) {
  solver::group::SingleGroupSolver test_solver(std::move(linear_solver_ptr_));

  /* Test objects. These are objects that will be returned by our mock functions
   * and used to verify correct mediation of all the classes. In most cases they
   * do not need to include any actual information, we will just verify they are
   * correctly passed by their reference.
   */
  std::vector<system::MPIVector> solution_vectors_(total_angles_);
  std::vector<std::shared_ptr<system::MPISparseMatrix>> lhs_matrices_(total_angles_);
  std::vector<std::shared_ptr<system::MPIVector>> rhs_vectors_(total_angles_);

  // Expect to retrieve total angles (to access all angles for the test group)
  EXPECT_CALL(solution_, total_angles())
      .WillOnce(Return(total_angles_));

  // Expectations for each angle:
  for (int angle = 0; angle < total_angles_; ++angle) {
    system::Index index{test_group_, angle};

    rhs_vectors_[angle] = std::make_shared<system::MPIVector>();
    lhs_matrices_[angle] = std::make_shared<system::MPISparseMatrix>();
    lhs_matrices_[angle]->reinit(matrix_1);
    lhs_matrices_[angle]->copy_from(matrix_1);

    // Expect to retrieve each solution
    EXPECT_CALL(solution_, BracketOp(angle))
        .WillOnce(ReturnRef(solution_vectors_[angle]));
    // Expect to retrieve each LHS
    EXPECT_CALL(*lhs_obs_ptr_, GetFullTermPtr(index))
        .WillOnce(Return(lhs_matrices_[angle]));
    // Expect to retrieve each RHS
    EXPECT_CALL(*rhs_obs_ptr_, GetFullTermPtr(index))
        .WillOnce(Return(rhs_vectors_[angle]));

    EXPECT_CALL(*linear_solver_obs_ptr_, Solve(
        lhs_matrices_[angle].get(),
        Pointee(solution_vectors_[angle]),
        rhs_vectors_[angle].get(),
        _));
  }

  test_solver.SolveGroup(test_group_, test_system_, solution_);
}

TEST_F(SolverGroupSingleGroupSolverTest, SolveGroupBadAngles) {
  solver::group::SingleGroupSolver test_solver(std::move(linear_solver_ptr_));

  std::array<int, 2> bad_angles = {-1, 0};
  for (const int angle : bad_angles) {
    EXPECT_CALL(solution_, total_angles())
        .WillOnce(Return(angle));
    EXPECT_ANY_THROW(test_solver.SolveGroup(0, test_system_, solution_));
  }
}

} // namespace
<|endoftext|>


<commit_before>#include <cstdio>
#include <boost/cstdint.hpp>
#include <sys/time.h>
#include <arpa/inet.h>

#include "ExternalOutput.h"
#include "../WebRtcConnection.h"
#include "../rtp/RtpHeaders.h"

namespace erizo {
#define FIR_INTERVAL_MS 4000

  DEFINE_LOGGER(ExternalOutput, "media.ExternalOutput");

  ExternalOutput::ExternalOutput(const std::string& outputUrl) :
    url(outputUrl)
  {
    ELOG_DEBUG("Created ExternalOutput to %s", outputUrl.c_str());
    videoCodec_ = NULL;
    audioCodec_ = NULL;
    video_st = NULL;
    audio_st = NULL;
    audioCoder_ = NULL;
    hasVideo_ = true;
    prevEstimatedFps_ = 0;
    warmupfpsCount_ = 0;
    sequenceNumberFIR_ = 0;
    aviores_ = -1;
    writeheadres_=-1;
    unpackagedBufferpart_ = unpackagedBuffer_;
    initTime_ = 0;
    lastTime_ = 0;
    sinkfbSource_ = this;
    fbSink_ = NULL;
  }

  bool ExternalOutput::init(){
    av_register_all();
    avcodec_register_all();
    context_ = avformat_alloc_context();
    if (context_==NULL){
      ELOG_ERROR("Error allocating memory for IO context");
      return false;
    }
    oformat_ = av_guess_format(NULL,  url.c_str(), NULL);
    if (!oformat_){
      ELOG_ERROR("Error opening output file %s", url.c_str());
      return false;
    }
    url.copy(context_->filename, sizeof(context_->filename),0);
    video_st = NULL;
    audio_st = NULL;
    in_ = new InputProcessor();
    MediaInfo m;
    //    m.processorType = RTP_ONLY;
    m.hasVideo = false;
    m.hasAudio = false;

    gotUnpackagedFrame_ = 0;
    unpackagedSize_ = 0;
    in_->init(m, this);
    thread_ = boost::thread(&ExternalOutput::sendLoop, this);
    sending_ = true;
    ELOG_DEBUG("Initialized successfully");
    return true;
  }

  ExternalOutput::~ExternalOutput(){
    ELOG_DEBUG("Destructor");
    ELOG_DEBUG("Closing Sink");
    delete in_;
    in_ = NULL;

    if (context_!=NULL){
      if (writeheadres_>=0)
        av_write_trailer(context_);
      if (avio_close>=0)
        avio_close(context_->pb);
      avformat_free_context(context_);
      context_=NULL;
    }
    if (videoCodec_!=NULL){
      avcodec_close(videoCodecCtx_);
      videoCodec_=NULL;
    }
    if (audioCodec_!=NULL){
      avcodec_close(audioCodecCtx_);
      audioCodec_ = NULL;
    }

    sending_ = false;
    cond_.notify_one();
    thread_.join();
    /* boost::unique_lock<boost::mutex> lock(queueMutex_); */
    ELOG_DEBUG("ExternalOutput closed Successfully");
  }

  void ExternalOutput::receiveRawData(RawDataPacket& packet){
    return;
  }

  int ExternalOutput::writeAudioData(char* buf, int len){
    if (in_!=NULL){
      if (audioCodec_ == NULL) {
        if (warmupfpsCount_>=100){
          hasVideo_ = false;
          if (!this->initContext()){
            ELOG_ERROR("Context cannot be initialized properly, closing...");
            return -1;
          }
        }
        return 0;
      }
      RtpHeader *head = reinterpret_cast<RtpHeader*> (buf);
      //We dont need any other payload at this time
      if(head->payloadtype != PCMU_8000_PT){
        return 0;
      }

      int ret = in_->unpackageAudio(reinterpret_cast<unsigned char*>(buf), len,
          unpackagedAudioBuffer_);
      if (ret <= 0)
        return ret;
      timeval time;
      gettimeofday(&time, NULL);
      unsigned long long millis = (time.tv_sec * 1000) + (time.tv_usec / 1000);
      if (millis -lastTime_ >FIR_INTERVAL_MS && (hasVideo_)){
        this->sendFirPacket();
        lastTime_ = millis;
      }
      if (initTime_ == 0) {
        initTime_ = millis;
      }
      if (millis < initTime_){
        ELOG_WARN("initTime is smaller than currentTime, possible problems when recording ");
      }
      if (ret > UNPACKAGE_BUFFER_SIZE){
        ELOG_ERROR("Unpackaged Audio size too big %d", ret);
      }
      AVPacket avpkt;
      av_init_packet(&avpkt);
      avpkt.data = unpackagedAudioBuffer_;
      avpkt.size = ret;
      avpkt.pts = millis - initTime_;
      avpkt.dts = avpkt.pts;
      avpkt.stream_index = hasVideo_?1:0;
      av_write_frame(context_, &avpkt);
      av_free_packet(&avpkt);
      return ret;

    }
    return 0;
  }

  int ExternalOutput::writeVideoData(char* buf, int len){
    if (in_!=NULL){
      RtpHeader *head = reinterpret_cast<RtpHeader*> (buf);
      if (head->payloadtype == RED_90000_PT) {
        int totalLength = 12;

        if (head->extension) {
          totalLength += ntohs(head->extensionlength)*4 + 4; // RTP Extension header
        }
        int rtpHeaderLength = totalLength;
        RedHeader *redhead = reinterpret_cast<RedHeader*> ((buf + totalLength));

        //redhead->payloadtype = remoteSdp_.inOutPTMap[redhead->payloadtype];
        if (redhead->payloadtype == VP8_90000_PT) {
          while (redhead->follow) {
            totalLength += redhead->getLength() + 4; // RED header
            redhead = reinterpret_cast<RedHeader*> ((buf + totalLength));
          }
          // Parse RED packet to VP8 packet.
          // Copy RTP header
          memcpy(deliverMediaBuffer_, buf, rtpHeaderLength);
          // Copy payload data
          memcpy(deliverMediaBuffer_ + totalLength, buf + totalLength + 1, len - totalLength - 1);
          // Copy payload type
          RtpHeader *mediahead = reinterpret_cast<RtpHeader*> (deliverMediaBuffer_);
          mediahead->payloadtype = redhead->payloadtype;
          buf = reinterpret_cast<char*> (deliverMediaBuffer_);
          len = len - 1 - totalLength + rtpHeaderLength;
        }
      }
      int estimatedFps=0;
      int ret = in_->unpackageVideo(reinterpret_cast<unsigned char*>(buf), len,
          unpackagedBufferpart_, &gotUnpackagedFrame_, &estimatedFps);

      if (ret < 0)
        return 0;

      if (videoCodec_ == NULL) {
        if ((estimatedFps!=0)&&((estimatedFps < prevEstimatedFps_*(1-0.2))||(estimatedFps > prevEstimatedFps_*(1+0.2)))){
          prevEstimatedFps_ = estimatedFps;
        }
        if (warmupfpsCount_ >= 50){
          if (prevEstimatedFps_==0){
            warmupfpsCount_ = 0;
            return 0;
          }
          hasVideo_=true;
          if (!this->initContext()){
            ELOG_ERROR("Context cannot be initialized properly, closing...");
            return -1;
          }
        }
        return 0;
      }

      unpackagedSize_ += ret;
      unpackagedBufferpart_ += ret;
      if (unpackagedSize_ > UNPACKAGE_BUFFER_SIZE){
        ELOG_ERROR("Unpackaged size bigget than buffer %d", unpackagedSize_);
      }
      if (gotUnpackagedFrame_ && videoCodec_!=NULL) {
        timeval time;
        gettimeofday(&time, NULL);
        unsigned long long millis = (time.tv_sec * 1000) + (time.tv_usec / 1000);
        if (initTime_ == 0) {
          initTime_ = millis;
        }
        if (millis < initTime_)
        {
          ELOG_WARN("initTime is smaller than currentTime, possible problems when recording ");
        }
        unpackagedBufferpart_ -= unpackagedSize_;

        AVPacket avpkt;
        av_init_packet(&avpkt);
        avpkt.data = unpackagedBufferpart_;
        avpkt.size = unpackagedSize_;
        avpkt.pts = millis - initTime_;
        avpkt.stream_index = 0;
        av_write_frame(context_, &avpkt);
        av_free_packet(&avpkt);
        gotUnpackagedFrame_ = 0;
        unpackagedSize_ = 0;
        unpackagedBufferpart_ = unpackagedBuffer_;

      }
    }
    return 0;
  }

  int ExternalOutput::deliverAudioData_(char* buf, int len) {
    RtcpHeader *head = reinterpret_cast<RtcpHeader*> (buf);
    warmupfpsCount_++;
    if (head->isRtcp()){
      return 0;
    }
    this->queueData(buf,len,AUDIO_PACKET);
    return 0;
  }

  int ExternalOutput::deliverVideoData_(char* buf, int len) {
    RtcpHeader *head = reinterpret_cast<RtcpHeader*> (buf);
    warmupfpsCount_++;
    if (head->isRtcp()){
      return 0;
    }
    this->queueData(buf,len,VIDEO_PACKET);
    return 0;
  }

  bool ExternalOutput::initContext() {
    ELOG_DEBUG("Init Context");
    context_->oformat = oformat_;
    if (hasVideo_){
      context_->oformat->video_codec = AV_CODEC_ID_VP8;
    }
    context_->oformat->audio_codec = AV_CODEC_ID_PCM_MULAW;
    if (oformat_->video_codec != AV_CODEC_ID_NONE && videoCodec_ == NULL) {
      if (hasVideo_){
        videoCodec_ = avcodec_find_encoder(oformat_->video_codec);
        ELOG_DEBUG("Found Codec %s", videoCodec_->name);
        ELOG_DEBUG("Initing context with fps: %d", (int)prevEstimatedFps_);
        if (videoCodec_==NULL){
          ELOG_ERROR("Could not find codec");
          return false;
        }
        video_st = avformat_new_stream (context_, videoCodec_);
        video_st->id = 0;
        videoCodecCtx_ = video_st->codec;
        videoCodecCtx_->codec_id = oformat_->video_codec;
        videoCodecCtx_->width = 640;
        videoCodecCtx_->height = 480;
        videoCodecCtx_->time_base = (AVRational){1,(int)prevEstimatedFps_};
        videoCodecCtx_->pix_fmt = PIX_FMT_YUV420P;
        if (oformat_->flags & AVFMT_GLOBALHEADER){
          videoCodecCtx_->flags|=CODEC_FLAG_GLOBAL_HEADER;
        }
        oformat_->flags |= AVFMT_VARIABLE_FPS;
      }
      ELOG_DEBUG("Init audio context");

      audioCodec_ = avcodec_find_encoder(oformat_->audio_codec);
      if (audioCodec_==NULL){
        ELOG_ERROR("Could not find audio codec");
        return false;
      }
      ELOG_DEBUG("Found Audio Codec %s", audioCodec_->name);
      audio_st = avformat_new_stream (context_, audioCodec_);
      audio_st->id = 1;
      audioCodecCtx_ = audio_st->codec;
      audioCodecCtx_->codec_id = oformat_->audio_codec;
      audioCodecCtx_->sample_rate = 8000;
      audioCodecCtx_->channels = 1;
      //      audioCodecCtx_->sample_fmt = AV_SAMPLE_FMT_S8;
      if (oformat_->flags & AVFMT_GLOBALHEADER){
        audioCodecCtx_->flags|=CODEC_FLAG_GLOBAL_HEADER;
      }
      if(hasVideo_){
        context_->streams[0] = video_st;
        context_->streams[1] = audio_st;
      } else{
        ELOG_ERROR("No Video");
        context_->streams[0] = audio_st;
      }
      aviores_ = avio_open(&context_->pb, url.c_str(), AVIO_FLAG_WRITE);
      if (aviores_<0){
        ELOG_ERROR("Error opening output file");
        return false;
      }
      writeheadres_ = avformat_write_header(context_, NULL);
      if (writeheadres_<0){
        ELOG_ERROR("Error writing header");
        return false;
      }
      ELOG_DEBUG("AVFORMAT CONFIGURED");
    }
    return true;
  }

  void ExternalOutput::queueData(char* buffer, int length, packetType type){
    if (in_==NULL) {
      return;
    }
    boost::mutex::scoped_lock lock(queueMutex_);
    if (type == VIDEO_PACKET){
      videoQueue_.pushPacket(buffer, length);
    }else{
      audioQueue_.pushPacket(buffer, length);
    }
    cond_.notify_one();

  }

  int ExternalOutput::sendFirPacket() {
    if (fbSink_ != NULL) {
      sequenceNumberFIR_++; // do not increase if repetition
      int pos = 0;
      uint8_t rtcpPacket[50];
      // add full intra request indicator
      uint8_t FMT = 4;
      rtcpPacket[pos++] = (uint8_t) 0x80 + FMT;
      rtcpPacket[pos++] = (uint8_t) 206;
      pos = 12;
      fbSink_->deliverFeedback((char*)rtcpPacket, pos);
      return pos;
    }

    return -1;
  }

  void ExternalOutput::sendLoop() {
    while (sending_ == true) {
      boost::unique_lock<boost::mutex> lock(queueMutex_);
      while ((!audioQueue_.getSize())&&(!videoQueue_.getSize())) {
        cond_.wait(lock);
        if (sending_ == false) {
          lock.unlock();
          return;
        }
      }
      if (audioQueue_.getSize()){
        boost::shared_ptr<dataPacket> audioP = audioQueue_.popPacket();
        this->writeAudioData(audioP->data, audioP->length);
      }
      if (videoQueue_.getSize()) {
        boost::shared_ptr<dataPacket> videoP = videoQueue_.popPacket();
        this->writeVideoData(videoP->data, videoP->length);

      }
      lock.unlock();
    }
  }

}

<commit_msg>Fixed recording video with no audio<commit_after>#include <cstdio>
#include <boost/cstdint.hpp>
#include <sys/time.h>
#include <arpa/inet.h>

#include "ExternalOutput.h"
#include "../WebRtcConnection.h"
#include "../rtp/RtpHeaders.h"

namespace erizo {
#define FIR_INTERVAL_MS 4000

  DEFINE_LOGGER(ExternalOutput, "media.ExternalOutput");

  ExternalOutput::ExternalOutput(const std::string& outputUrl) :
    url(outputUrl)
  {
    ELOG_DEBUG("Created ExternalOutput to %s", outputUrl.c_str());
    videoCodec_ = NULL;
    audioCodec_ = NULL;
    video_st = NULL;
    audio_st = NULL;
    audioCoder_ = NULL;
    hasVideo_ = true;
    prevEstimatedFps_ = 0;
    warmupfpsCount_ = 0;
    sequenceNumberFIR_ = 0;
    aviores_ = -1;
    writeheadres_=-1;
    unpackagedBufferpart_ = unpackagedBuffer_;
    initTime_ = 0;
    lastTime_ = 0;
    sinkfbSource_ = this;
    fbSink_ = NULL;
  }

  bool ExternalOutput::init(){
    av_register_all();
    avcodec_register_all();
    context_ = avformat_alloc_context();
    if (context_==NULL){
      ELOG_ERROR("Error allocating memory for IO context");
      return false;
    }
    oformat_ = av_guess_format(NULL,  url.c_str(), NULL);
    if (!oformat_){
      ELOG_ERROR("Error opening output file %s", url.c_str());
      return false;
    }
    url.copy(context_->filename, sizeof(context_->filename),0);
    video_st = NULL;
    audio_st = NULL;
    in_ = new InputProcessor();
    MediaInfo m;
    //    m.processorType = RTP_ONLY;
    m.hasVideo = false;
    m.hasAudio = false;

    gotUnpackagedFrame_ = 0;
    unpackagedSize_ = 0;
    in_->init(m, this);
    thread_ = boost::thread(&ExternalOutput::sendLoop, this);
    sending_ = true;
    ELOG_DEBUG("Initialized successfully");
    return true;
  }

  ExternalOutput::~ExternalOutput(){
    ELOG_DEBUG("Destructor");
    ELOG_DEBUG("Closing Sink");
    delete in_;
    in_ = NULL;

    if (context_!=NULL){
      if (writeheadres_>=0)
        av_write_trailer(context_);
      if (avio_close>=0)
        avio_close(context_->pb);
      avformat_free_context(context_);
      context_=NULL;
    }
    if (videoCodec_!=NULL){
      avcodec_close(videoCodecCtx_);
      videoCodec_=NULL;
    }
    if (audioCodec_!=NULL){
      avcodec_close(audioCodecCtx_);
      audioCodec_ = NULL;
    }

    sending_ = false;
    cond_.notify_one();
    thread_.join();
    /* boost::unique_lock<boost::mutex> lock(queueMutex_); */
    ELOG_DEBUG("ExternalOutput closed Successfully");
  }

  void ExternalOutput::receiveRawData(RawDataPacket& packet){
    return;
  }

  int ExternalOutput::writeAudioData(char* buf, int len){
    if (in_!=NULL){
      if (audioCodec_ == NULL) {
        if (warmupfpsCount_>=100){
          hasVideo_ = false;
          if (!this->initContext()){
            ELOG_ERROR("Context cannot be initialized properly, closing...");
            return -1;
          }
        }
        return 0;
      }

      timeval time;
      gettimeofday(&time, NULL);
      RtpHeader *head = reinterpret_cast<RtpHeader*> (buf);
      //We dont need any other payload at this time
      if(head->payloadtype != PCMU_8000_PT){
        return 0;
      }

      unsigned long long millis = (time.tv_sec * 1000) + (time.tv_usec / 1000);
      int ret = in_->unpackageAudio(reinterpret_cast<unsigned char*>(buf), len,
          unpackagedAudioBuffer_);
      if (ret <= 0)
        return ret;
      if (ret > UNPACKAGE_BUFFER_SIZE){
        ELOG_ERROR("Unpackaged Audio size too big %d", ret);
      }
      AVPacket avpkt;
      av_init_packet(&avpkt);
      avpkt.data = unpackagedAudioBuffer_;
      avpkt.size = ret;
      avpkt.pts = millis - initTime_;
      avpkt.dts = avpkt.pts;
      avpkt.stream_index = hasVideo_?1:0;
      av_write_frame(context_, &avpkt);
      av_free_packet(&avpkt);
      return ret;

    }
    return 0;
  }

  int ExternalOutput::writeVideoData(char* buf, int len){
    if (in_!=NULL){
      RtpHeader *head = reinterpret_cast<RtpHeader*> (buf);
      if (head->payloadtype == RED_90000_PT) {
        int totalLength = 12;

        if (head->extension) {
          totalLength += ntohs(head->extensionlength)*4 + 4; // RTP Extension header
        }
        int rtpHeaderLength = totalLength;
        RedHeader *redhead = reinterpret_cast<RedHeader*> ((buf + totalLength));

        //redhead->payloadtype = remoteSdp_.inOutPTMap[redhead->payloadtype];
        if (redhead->payloadtype == VP8_90000_PT) {
          while (redhead->follow) {
            totalLength += redhead->getLength() + 4; // RED header
            redhead = reinterpret_cast<RedHeader*> ((buf + totalLength));
          }
          // Parse RED packet to VP8 packet.
          // Copy RTP header
          memcpy(deliverMediaBuffer_, buf, rtpHeaderLength);
          // Copy payload data
          memcpy(deliverMediaBuffer_ + totalLength, buf + totalLength + 1, len - totalLength - 1);
          // Copy payload type
          RtpHeader *mediahead = reinterpret_cast<RtpHeader*> (deliverMediaBuffer_);
          mediahead->payloadtype = redhead->payloadtype;
          buf = reinterpret_cast<char*> (deliverMediaBuffer_);
          len = len - 1 - totalLength + rtpHeaderLength;
        }
      }
      int estimatedFps=0;
      int ret = in_->unpackageVideo(reinterpret_cast<unsigned char*>(buf), len,
          unpackagedBufferpart_, &gotUnpackagedFrame_, &estimatedFps);

      if (ret < 0)
        return 0;

      if (videoCodec_ == NULL) {
        if ((estimatedFps!=0)&&((estimatedFps < prevEstimatedFps_*(1-0.2))||(estimatedFps > prevEstimatedFps_*(1+0.2)))){
          prevEstimatedFps_ = estimatedFps;
        }
        if (warmupfpsCount_ >= 50){
          if (prevEstimatedFps_==0){
            warmupfpsCount_ = 0;
            return 0;
          }
          hasVideo_=true;
          if (!this->initContext()){
            ELOG_ERROR("Context cannot be initialized properly, closing...");
            return -1;
          }
        }
        return 0;
      }

      unpackagedSize_ += ret;
      unpackagedBufferpart_ += ret;
      if (unpackagedSize_ > UNPACKAGE_BUFFER_SIZE){
        ELOG_ERROR("Unpackaged size bigget than buffer %d", unpackagedSize_);
      }
      if (gotUnpackagedFrame_ && videoCodec_!=NULL) {
        timeval time;
        gettimeofday(&time, NULL);
        unsigned long long millis = (time.tv_sec * 1000) + (time.tv_usec / 1000);
        if (initTime_ == 0) {
          initTime_ = millis;
        }
        if (millis < initTime_)
        {
          ELOG_WARN("initTime is smaller than currentTime, possible problems when recording ");
        }
        unpackagedBufferpart_ -= unpackagedSize_;

        AVPacket avpkt;
        av_init_packet(&avpkt);
        avpkt.data = unpackagedBufferpart_;
        avpkt.size = unpackagedSize_;
        avpkt.pts = millis - initTime_;
        avpkt.stream_index = 0;
        av_write_frame(context_, &avpkt);
        av_free_packet(&avpkt);
        gotUnpackagedFrame_ = 0;
        unpackagedSize_ = 0;
        unpackagedBufferpart_ = unpackagedBuffer_;

      }
    }
    return 0;
  }

  int ExternalOutput::deliverAudioData_(char* buf, int len) {
    RtcpHeader *head = reinterpret_cast<RtcpHeader*> (buf);
    warmupfpsCount_++;
    if (head->isRtcp()){
      return 0;
    }
    this->queueData(buf,len,AUDIO_PACKET);
    return 0;
  }

  int ExternalOutput::deliverVideoData_(char* buf, int len) {
    RtcpHeader *head = reinterpret_cast<RtcpHeader*> (buf);
    warmupfpsCount_++;
    if (head->isRtcp()){
      return 0;
    }
    this->queueData(buf,len,VIDEO_PACKET);
    return 0;
  }

  bool ExternalOutput::initContext() {
    ELOG_DEBUG("Init Context");
    context_->oformat = oformat_;
    if (hasVideo_){
      context_->oformat->video_codec = AV_CODEC_ID_VP8;
    }
    context_->oformat->audio_codec = AV_CODEC_ID_PCM_MULAW;
    if (oformat_->video_codec != AV_CODEC_ID_NONE && videoCodec_ == NULL) {
      if (hasVideo_){
        videoCodec_ = avcodec_find_encoder(oformat_->video_codec);
        ELOG_DEBUG("Found Codec %s", videoCodec_->name);
        ELOG_DEBUG("Initing context with fps: %d", (int)prevEstimatedFps_);
        if (videoCodec_==NULL){
          ELOG_ERROR("Could not find codec");
          return false;
        }
        video_st = avformat_new_stream (context_, videoCodec_);
        video_st->id = 0;
        videoCodecCtx_ = video_st->codec;
        videoCodecCtx_->codec_id = oformat_->video_codec;
        videoCodecCtx_->width = 640;
        videoCodecCtx_->height = 480;
        videoCodecCtx_->time_base = (AVRational){1,(int)prevEstimatedFps_};
        videoCodecCtx_->pix_fmt = PIX_FMT_YUV420P;
        if (oformat_->flags & AVFMT_GLOBALHEADER){
          videoCodecCtx_->flags|=CODEC_FLAG_GLOBAL_HEADER;
        }
        oformat_->flags |= AVFMT_VARIABLE_FPS;
      }
      ELOG_DEBUG("Init audio context");

      audioCodec_ = avcodec_find_encoder(oformat_->audio_codec);
      if (audioCodec_==NULL){
        ELOG_ERROR("Could not find audio codec");
        return false;
      }
      ELOG_DEBUG("Found Audio Codec %s", audioCodec_->name);
      audio_st = avformat_new_stream (context_, audioCodec_);
      audio_st->id = 1;
      audioCodecCtx_ = audio_st->codec;
      audioCodecCtx_->codec_id = oformat_->audio_codec;
      audioCodecCtx_->sample_rate = 8000;
      audioCodecCtx_->channels = 1;
      //      audioCodecCtx_->sample_fmt = AV_SAMPLE_FMT_S8;
      if (oformat_->flags & AVFMT_GLOBALHEADER){
        audioCodecCtx_->flags|=CODEC_FLAG_GLOBAL_HEADER;
      }
      if(hasVideo_){
        context_->streams[0] = video_st;
        context_->streams[1] = audio_st;
      } else{
        ELOG_ERROR("No Video");
        context_->streams[0] = audio_st;
      }
      aviores_ = avio_open(&context_->pb, url.c_str(), AVIO_FLAG_WRITE);
      if (aviores_<0){
        ELOG_ERROR("Error opening output file");
        return false;
      }
      writeheadres_ = avformat_write_header(context_, NULL);
      if (writeheadres_<0){
        ELOG_ERROR("Error writing header");
        return false;
      }
      ELOG_DEBUG("AVFORMAT CONFIGURED");
    }
    return true;
  }

  void ExternalOutput::queueData(char* buffer, int length, packetType type){
    if (in_==NULL) {
      return;
    }
    boost::mutex::scoped_lock lock(queueMutex_);
    if (type == VIDEO_PACKET){
      videoQueue_.pushPacket(buffer, length);
    }else{
      audioQueue_.pushPacket(buffer, length);
    }
    cond_.notify_one();

  }

  int ExternalOutput::sendFirPacket() {
    if (fbSink_ != NULL) {
      sequenceNumberFIR_++; // do not increase if repetition
      int pos = 0;
      uint8_t rtcpPacket[50];
      // add full intra request indicator
      uint8_t FMT = 4;
      rtcpPacket[pos++] = (uint8_t) 0x80 + FMT;
      rtcpPacket[pos++] = (uint8_t) 206;
      pos = 12;
      fbSink_->deliverFeedback((char*)rtcpPacket, pos);
      return pos;
    }

    return -1;
  }

  void ExternalOutput::sendLoop() {
    while (sending_ == true) {
      boost::unique_lock<boost::mutex> lock(queueMutex_);
      while ((!audioQueue_.getSize())&&(!videoQueue_.getSize())) {
        cond_.wait(lock);
        if (sending_ == false) {
          lock.unlock();
          return;
        }
      }
      timeval time;
      gettimeofday(&time, NULL);
      unsigned long long millis = (time.tv_sec * 1000) + (time.tv_usec / 1000);
      if (millis -lastTime_ >FIR_INTERVAL_MS && (hasVideo_)){
        ELOG_DEBUG("SendingFIR");
        this->sendFirPacket();
        lastTime_ = millis;
      }
      if (initTime_ == 0) {
        initTime_ = millis;
      }
      if (millis < initTime_){
        ELOG_WARN("initTime is smaller than currentTime, possible problems when recording ");
      }
      if (audioQueue_.getSize()){
        boost::shared_ptr<dataPacket> audioP = audioQueue_.popPacket();
        this->writeAudioData(audioP->data, audioP->length);
      }
      if (videoQueue_.getSize()) {
        boost::shared_ptr<dataPacket> videoP = videoQueue_.popPacket();
        this->writeVideoData(videoP->data, videoP->length);

      }
      lock.unlock();
    }
  }

}

<|endoftext|>


<commit_before>void P020_TDCacheSystem()
{
   gPluginMgr->AddHandler("TSystem", "^dcache:", "TDCacheSystem",
      "DCache", "TDCacheSystem()");
   gPluginMgr->AddHandler("TSystem", "^dcap:", "TDCacheSystem",
      "DCache", "TDCacheSystem()");
}
<commit_msg>add missing entry for gsidcap protocol. Fixes issue 69656.<commit_after>void P020_TDCacheSystem()
{
   gPluginMgr->AddHandler("TSystem", "^dcache:", "TDCacheSystem",
      "DCache", "TDCacheSystem()");
   gPluginMgr->AddHandler("TSystem", "^dcap:", "TDCacheSystem",
      "DCache", "TDCacheSystem()");
   gPluginMgr->AddHandler("TSystem", "^gsidcap:", "TDCacheSystem",
      "DCache", "TDCacheSystem()");
}
<|endoftext|>


<commit_before>/*
 *
 *    Copyright (c) 2020 Project CHIP Authors
 *    All rights reserved.
 *
 *    Licensed under the Apache License, Version 2.0 (the "License");
 *    you may not use this file except in compliance with the License.
 *    You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing, software
 *    distributed under the License is distributed on an "AS IS" BASIS,
 *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *    See the License for the specific language governing permissions and
 *    limitations under the License.
 */

#include <app-common/zap-generated/callback.h>
#include <app-common/zap-generated/ids/Clusters.h>
#include <app/Command.h>
#include <app/ConcreteAttributePath.h>
#include <app/clusters/identify-server/identify-server.h>
#include <app/util/af.h>

#include "AppMain.h"

using namespace chip;

bool emberAfBasicClusterMfgSpecificPingCallback(chip::app::Command * commandObj)
{
    emberAfSendDefaultResponse(emberAfCurrentCommand(), EMBER_ZCL_STATUS_SUCCESS);
    return true;
}

void OnIdentifyStart(Identify *)
{
    ChipLogProgress(Zcl, "OnIdentifyStart");
}

void OnIdentifyStop(Identify *)
{
    ChipLogProgress(Zcl, "OnIdentifyStop");
}

void OnTriggerEffect(Identify * identify)
{
    switch (identify->mCurrentEffectIdentifier)
    {
    case EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_BLINK:
        ChipLogProgress(Zcl, "EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_BLINK");
        break;
    case EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_BREATHE:
        ChipLogProgress(Zcl, "EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_BREATHE");
        break;
    case EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_OKAY:
        ChipLogProgress(Zcl, "EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_OKAY");
        break;
    case EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_CHANNEL_CHANGE:
        ChipLogProgress(Zcl, "EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_CHANNEL_CHANGE");
        break;
    default:
        ChipLogProgress(Zcl, "No identifier effect");
        return;
    }
}

static Identify gIdentify0 = {
    chip::EndpointId{ 0 }, OnIdentifyStart, OnIdentifyStop, EMBER_ZCL_IDENTIFY_IDENTIFY_TYPE_VISIBLE_LED, OnTriggerEffect,
};

static Identify gIdentify1 = {
    chip::EndpointId{ 1 }, OnIdentifyStart, OnIdentifyStop, EMBER_ZCL_IDENTIFY_IDENTIFY_TYPE_VISIBLE_LED, OnTriggerEffect,
};

int main(int argc, char * argv[])
{
    VerifyOrDie(ChipLinuxAppInit(argc, argv) == 0);
    ChipLinuxAppMainLoop();
    return 0;
}
<commit_msg>Remove unnecessary headers from all-clusters-app main. (#11587)<commit_after>/*
 *
 *    Copyright (c) 2020 Project CHIP Authors
 *    All rights reserved.
 *
 *    Licensed under the Apache License, Version 2.0 (the "License");
 *    you may not use this file except in compliance with the License.
 *    You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing, software
 *    distributed under the License is distributed on an "AS IS" BASIS,
 *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *    See the License for the specific language governing permissions and
 *    limitations under the License.
 */

#include <app/Command.h>
#include <app/clusters/identify-server/identify-server.h>
#include <app/util/af.h>

#include "AppMain.h"

bool emberAfBasicClusterMfgSpecificPingCallback(chip::app::Command * commandObj)
{
    emberAfSendDefaultResponse(emberAfCurrentCommand(), EMBER_ZCL_STATUS_SUCCESS);
    return true;
}

void OnIdentifyStart(Identify *)
{
    ChipLogProgress(Zcl, "OnIdentifyStart");
}

void OnIdentifyStop(Identify *)
{
    ChipLogProgress(Zcl, "OnIdentifyStop");
}

void OnTriggerEffect(Identify * identify)
{
    switch (identify->mCurrentEffectIdentifier)
    {
    case EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_BLINK:
        ChipLogProgress(Zcl, "EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_BLINK");
        break;
    case EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_BREATHE:
        ChipLogProgress(Zcl, "EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_BREATHE");
        break;
    case EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_OKAY:
        ChipLogProgress(Zcl, "EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_OKAY");
        break;
    case EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_CHANNEL_CHANGE:
        ChipLogProgress(Zcl, "EMBER_ZCL_IDENTIFY_EFFECT_IDENTIFIER_CHANNEL_CHANGE");
        break;
    default:
        ChipLogProgress(Zcl, "No identifier effect");
        return;
    }
}

static Identify gIdentify0 = {
    chip::EndpointId{ 0 }, OnIdentifyStart, OnIdentifyStop, EMBER_ZCL_IDENTIFY_IDENTIFY_TYPE_VISIBLE_LED, OnTriggerEffect,
};

static Identify gIdentify1 = {
    chip::EndpointId{ 1 }, OnIdentifyStart, OnIdentifyStop, EMBER_ZCL_IDENTIFY_IDENTIFY_TYPE_VISIBLE_LED, OnTriggerEffect,
};

int main(int argc, char * argv[])
{
    VerifyOrDie(ChipLinuxAppInit(argc, argv) == 0);
    ChipLinuxAppMainLoop();
    return 0;
}
<|endoftext|>


<commit_before>/**
 * @copyright
 * ====================================================================
 *    Licensed to the Apache Software Foundation (ASF) under one
 *    or more contributor license agreements.  See the NOTICE file
 *    distributed with this work for additional information
 *    regarding copyright ownership.  The ASF licenses this file
 *    to you under the Apache License, Version 2.0 (the
 *    "License"); you may not use this file except in compliance
 *    with the License.  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing,
 *    software distributed under the License is distributed on an
 *    "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 *    KIND, either express or implied.  See the License for the
 *    specific language governing permissions and limitations
 *    under the License.
 * ====================================================================
 * @endcopyright
 *
 * @file org_apache_subversion_javahl_types_Version.cpp
 * @brief Implementation of the native methods in the Java class Version.
 */

#include "../include/org_apache_subversion_javahl_types_Version.h"
#include "JNIStackElement.h"
#include "svn_version.h"

JNIEXPORT jint JNICALL
Java_org_apache_subversion_javahl_types_Version_getMajor(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getMajor);
  return SVN_VER_MAJOR;
}

JNIEXPORT jint JNICALL
Java_org_apache_subversion_javahl_types_Version_getMinor(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getMinor);
  return SVN_VER_MINOR;
}

JNIEXPORT jint JNICALL
Java_org_apache_subversion_javahl_types_Version_getPatch(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getPatch);
  return SVN_VER_PATCH;
}

JNIEXPORT jstring JNICALL
Java_org_apache_subversion_javahl_types_Version_getTag(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getTag);
  jstring tag = JNIUtil::makeJString(SVN_VER_TAG);
  if (JNIUtil::isJavaExceptionThrown())
    return NULL;

  return tag;
}

JNIEXPORT jstring JNICALL
Java_org_apache_subversion_javahl_types_Version_getNumberTag(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getNumberTag);
  jstring numtag = JNIUtil::makeJString(SVN_VER_NUMTAG);
  if (JNIUtil::isJavaExceptionThrown())
    return NULL;

  return numtag;
}
<commit_msg>Follow up to r1651565: make sure that the JavaHL native library has an embedded version string like the core libraries do.<commit_after>/**
 * @copyright
 * ====================================================================
 *    Licensed to the Apache Software Foundation (ASF) under one
 *    or more contributor license agreements.  See the NOTICE file
 *    distributed with this work for additional information
 *    regarding copyright ownership.  The ASF licenses this file
 *    to you under the Apache License, Version 2.0 (the
 *    "License"); you may not use this file except in compliance
 *    with the License.  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing,
 *    software distributed under the License is distributed on an
 *    "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 *    KIND, either express or implied.  See the License for the
 *    specific language governing permissions and limitations
 *    under the License.
 * ====================================================================
 * @endcopyright
 *
 * @file org_apache_subversion_javahl_types_Version.cpp
 * @brief Implementation of the native methods in the Java class Version.
 */

#include "../include/org_apache_subversion_javahl_types_Version.h"
#include "JNIStackElement.h"
#include "svn_version.h"

namespace {
const svn_version_t* javahl_version()
{
  SVN_VERSION_BODY;
}
} //anonymous namespace

JNIEXPORT jint JNICALL
Java_org_apache_subversion_javahl_types_Version_getMajor(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getMajor);
  return javahl_version()->major;
}

JNIEXPORT jint JNICALL
Java_org_apache_subversion_javahl_types_Version_getMinor(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getMinor);
  return javahl_version()->minor;
}

JNIEXPORT jint JNICALL
Java_org_apache_subversion_javahl_types_Version_getPatch(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getPatch);
  return javahl_version()->patch;
}

JNIEXPORT jstring JNICALL
Java_org_apache_subversion_javahl_types_Version_getTag(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getTag);
  jstring tag = JNIUtil::makeJString(SVN_VER_TAG);
  if (JNIUtil::isJavaExceptionThrown())
    return NULL;

  return tag;
}

JNIEXPORT jstring JNICALL
Java_org_apache_subversion_javahl_types_Version_getNumberTag(JNIEnv *env,
 jobject jthis)
{
  JNIEntry(Version, getNumberTag);
  jstring numtag = JNIUtil::makeJString(SVN_VER_NUMTAG);
  if (JNIUtil::isJavaExceptionThrown())
    return NULL;

  return numtag;
}
<|endoftext|>


<commit_before>#include <unistd.h>
#include <array>
#include <atomic>
#include <cmath>
#include <functional>
#include <iostream>

#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>

#include <eigen3/Eigen/Dense>

/**
 * @example cartesian_impedance_example.cpp
 * An example showing a simple cartesian impedance controller that renders a spring damper system
 * where the equilibrium is the initial configuration.
 * After starting the controller try to push the robot around and try different stiffness levels.
 * Usage:
 * ./cartesian_impedance_example <robot-hostname> <translational_stiffness> <rotational_stiffness>
 */

int main(int argc, char** argv) {
  // Check whether the required arguments were passed
  if (argc != 4) {
    std::cerr << "Usage: ./" << argv[0]
              << " <robot-hostname> <translational_stiffness> <rotational_stiffness>" << std::endl;
    return -1;
  }

  // Compliance parameters
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  stiffness.setZero();
  stiffness.topLeftCorner(3, 3) << std::stod(argv[2]) * Eigen::MatrixXd::Identity(3, 3);
  stiffness.bottomRightCorner(3, 3) << std::stod(argv[3]) * Eigen::MatrixXd::Identity(3, 3);
  damping.setZero();
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(std::stod(argv[2])) * Eigen::MatrixXd::Identity(3, 3);
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(std::stod(argv[3])) *
                                         Eigen::MatrixXd::Identity(3, 3);

  try {
    // connect to robot
    franka::Robot robot(argv[1]);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();

    franka::RobotState initial_state = robot.readOnce();

    // equilibrium point is the initial position
    Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
    Eigen::Vector3d position_d(initial_transform.translation());
    Eigen::Quaterniond orientation_d(initial_transform.linear());

    // set collision behavior
    robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});

    // define callback for the torque control loop
    std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
        impedance_control_callback = [&](const franka::RobotState& robot_state,
                                         franka::Duration /*duration*/) -> franka::Torques {
      // get state variables
      std::array<double, 7> coriolis_array = model.coriolis(
          robot_state, {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}, 0.0, {{0.0, 0.0, 0.0}});
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);

      // convert to Eigen
      Eigen::Map<const Eigen::Matrix<double, 7, 1> > coriolis(coriolis_array.data());
      Eigen::Map<const Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1> > q(robot_state.q.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1> > dq(robot_state.dq.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1> > tau_ext(
          robot_state.tau_ext_hat_filtered.data());
      Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
      Eigen::Vector3d position(transform.translation());
      Eigen::Quaterniond orientation(transform.linear());

      // compute error to desired equilibrium pose
      // position error
      Eigen::Matrix<double, 6, 1> error;
      error.head(3) << position - position_d;

      // orientation error
      // "difference" quaternion
      Eigen::Quaterniond error_quaternion(orientation * orientation_d.inverse());
      // convert to axis angle
      Eigen::AngleAxisd error_quaternion_angle_axis(error_quaternion);
      // compute "orientation error"
      error.tail(3) << error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle();

      // compute control
      Eigen::VectorXd tau_task(7), tau_d(7);

      // Spring damper system with damping ratio=1
      tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
      tau_d << tau_task + coriolis - tau_ext;

      std::array<double, 7> tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
      return tau_d_array;
    };

    // start real-time control loop
    robot.control(impedance_control_callback);

  } catch (const franka::Exception& ex) {
    // print exception
    std::cout << ex.what() << std::endl;
  }

  return 0;
}<commit_msg>corrects comments<commit_after>#include <unistd.h>
#include <array>
#include <atomic>
#include <cmath>
#include <functional>
#include <iostream>

#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>

#include <eigen3/Eigen/Dense>

/**
 * @example cartesian_impedance_control.cpp
 * An example showing a simple cartesian impedance controller that renders a spring damper system
 * where the equilibrium is the initial configuration.
 * After starting the controller try to push the robot around and try different stiffness levels.
 * Usage:
 * ./cartesian_impedance_control <robot-hostname> <translational_stiffness> <rotational_stiffness>
 */

int main(int argc, char** argv) {
  // Check whether the required arguments were passed
  if (argc != 4) {
    std::cerr << "Usage: ./" << argv[0]
              << " <robot-hostname> <translational_stiffness> <rotational_stiffness>" << std::endl;
    return -1;
  }

  // Compliance parameters
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  stiffness.setZero();
  stiffness.topLeftCorner(3, 3) << std::stod(argv[2]) * Eigen::MatrixXd::Identity(3, 3);
  stiffness.bottomRightCorner(3, 3) << std::stod(argv[3]) * Eigen::MatrixXd::Identity(3, 3);
  damping.setZero();
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(std::stod(argv[2])) * Eigen::MatrixXd::Identity(3, 3);
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(std::stod(argv[3])) *
                                         Eigen::MatrixXd::Identity(3, 3);

  try {
    // connect to robot
    franka::Robot robot(argv[1]);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();

    franka::RobotState initial_state = robot.readOnce();

    // equilibrium point is the initial position
    Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
    Eigen::Vector3d position_d(initial_transform.translation());
    Eigen::Quaterniond orientation_d(initial_transform.linear());

    // set collision behavior
    robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});

    // define callback for the torque control loop
    std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
        impedance_control_callback = [&](const franka::RobotState& robot_state,
                                         franka::Duration /*duration*/) -> franka::Torques {
      // get state variables
      std::array<double, 7> coriolis_array = model.coriolis(
          robot_state, {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}, 0.0, {{0.0, 0.0, 0.0}});
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);

      // convert to Eigen
      Eigen::Map<const Eigen::Matrix<double, 7, 1> > coriolis(coriolis_array.data());
      Eigen::Map<const Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1> > q(robot_state.q.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1> > dq(robot_state.dq.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1> > tau_ext(
          robot_state.tau_ext_hat_filtered.data());
      Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
      Eigen::Vector3d position(transform.translation());
      Eigen::Quaterniond orientation(transform.linear());

      // compute error to desired equilibrium pose
      // position error
      Eigen::Matrix<double, 6, 1> error;
      error.head(3) << position - position_d;

      // orientation error
      // "difference" quaternion
      Eigen::Quaterniond error_quaternion(orientation * orientation_d.inverse());
      // convert to axis angle
      Eigen::AngleAxisd error_quaternion_angle_axis(error_quaternion);
      // compute "orientation error"
      error.tail(3) << error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle();

      // compute control
      Eigen::VectorXd tau_task(7), tau_d(7);

      // Spring damper system with damping ratio=1
      tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
      tau_d << tau_task + coriolis - tau_ext;

      std::array<double, 7> tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
      return tau_d_array;
    };

    // start real-time control loop
    robot.control(impedance_control_callback);

  } catch (const franka::Exception& ex) {
    // print exception
    std::cout << ex.what() << std::endl;
  }

  return 0;
}<|endoftext|>


<commit_before><commit_msg>In ~BrowserToolbarGtk, remove notifications we added in the ctor.<commit_after><|endoftext|>