Support of Gazebo 7 was added.
authorDmitry Suvorov <dmitry.suvorov@moley.com>
Fri, 14 Oct 2016 15:13:35 +0000 (18:13 +0300)
committerDmitry Suvorov <dmitry.suvorov@moley.com>
Fri, 14 Oct 2016 15:13:35 +0000 (18:13 +0300)
CMakeLists.txt
src/mimic_joint_plugin.cpp

index 58b3940..79f45f4 100644 (file)
@@ -10,6 +10,10 @@ find_package(catkin REQUIRED COMPONENTS
 
 # Depend on system install of Gazebo
 find_package(gazebo REQUIRED)
+if(${GAZEBO_MAJOR_VERSION} GREATER "5")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11")
+endif()
+
 find_package(Boost REQUIRED)
 
 catkin_package(
index c5378a4..c2683bc 100644 (file)
@@ -138,8 +138,13 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
   }
   
   // Set max effort
-  if(!has_pid_)
+  if(!has_pid_) {
+#if GAZEBO_MAJOR_VERSION > 2
+    mimic_joint_->SetEffortLimit(0,max_effort_);
+#else
     mimic_joint_->SetMaxForce(0,max_effort_);
+#endif
+  }
 
   // Listen to the update event. This event is broadcast every
   // simulation iteration.
@@ -165,8 +170,13 @@ void MimicJointPlugin::UpdateChild()
       double effort = gazebo::math::clamp(pid_.computeCommand(error, period), -max_effort_, max_effort_);
       mimic_joint_->SetForce(0, effort);
     }
-    else
+    else {
+#if GAZEBO_MAJOR_VERSION > 2
+      mimic_joint_->SetPosition(0, angle );
+#else
       mimic_joint_->SetAngle(0, math::Angle(angle));
+#endif
+    }
   }
 }