diff --git a/gzbridge/GazeboInterface.cc b/gzbridge/GazeboInterface.cc index 3c656077fd7d4b0ec6b2c10a7523ed1fffc65178..7e11433d9828d7de68d4668842c21e7479f3034a 100644 --- a/gzbridge/GazeboInterface.cc +++ b/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) { diff --git a/gzbridge/GazeboInterface.hh b/gzbridge/GazeboInterface.hh index a10710e35168bc5249db9f033e3b0e9d0b22d139..da75d94eb70e4c95ff1317350d94a6973352827a 100644 --- a/gzbridge/GazeboInterface.hh +++ b/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;