Merge "[NRRPLT-2545] Added joint_states topic to gzbridge for passing through...

Merge "[NRRPLT-2545] Added joint_states topic to gzbridge for passing through joint update messages to gz3d."

gzbridge/GazeboInterface.cc

@@ -47,6 +47,9 @@ GazeboInterface::GazeboInterface()
   this->sensorTopic = "~/sensor";
   this->visualTopic = "~/visual";
   this->jointTopic = "~/joint";
+
+  this->jointStatesTopic = "~/joint_states";
+
   this->modelTopic = "~/model/info";
   this->poseTopic = "~/pose/info";
   this->requestTopic = "~/request";
@@ -74,6 +77,9 @@ GazeboInterface::GazeboInterface()
   this->jointSub = this->node->Subscribe(this->jointTopic,
       &GazeboInterface::OnJointMsg, this);
 
+  this->jointStatesSub = this->node->Subscribe(this->jointStatesTopic,
+      &GazeboInterface::OnJointStateMsg, this);
+
   // For entity creation
   this->modelInfoSub = node->Subscribe(this->modelTopic,
       &GazeboInterface::OnModelMsg, this);
@@ -165,6 +171,9 @@ GazeboInterface::~GazeboInterface()
   this->sceneMsgs.clear();
   this->physicsMsgs.clear();
   this->jointMsgs.clear();
+
+  this->jointStateMsgs.clear();
+
   this->sensorMsgs.clear();
 
   this->sensorSub.reset();
@@ -172,6 +181,9 @@ GazeboInterface::~GazeboInterface()
   this->lightSub.reset();
   this->sceneSub.reset();
   this->jointSub.reset();
+
+  this->jointStatesSub.reset();
+
   this->modelInfoSub.reset();
   this->requestPub.reset();
   this->modelPub.reset();
@@ -256,6 +268,8 @@ void GazeboInterface::ProcessMessages()
   static JointMsgs_L::iterator jointIter;
   static SensorMsgs_L::iterator sensorIter;
 
+  static JointStateMsgs_L::iterator jointStateIter;
+
   {
     boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
 
@@ -679,6 +693,16 @@ void GazeboInterface::ProcessMessages()
     }
     this->jointMsgs.clear();
 
+    // Forward joint state messages.
+    for (jointStateIter = this->jointStateMsgs.begin();
+        jointStateIter != this->jointStateMsgs.end(); ++jointStateIter)
+    {
+      msg = this->PackOutgoingTopicMsg(this->jointStatesTopic,
+          pb2json(*(*jointStateIter).get()));
+      this->Send(msg);
+    }
+    this->jointStateMsgs.clear();
+
     // Forward the request messages
     for (rIter =  this->requestMsgs.begin(); rIter != this->requestMsgs.end();
         ++rIter)
@@ -1021,6 +1045,19 @@ void GazeboInterface::OnJointMsg(ConstJointPtr &_msg)
   this->receiveCondition->notify_one();
 }
 
+/////////////////////////////////////////////////
+void GazeboInterface::OnJointStateMsg(ConstJointStatePtr &_msg)
+{
+  if (!this->IsConnected())
+    return;
+
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+  this->jointStateMsgs.push_back(_msg);
+  this->receivedMessage = true;
+  lock.unlock();
+  this->receiveCondition->notify_one();
+}
+
 /////////////////////////////////////////////////
 void GazeboInterface::OnSensorMsg(ConstSensorPtr &_msg)
 {

gzbridge/GazeboInterface.hh

@@ -130,6 +130,10 @@ namespace gzweb
     /// \param[in] _msg The message data.
     private: void OnJointMsg(ConstJointPtr &_msg);
 
+    /// \brief Joint message callback.
+    /// \param[in] _msg The message data.
+    private: void OnJointStateMsg(ConstJointStatePtr &_msg);
+
     /// \brief Scene message callback.
     /// \param[in] _msg The message data.
     private: void OnSensorMsg(ConstSensorPtr &_msg);
@@ -222,6 +226,9 @@ namespace gzweb
 
     /// \brief Subscribe to joint updates.
     private: gazebo::transport::SubscriberPtr jointSub;
+    
+    /// \brief Subscribe to joint state updates.
+    private: gazebo::transport::SubscriberPtr jointStatesSub;
 
     /// \brief Subscribe to road msgs.
     private: gazebo::transport::SubscriberPtr roadSub;
@@ -322,6 +329,14 @@ namespace gzweb
     /// \brief List of joint message to process.
     private: JointMsgs_L jointMsgs;
 
+    /// \def JointStateMsgs_L
+    /// \brief List of joint state messages.
+    typedef std::list<boost::shared_ptr<gazebo::msgs::JointState const> >
+        JointStateMsgs_L;
+
+    /// \brief List of joint message to process.
+    private: JointStateMsgs_L jointStateMsgs;
+
     /// \def SceneMsgs_L
     /// \brief List of scene messages.
     typedef std::list<boost::shared_ptr<gazebo::msgs::Scene const> >
@@ -375,6 +390,9 @@ namespace gzweb
     /// \brief Name of joint topic.
     private: std::string jointTopic;
 
+    /// \brief Name of joint states topic.
+    private: std::string jointStatesTopic;
+
     /// \brief Name of model topic.
     private: std::string modelTopic;