Enable to compile hrpsys_gazebo_general with indigo

start-jsk · Nov 21, 2019 · 11de69e · 11de69e
1 parent 02177b9
commit 11de69e
Show file tree

Hide file tree

Showing 6 changed files with 86 additions and 36 deletions.
diff --git a/hrpsys_gazebo_general/CMakeLists.txt b/hrpsys_gazebo_general/CMakeLists.txt
@@ -17,7 +17,20 @@ if(NOT CMAKE_BUILD_TYPE)
     "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel."
     FORCE)
 endif()
-set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
+
+# Gazebo only supports C++11 from version 5
+# http://answers.gazebosim.org/question/13869/ros-enabled-plugin-examples-use-c11/
+include (FindPkgConfig)
+if (PKG_CONFIG_FOUND)
+  pkg_check_modules(GAZEBO gazebo)
+else()
+  message(FATAL_ERROR "pkg-config is required; please install it")
+endif()
+if(${GAZEBO_VERSION} VERSION_LESS "5.0.0")
+else()
+  set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+endif()
+
 ## Build only gazebo iob
 pkg_check_modules(omniorb omniORB4 REQUIRED)
 pkg_check_modules(omnidynamic omniDynamic4 REQUIRED)
@@ -42,13 +55,6 @@ add_custom_target(hrpsys_gazebo_general_iob ALL DEPENDS RobotHardware_gazebo)
 add_dependencies(hrpsys_gazebo_general_iob hrpsys_gazebo_msgs_gencpp)
 
 ## Gazebo plugins
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
-  pkg_check_modules(GAZEBO gazebo)
-else()
-  message(FATAL_ERROR "pkg-config is required; please install it")
-endif()
-
 include_directories( ${GAZEBO_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${openhrp3_INCLUDE_DIRS})
 link_directories( ${GAZEBO_LIBRARY_DIRS} ${openhrp3_LIBRARY_DIRS})
 

diff --git a/hrpsys_gazebo_general/src/IOBPlugin.cpp b/hrpsys_gazebo_general/src/IOBPlugin.cpp
@@ -1,13 +1,19 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string>
-#include <memory>
 
 #include <gazebo/transport/Node.hh>
 #include <gazebo/common/Assert.hh>
 
 #include "IOBPlugin.h"
 
+#if __cplusplus >= 201103L
+#include <memory>
+using std::dynamic_pointer_cast;
+#else
+using boost::dynamic_pointer_cast;
+#endif
+
 namespace gazebo
 {
 GZ_REGISTER_MODEL_PLUGIN(IOBPlugin);
@@ -241,10 +247,10 @@ void IOBPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
             ROS_ERROR("Force-Torque sensor: %s has invalid configuration", sensor_name.c_str());
           }
           // setup force sensor publishers
-          std::shared_ptr<std::vector<std::shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue(new std::vector<std::shared_ptr<geometry_msgs::WrenchStamped> >);
+          shared_ptr<std::vector<shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue(new std::vector<shared_ptr<geometry_msgs::WrenchStamped> >);
           // forceValQueue->resize(this->force_sensor_average_window_size);
           for ( int i=0; i<this->force_sensor_average_window_size; i++ ){
-            std::shared_ptr<geometry_msgs::WrenchStamped> fbuf(new geometry_msgs::WrenchStamped);
+            shared_ptr<geometry_msgs::WrenchStamped> fbuf(new geometry_msgs::WrenchStamped);
             forceValQueue->push_back(fbuf);
           }
           this->forceValQueueMap[sensor_name] = forceValQueue;
@@ -276,7 +282,7 @@ void IOBPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
               gzerr << ln << " not found\n";
             } else {
               // Get imu sensors
-              msi.sensor = std::dynamic_pointer_cast<sensors::ImuSensor>
+              msi.sensor = dynamic_pointer_cast<sensors::ImuSensor>
                 (sensors::SensorManager::Instance()->GetSensor
                  (this->world->GetName() + "::" + msi.link->GetScopedName() + "::" + msi.sensor_name));
 
@@ -378,7 +384,7 @@ void IOBPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
     // effort average
     effortValQueue.resize(0);
     for(int i = 0; i < this->effort_average_window_size; i++) {
-      std::shared_ptr<std::vector<double> > vbuf(new std::vector<double> (this->joints.size()));
+      shared_ptr<std::vector<double> > vbuf(new std::vector<double> (this->joints.size()));
       effortValQueue.push_back(vbuf);
     }
     // for reference
@@ -780,7 +786,7 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
   // populate robotState from robot
   this->robotState.header.stamp = ros::Time(_curTime.sec, _curTime.nsec);
 
-  std::shared_ptr<std::vector<double > > vbuf = effortValQueue.at(this->effort_average_cnt);
+  shared_ptr<std::vector<double > > vbuf = effortValQueue.at(this->effort_average_cnt);
   // joint states
   for (unsigned int i = 0; i < this->joints.size(); ++i) {
     this->robotState.position[i] = this->joints[i]->GetAngle(0).Radian();
@@ -810,7 +816,7 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
   }
   if (this->use_joint_effort) {
     for (int j = 0; j < effortValQueue.size(); j++) {
-      std::shared_ptr<std::vector<double > > vbuf = effortValQueue.at(j);
+      shared_ptr<std::vector<double > > vbuf = effortValQueue.at(j);
       for (int i = 0; i < this->joints.size(); i++) {
         this->robotState.effort[i] += vbuf->at(i);
       }
@@ -829,8 +835,8 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
   this->robotState.sensors.resize(this->forceSensorNames.size());
   for (unsigned int i = 0; i < this->forceSensorNames.size(); i++) {
     forceSensorMap::iterator it = this->forceSensors.find(this->forceSensorNames[i]);
-    std::shared_ptr<std::vector<std::shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue = this->forceValQueueMap.find(this->forceSensorNames[i])->second;
-    std::shared_ptr<geometry_msgs::WrenchStamped> forceVal = forceValQueue->at(this->force_sensor_average_cnt);
+    shared_ptr<std::vector<shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue = this->forceValQueueMap.find(this->forceSensorNames[i])->second;
+    shared_ptr<geometry_msgs::WrenchStamped> forceVal = forceValQueue->at(this->force_sensor_average_cnt);
     if(it != this->forceSensors.end()) {
       physics::JointPtr jt = it->second.joint;
       if (!!jt) {
@@ -869,7 +875,7 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
     this->robotState.sensors[i].torque.y = 0;
     this->robotState.sensors[i].torque.z = 0;
     for ( int j=0; j<forceValQueue->size() ; j++ ){
-      std::shared_ptr<geometry_msgs::WrenchStamped> forceValBuf = forceValQueue->at(j);
+      shared_ptr<geometry_msgs::WrenchStamped> forceValBuf = forceValQueue->at(j);
       this->robotState.sensors[i].force.x += forceValBuf->wrench.force.x;
       this->robotState.sensors[i].force.y += forceValBuf->wrench.force.y;
       this->robotState.sensors[i].force.z += forceValBuf->wrench.force.z;
@@ -898,9 +904,15 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
     if(!!sp) {
       this->robotState.Imus[i].name = this->imuSensorNames[i];
       this->robotState.Imus[i].frame_id = it->second.frame_id;
+#if __cplusplus >= 201103L
       math::Vector3 wLocal = sp->AngularVelocity();
       math::Vector3 accel = sp->LinearAcceleration();
       math::Quaternion imuRot = sp->Orientation();
+#else
+      math::Vector3 wLocal = sp->GetAngularVelocity();
+      math::Vector3 accel = sp->GetLinearAcceleration();
+      math::Quaternion imuRot = sp->GetOrientation();
+#endif
       this->robotState.Imus[i].angular_velocity.x = wLocal.x;
       this->robotState.Imus[i].angular_velocity.y = wLocal.y;
       this->robotState.Imus[i].angular_velocity.z = wLocal.z;
@@ -949,7 +961,11 @@ void IOBPlugin::UpdatePID_Velocity_Control(double _dt) {
       static_cast<double>(this->robotState.kpv_velocity[i]) * this->errorTerms[i].qd_p;
 
     // update max force
+#if __cplusplus >= 201103L
     this->joints[i]->SetParam("max_force", 0, this->joints[i]->GetEffortLimit(0));
+#else
+    this->joints[i]->SetMaxForce(0, this->joints[i]->GetEffortLimit(0));
+#endif
     // clamp max velocity
     j_velocity = math::clamp(j_velocity, -max_vel, max_vel);
 #if 0

diff --git a/hrpsys_gazebo_general/src/IOBPlugin.h b/hrpsys_gazebo_general/src/IOBPlugin.h
@@ -1,6 +1,5 @@
 #include <string>
 #include <vector>
-#include <memory>
 
 #include <boost/thread/mutex.hpp>
 
@@ -43,12 +42,19 @@
 
 #include "PubQueue.h"
 
+#if __cplusplus >= 201103L
+#include <memory>
+using std::shared_ptr;
+#else
+using boost::shared_ptr;
+#endif
+
 namespace gazebo
 {
-  typedef std::shared_ptr< sensors::ImuSensor > ImuSensorPtr;
+  typedef shared_ptr< sensors::ImuSensor > ImuSensorPtr;
   typedef hrpsys_gazebo_msgs::JointCommand JointCommand;
   typedef hrpsys_gazebo_msgs::RobotState RobotState;
-  typedef std::shared_ptr< math::Pose > PosePtr;
+  typedef shared_ptr< math::Pose > PosePtr;
 
   class IOBPlugin : public ModelPlugin
   {
@@ -198,11 +204,11 @@ namespace gazebo
     // force sensor averaging
     int force_sensor_average_window_size;
     int force_sensor_average_cnt;
-    std::map<std::string, std::shared_ptr<std::vector<std::shared_ptr<geometry_msgs::WrenchStamped> > > > forceValQueueMap;
+    std::map<std::string, shared_ptr<std::vector<shared_ptr<geometry_msgs::WrenchStamped> > > > forceValQueueMap;
     // effort averaging
     int effort_average_cnt;
     int effort_average_window_size;
-    std::vector< std::shared_ptr<std::vector<double> > > effortValQueue;
+    std::vector< shared_ptr<std::vector<double> > > effortValQueue;
     // stepping data publish cycle
     int publish_count;
     int publish_step;

diff --git a/hrpsys_gazebo_general/src/LIPPlugin.cpp b/hrpsys_gazebo_general/src/LIPPlugin.cpp
@@ -133,7 +133,11 @@ namespace gazebo
       this->root_y_joint_->SetVelocity(0, 0);
       this->linear_joint_->SetVelocity(0, 0);
       // set position and velocity
+#if __cplusplus >= 201103L
       this->model_->SetWorldPose(base_pose);
+#else
+      this->model_->SetWorldPose(base_pose, this->base_link_);
+#endif
       this->mass_link_->SetWorldPose(mass_pose);
       this->mass_link_->SetLinearVel(mass_velocity);
 
@@ -157,7 +161,11 @@ namespace gazebo
       this->root_y_joint_->SetVelocity(0, 0);
       this->linear_joint_->SetVelocity(0, 0);
       // set position and velocity
+#if __cplusplus >= 201103L
       this->model_->SetWorldPose(base_pose);
+#else
+      this->model_->SetWorldPose(base_pose, this->base_link_);
+#endif
       this->mass_link_->SetWorldPose(mass_pose);
       this->mass_link_->SetLinearVel(mass_velocity);
 

diff --git a/hrpsys_gazebo_general/src/PubQueue.h b/hrpsys_gazebo_general/src/PubQueue.h
@@ -27,6 +27,12 @@
 
 #include <ros/ros.h>
 
+#if __cplusplus >= 201103L
+using std::shared_ptr;
+#else
+using boost::shared_ptr;
+#endif
+
 
 /// \brief Container for a (ROS publisher, outgoing message) pair.  
 /// We'll have queues of these.  Templated on a ROS message type.
@@ -49,21 +55,21 @@ template<class T>
 class PubQueue
 {
   public:
-    typedef std::shared_ptr<std::deque<std::shared_ptr<
+    typedef shared_ptr<std::deque<shared_ptr<
       PubMessagePair<T> > > > QueuePtr;
-    typedef std::shared_ptr<PubQueue<T> > Ptr;
+    typedef shared_ptr<PubQueue<T> > Ptr;
 
   private:
     /// \brief Our queue of outgoing messages.
     QueuePtr queue_;
     /// \brief Mutex to control access to the queue.
-    std::shared_ptr<boost::mutex> queue_lock_;
+    shared_ptr<boost::mutex> queue_lock_;
     /// \brief Function that will be called when a new message is pushed on.
     boost::function<void()> notify_func_;
 
   public:
     PubQueue(QueuePtr queue, 
-             std::shared_ptr<boost::mutex> queue_lock,
+             shared_ptr<boost::mutex> queue_lock,
              boost::function<void()> notify_func) :
       queue_(queue), queue_lock_(queue_lock), notify_func_(notify_func) {}
     ~PubQueue() {}
@@ -73,15 +79,15 @@ class PubQueue
     /// \param[in] pub The ROS publisher to use to publish the message
     void push(T& msg, ros::Publisher& pub)
     {
-      std::shared_ptr<PubMessagePair<T> > el(new PubMessagePair<T>(msg, pub));
+      shared_ptr<PubMessagePair<T> > el(new PubMessagePair<T>(msg, pub));
       boost::mutex::scoped_lock lock(*queue_lock_);
       queue_->push_back(el);
       notify_func_();
     }
 
     /// \brief Pop all waiting messages off the queue.
     /// \param[out] els Place to store the popped messages
-    void pop(std::vector<std::shared_ptr<PubMessagePair<T> > >& els)
+    void pop(std::vector<shared_ptr<PubMessagePair<T> > >& els)
     {
       boost::mutex::scoped_lock lock(*queue_lock_);
       while(!queue_->empty())
@@ -113,11 +119,11 @@ class PubMultiQueue
     /// \brief Service a given queue by popping outgoing message off it and
     /// publishing them.
     template <class T> 
-    void serviceFunc(std::shared_ptr<PubQueue<T> > pq)
+    void serviceFunc(shared_ptr<PubQueue<T> > pq)
     {
-      std::vector<std::shared_ptr<PubMessagePair<T> > > els;
+      std::vector<shared_ptr<PubMessagePair<T> > > els;
       pq->pop(els);
-      for(typename std::vector<std::shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
+      for(typename std::vector<shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
           it != els.end();
           ++it)
       {
@@ -141,11 +147,11 @@ class PubMultiQueue
     /// least each type of publish message).
     /// \return Pointer to the newly created queue, good for calling push() on.
     template <class T>
-    std::shared_ptr<PubQueue<T> > addPub()
+    shared_ptr<PubQueue<T> > addPub()
     {
-      typename PubQueue<T>::QueuePtr queue(new std::deque<std::shared_ptr<PubMessagePair<T> > >);
-      std::shared_ptr<boost::mutex> queue_lock(new boost::mutex);
-      std::shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this)));
+      typename PubQueue<T>::QueuePtr queue(new std::deque<shared_ptr<PubMessagePair<T> > >);
+      shared_ptr<boost::mutex> queue_lock(new boost::mutex);
+      shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this)));
       boost::function<void()> f = boost::bind(&PubMultiQueue::serviceFunc<T>, this, pq);
       {
         boost::mutex::scoped_lock lock(service_funcs_lock_);

diff --git a/hrpsys_gazebo_general/src/SetVelPlugin.cpp b/hrpsys_gazebo_general/src/SetVelPlugin.cpp
@@ -120,7 +120,11 @@ namespace gazebo
       gzdbg << "subscribed SetPoseCommand. ( position: " << this->pose.pos << "  orientation: " << this->pose.rot << " )" << std::endl;
       if (!this->apply_in_gazebo_loop) {
         // this->model->SetLinkWorldPose(this->pose, this->link);
+#if __cplusplus >= 201103L
         this->model->SetWorldPose(this->pose);
+#else
+        this->model->SetWorldPose(this->pose, this->link);
+#endif
       }
     }
 
@@ -129,7 +133,11 @@ namespace gazebo
     {
       if (this->apply_in_gazebo_loop) {
         if (this->set_pose_flag) {
+#if __cplusplus >= 201103L
           this->model->SetWorldPose(this->pose);
+#else
+          this->model->SetWorldPose(this->pose, this->link);
+#endif
         }
         if (this->set_vel_flag) {
           this->model->SetLinearVel(this->linear_vel);