Merge "[No story] Added version check to allow compilation with Gazebo...

Merge "[No story] Added version check to allow compilation with Gazebo versions that do not have the joint state message extension from the EPFL repository."

gzbridge/GazeboInterface.cc

@@ -48,7 +48,9 @@ GazeboInterface::GazeboInterface()
   this->visualTopic = "~/visual";
   this->jointTopic = "~/joint";
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
   this->jointStatesTopic = "~/joint_states";
+#endif
 
   this->modelTopic = "~/model/info";
   this->poseTopic = "~/pose/info";
@@ -77,8 +79,10 @@ GazeboInterface::GazeboInterface()
   this->jointSub = this->node->Subscribe(this->jointTopic,
       &GazeboInterface::OnJointMsg, this);
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
   this->jointStatesSub = this->node->Subscribe(this->jointStatesTopic,
       &GazeboInterface::OnJointStateMsg, this);
+#endif
 
   // For entity creation
   this->modelInfoSub = node->Subscribe(this->modelTopic,
@@ -172,7 +176,9 @@ GazeboInterface::~GazeboInterface()
   this->physicsMsgs.clear();
   this->jointMsgs.clear();
 
+ #ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
   this->jointStateMsgs.clear();
+#endif
   
   this->sensorMsgs.clear();
 
@@ -182,7 +188,9 @@ GazeboInterface::~GazeboInterface()
   this->sceneSub.reset();
   this->jointSub.reset();
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
   this->jointStatesSub.reset();
+#endif
 
   this->modelInfoSub.reset();
   this->requestPub.reset();
@@ -268,7 +276,9 @@ void GazeboInterface::ProcessMessages()
   static JointMsgs_L::iterator jointIter;
   static SensorMsgs_L::iterator sensorIter;
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES   
   static JointStateMsgs_L::iterator jointStateIter;
+#endif
   
   {
     boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
@@ -693,6 +703,7 @@ void GazeboInterface::ProcessMessages()
     }
     this->jointMsgs.clear();
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
     // Forward joint state messages.
     for (jointStateIter = this->jointStateMsgs.begin();
         jointStateIter != this->jointStateMsgs.end(); ++jointStateIter)
@@ -702,6 +713,7 @@ void GazeboInterface::ProcessMessages()
       this->Send(msg);
     }
     this->jointStateMsgs.clear();
+#endif
 
     // Forward the request messages
     for (rIter =  this->requestMsgs.begin(); rIter != this->requestMsgs.end();
@@ -1045,6 +1057,7 @@ void GazeboInterface::OnJointMsg(ConstJointPtr &_msg)
   this->receiveCondition->notify_one();
 }
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
 /////////////////////////////////////////////////
 void GazeboInterface::OnJointStateMsg(ConstJointStatePtr &_msg)
 {
@@ -1057,6 +1070,7 @@ void GazeboInterface::OnJointStateMsg(ConstJointStatePtr &_msg)
   lock.unlock();
   this->receiveCondition->notify_one();
 }
+#endif
 
 /////////////////////////////////////////////////
 void GazeboInterface::OnSensorMsg(ConstSensorPtr &_msg)

gzbridge/GazeboInterface.hh

@@ -25,6 +25,14 @@
 #include <gazebo/msgs/msgs.hh>
 #include <gazebo/transport/TransportIface.hh>
 
+// Version check for joint update message support
+#ifdef GAZEBO_VERSION_FULL
+  #define GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
+#else
+  #pragma message("Found no HBP Gazebo version : disabling HBP features")
+#endif
+
+
 namespace boost
 {
   class thread;
@@ -130,9 +138,11 @@ namespace gzweb
     /// \param[in] _msg The message data.
     private: void OnJointMsg(ConstJointPtr &_msg);
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
     /// \brief Joint message callback.
     /// \param[in] _msg The message data.
     private: void OnJointStateMsg(ConstJointStatePtr &_msg);
+#endif
     
     /// \brief Scene message callback.
     /// \param[in] _msg The message data.
@@ -227,8 +237,10 @@ namespace gzweb
     /// \brief Subscribe to joint updates.
     private: gazebo::transport::SubscriberPtr jointSub;
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
     /// \brief Subscribe to joint state updates.
     private: gazebo::transport::SubscriberPtr jointStatesSub;
+#endif
 
     /// \brief Subscribe to road msgs.
     private: gazebo::transport::SubscriberPtr roadSub;
@@ -329,6 +341,7 @@ namespace gzweb
     /// \brief List of joint message to process.
     private: JointMsgs_L jointMsgs;
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
     /// \def JointStateMsgs_L
     /// \brief List of joint state messages.
     typedef std::list<boost::shared_ptr<gazebo::msgs::JointState const> >
@@ -336,6 +349,7 @@ namespace gzweb
 
     /// \brief List of joint message to process.
     private: JointStateMsgs_L jointStateMsgs;
+#endif
     
     /// \def SceneMsgs_L
     /// \brief List of scene messages.
@@ -390,8 +404,10 @@ namespace gzweb
     /// \brief Name of joint topic.
     private: std::string jointTopic;
 
+#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
     /// \brief Name of joint states topic.
     private: std::string jointStatesTopic;
+#endif
 
     /// \brief Name of model topic.
     private: std::string modelTopic;