Connect to gazebo using a custom socket client

edit

Hi,

My goal here is to control a model in gazebo using keys from the keyboard. I was able to do that using plugins within gazebo using a publisher and subscriber pattern that sends and receives information over topics. But now I want to achieve the same by a completely independent process say using ZeroMQ that does not include any gazebo dependencies. Can you guys give me any insights to this. It will be very helpful.

Code for plugin1:

KeyboardGUIPlugin::KeyboardGUIPlugin()
  : GUIPlugin(), dataPtr(new KeyboardGUIPluginPrivate)
{
  gazebo::gui::MainWindow *mainWindow = gazebo::gui::get_main_window();
  if (!mainWindow)
  {
    gzerr << "Couldn't get main window, keyboard events won't be filtered."
        << std::endl;
    return;
  }
  mainWindow->installEventFilter(this);

  // Make this invisible
  this->move(-1, -1);
  this->resize(1, 1);

  // Initialize transport.
  // Publish the key presses to the autoCrane topic
  this->dataPtr->gzNode = transport::NodePtr(new transport::Node());
  this->dataPtr->gzNode->Init();
  this->dataPtr->keyboardPub =
      this->dataPtr->gzNode->Advertise<msgs::Any>("~/keyboard/keypress");
}

/////////////////////////////////////////////////
KeyboardGUIPlugin::~KeyboardGUIPlugin()
{
  this->dataPtr->keyboardPub.reset();
  this->dataPtr->gzNode->Fini();
}

/////////////////////////////////////////////////
// Send the message
void KeyboardGUIPlugin::OnKeyPress(const gazebo::common::KeyEvent &_event)
{
  msgs::Any msg;
  msg.set_type(msgs::Any_ValueType_INT32);
  msg.set_int_value(_event.key);
  this->dataPtr->keyboardPub->Publish(msg);
}

/////////////////////////////////////////////////  
bool KeyboardGUIPlugin::eventFilter(QObject *_obj, QEvent *_event)
{
  // Check if there was a keypress in a child
  if (_event->type() == QEvent::ShortcutOverride)
  {
    QKeyEvent *qtKeyEvent = dynamic_cast<QKeyEvent *>(_event);

    gazebo::common::KeyEvent gazeboKeyEvent;
    gazeboKeyEvent.text = qtKeyEvent->text().toStdString();

    // QKeyEvent::key() does not distiguish between lowercase and uppercase.
    // Need to use QKeyEvent::text() to get the unicode character.
    // Special keys (shift, ctrl) will have an empty text field.
    gazeboKeyEvent.key = !gazeboKeyEvent.text.empty() ? gazeboKeyEvent.text[0] :
      qtKeyEvent->key();

    this->OnKeyPress(gazeboKeyEvent);
  }
  return QObject::eventFilter(_obj, _event);
}

code for plugin2:

void KeysToJointsPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{ 
  this->model = _model;
  auto controller = this->model->GetJointController();

  // Load params from SDF
  if (_sdf->HasElement("map"))
  {
    auto mapElem = _sdf->GetElement("map");
    while (mapElem)
    {
       // Get the joints from the sdf file
      auto jointName = mapElem->Get<std::string>("joint");
      auto joint = this->model->GetJoint(jointName);
      if (!joint)
      {
        // It will complain if no joints are found
        gzwarn << "Can't find joint [" << jointName << "]" << std::endl;
      }
      else
      {
        // The joint should have the below characteristics
        if (!mapElem->HasAttribute("key") ||
            !mapElem->HasAttribute("scale") ||
            !mapElem->HasAttribute("type"))
        {
          gzwarn << "Missing [key], [scale] or [type] attribute, skipping map."
              << std::endl;
          mapElem = mapElem->GetNextElement("map");
          continue;
        }
        KeyInfo info;
        info.key = mapElem->Get<int>("key");
        info.joint = joint;
        info.scale = mapElem->Get<double>("scale");
        info.type = mapElem->Get<std::string>("type");

        if (info.type != "force")
        {
          double kp = 0;
          double ki = 0;
          double kd = 0;
          if (mapElem->HasAttribute("kp"))
            kp = mapElem->Get<double>("kp");
          if (mapElem->HasAttribute("ki"))
            ki = mapElem->Get<double>("ki");
          if (mapElem->HasAttribute("kd"))
            kd = mapElem->Get<double>("kd");

          common::PID pid(kp, ki, kd);
          if (info.type == "position")
            controller->SetPositionPID(info.joint->GetScopedName(), pid);
          else if (info.type == "velocity")
            controller->SetVelocityPID(info.joint->GetScopedName(), pid);
         }

         this->keys.push_back(info);
       }

      mapElem = mapElem->GetNextElement("map");
    }
  }

  // Initialize transport
  this->node = transport::NodePtr(new transport::Node());
  this->node->Init();

  // subscribe to KeyboardGUIPlugin by registering OnKeyPress() callback ...