Modified for kinetic(c++11)

hrpsys_gazebo_general/CMakeLists.txt

@@ -17,7 +17,7 @@ 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}")
 ## Build only gazebo iob
 pkg_check_modules(omniorb omniORB4 REQUIRED)
 pkg_check_modules(omnidynamic omniDynamic4 REQUIRED)

hrpsys_gazebo_general/src/IOBPlugin.cpp

@@ -1,6 +1,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string>
+#include <memory>
 
 #include <gazebo/transport/Node.hh>
 #include <gazebo/common/Assert.hh>
@@ -240,10 +241,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
-          boost::shared_ptr<std::vector<boost::shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue(new std::vector<boost::shared_ptr<geometry_msgs::WrenchStamped> >);
+          std::shared_ptr<std::vector<std::shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue(new std::vector<std::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++ ){
-            boost::shared_ptr<geometry_msgs::WrenchStamped> fbuf(new geometry_msgs::WrenchStamped);
+            std::shared_ptr<geometry_msgs::WrenchStamped> fbuf(new geometry_msgs::WrenchStamped);
             forceValQueue->push_back(fbuf);
           }
           this->forceValQueueMap[sensor_name] = forceValQueue;
@@ -275,7 +276,7 @@ void IOBPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
               gzerr << ln << " not found\n";
             } else {
               // Get imu sensors
-              msi.sensor = boost::dynamic_pointer_cast<sensors::ImuSensor>
+              msi.sensor = std::dynamic_pointer_cast<sensors::ImuSensor>
                 (sensors::SensorManager::Instance()->GetSensor
                  (this->world->GetName() + "::" + msi.link->GetScopedName() + "::" + msi.sensor_name));
 
@@ -377,7 +378,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++) {
-      boost::shared_ptr<std::vector<double> > vbuf(new std::vector<double> (this->joints.size()));
+      std::shared_ptr<std::vector<double> > vbuf(new std::vector<double> (this->joints.size()));
       effortValQueue.push_back(vbuf);
     }
     // for reference
@@ -779,7 +780,7 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
   // populate robotState from robot
   this->robotState.header.stamp = ros::Time(_curTime.sec, _curTime.nsec);
 
-  boost::shared_ptr<std::vector<double > > vbuf = effortValQueue.at(this->effort_average_cnt);
+  std::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();
@@ -809,7 +810,7 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
   }
   if (this->use_joint_effort) {
     for (int j = 0; j < effortValQueue.size(); j++) {
-      boost::shared_ptr<std::vector<double > > vbuf = effortValQueue.at(j);
+      std::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);
       }
@@ -828,8 +829,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]);
-    boost::shared_ptr<std::vector<boost::shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue = this->forceValQueueMap.find(this->forceSensorNames[i])->second;
-    boost::shared_ptr<geometry_msgs::WrenchStamped> forceVal = forceValQueue->at(this->force_sensor_average_cnt);
+    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);
     if(it != this->forceSensors.end()) {
       physics::JointPtr jt = it->second.joint;
       if (!!jt) {
@@ -868,7 +869,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++ ){
-      boost::shared_ptr<geometry_msgs::WrenchStamped> forceValBuf = forceValQueue->at(j);
+      std::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;
@@ -897,9 +898,9 @@ 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;
-      math::Vector3 wLocal = sp->GetAngularVelocity();
-      math::Vector3 accel = sp->GetLinearAcceleration();
-      math::Quaternion imuRot = sp->GetOrientation();
+      math::Vector3 wLocal = sp->AngularVelocity();
+      math::Vector3 accel = sp->LinearAcceleration();
+      math::Quaternion imuRot = sp->Orientation();
       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;
@@ -948,7 +949,7 @@ void IOBPlugin::UpdatePID_Velocity_Control(double _dt) {
       static_cast<double>(this->robotState.kpv_velocity[i]) * this->errorTerms[i].qd_p;
 
     // update max force
-    this->joints[i]->SetMaxForce(0, this->joints[i]->GetEffortLimit(0));
+    this->joints[i]->SetParam("max_force", 0, this->joints[i]->GetEffortLimit(0));
     // clamp max velocity
     j_velocity = math::clamp(j_velocity, -max_vel, max_vel);
 #if 0

hrpsys_gazebo_general/src/IOBPlugin.h

@@ -1,5 +1,6 @@
 #include <string>
 #include <vector>
+#include <memory>
 
 #include <boost/thread/mutex.hpp>
 
@@ -44,10 +45,10 @@
 
 namespace gazebo
 {
-  typedef boost::shared_ptr< sensors::ImuSensor > ImuSensorPtr;
+  typedef std::shared_ptr< sensors::ImuSensor > ImuSensorPtr;
   typedef hrpsys_gazebo_msgs::JointCommand JointCommand;
   typedef hrpsys_gazebo_msgs::RobotState RobotState;
-  typedef boost::shared_ptr< math::Pose > PosePtr;
+  typedef std::shared_ptr< math::Pose > PosePtr;
 
   class IOBPlugin : public ModelPlugin
   {
@@ -197,11 +198,11 @@ namespace gazebo
     // force sensor averaging
     int force_sensor_average_window_size;
     int force_sensor_average_cnt;
-    std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<geometry_msgs::WrenchStamped> > > > forceValQueueMap;
+    std::map<std::string, std::shared_ptr<std::vector<std::shared_ptr<geometry_msgs::WrenchStamped> > > > forceValQueueMap;
     // effort averaging
     int effort_average_cnt;
     int effort_average_window_size;
-    std::vector< boost::shared_ptr<std::vector<double> > > effortValQueue;
+    std::vector< std::shared_ptr<std::vector<double> > > effortValQueue;
     // stepping data publish cycle
     int publish_count;
     int publish_step;

hrpsys_gazebo_general/src/LIPPlugin.cpp

@@ -133,7 +133,7 @@ namespace gazebo
       this->root_y_joint_->SetVelocity(0, 0);
       this->linear_joint_->SetVelocity(0, 0);
       // set position and velocity
-      this->model_->SetWorldPose(base_pose, this->base_link_);
+      this->model_->SetWorldPose(base_pose);
       this->mass_link_->SetWorldPose(mass_pose);
       this->mass_link_->SetLinearVel(mass_velocity);
 
@@ -157,7 +157,7 @@ namespace gazebo
       this->root_y_joint_->SetVelocity(0, 0);
       this->linear_joint_->SetVelocity(0, 0);
       // set position and velocity
-      this->model_->SetWorldPose(base_pose, this->base_link_);
+      this->model_->SetWorldPose(base_pose);
       this->mass_link_->SetWorldPose(mass_pose);
       this->mass_link_->SetLinearVel(mass_velocity);
 

hrpsys_gazebo_general/src/PubQueue.h

@@ -49,21 +49,21 @@ template<class T>
 class PubQueue
 {
   public:
-    typedef boost::shared_ptr<std::deque<boost::shared_ptr<
+    typedef std::shared_ptr<std::deque<std::shared_ptr<
       PubMessagePair<T> > > > QueuePtr;
-    typedef boost::shared_ptr<PubQueue<T> > Ptr;
+    typedef std::shared_ptr<PubQueue<T> > Ptr;
 
   private:
     /// \brief Our queue of outgoing messages.
     QueuePtr queue_;
     /// \brief Mutex to control access to the queue.
-    boost::shared_ptr<boost::mutex> queue_lock_;
+    std::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, 
-             boost::shared_ptr<boost::mutex> queue_lock,
+             std::shared_ptr<boost::mutex> queue_lock,
              boost::function<void()> notify_func) :
       queue_(queue), queue_lock_(queue_lock), notify_func_(notify_func) {}
     ~PubQueue() {}
@@ -73,15 +73,15 @@ class PubQueue
     /// \param[in] pub The ROS publisher to use to publish the message
     void push(T& msg, ros::Publisher& pub)
     {
-      boost::shared_ptr<PubMessagePair<T> > el(new PubMessagePair<T>(msg, pub));
+      std::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<boost::shared_ptr<PubMessagePair<T> > >& els)
+    void pop(std::vector<std::shared_ptr<PubMessagePair<T> > >& els)
     {
       boost::mutex::scoped_lock lock(*queue_lock_);
       while(!queue_->empty())
@@ -113,11 +113,11 @@ class PubMultiQueue
     /// \brief Service a given queue by popping outgoing message off it and
     /// publishing them.
     template <class T> 
-    void serviceFunc(boost::shared_ptr<PubQueue<T> > pq)
+    void serviceFunc(std::shared_ptr<PubQueue<T> > pq)
     {
-      std::vector<boost::shared_ptr<PubMessagePair<T> > > els;
+      std::vector<std::shared_ptr<PubMessagePair<T> > > els;
       pq->pop(els);
-      for(typename std::vector<boost::shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
+      for(typename std::vector<std::shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
           it != els.end();
           ++it)
       {
@@ -141,11 +141,11 @@ class PubMultiQueue
     /// least each type of publish message).
     /// \return Pointer to the newly created queue, good for calling push() on.
     template <class T>
-    boost::shared_ptr<PubQueue<T> > addPub()
+    std::shared_ptr<PubQueue<T> > addPub()
     {
-      typename PubQueue<T>::QueuePtr queue(new std::deque<boost::shared_ptr<PubMessagePair<T> > >);
-      boost::shared_ptr<boost::mutex> queue_lock(new boost::mutex);
-      boost::shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this)));
+      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)));
       boost::function<void()> f = boost::bind(&PubMultiQueue::serviceFunc<T>, this, pq);
       {
         boost::mutex::scoped_lock lock(service_funcs_lock_);

hrpsys_gazebo_general/src/SetVelPlugin.cpp

@@ -120,7 +120,7 @@ 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);
-        this->model->SetWorldPose(this->pose, this->link);
+        this->model->SetWorldPose(this->pose);
       }
     }
 
@@ -129,7 +129,7 @@ namespace gazebo
     {
       if (this->apply_in_gazebo_loop) {
         if (this->set_pose_flag) {
-          this->model->SetWorldPose(this->pose, this->link);
+          this->model->SetWorldPose(this->pose);
         }
         if (this->set_vel_flag) {
           this->model->SetLinearVel(this->linear_vel);