Port to Galactic.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1b69e4a..0511a27 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,51 +1,77 @@
-cmake_minimum_required(VERSION 3.1.0)
+cmake_minimum_required(VERSION 3.8)

 project(roboticsgroup_upatras_gazebo_plugins)
 
 project(roboticsgroup_upatras_gazebo_plugins)
 

-find_package(catkin REQUIRED
-  COMPONENTS
-    control_toolbox
-    gazebo_ros
-    roscpp
-)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()

 
 

-find_package(gazebo REQUIRED)
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(control_toolbox REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(gazebo_ros REQUIRED)

 
 

-catkin_package(
-  INCLUDE_DIRS
-    include
-  LIBRARIES
-    roboticsgroup_upatras_gazebo_mimic_joint_plugin
-    roboticsgroup_upatras_gazebo_disable_link_plugin
-  CATKIN_DEPENDS
-    control_toolbox
-    gazebo_ros
-    roscpp
+add_library(roboticsgroup_upatras_gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp)
+target_compile_features(roboticsgroup_upatras_gazebo_mimic_joint_plugin PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_include_directories(roboticsgroup_upatras_gazebo_mimic_joint_plugin PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(
+  roboticsgroup_upatras_gazebo_mimic_joint_plugin
+  "rclcpp"
+  "control_toolbox"
+  "control_msgs"
+  "gazebo_ros"

 )
 
 )
 

-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-  ${GAZEBO_INCLUDE_DIRS}
-)
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(roboticsgroup_upatras_gazebo_mimic_joint_plugin PRIVATE "ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_BUILDING_LIBRARY")

 
 

-link_directories(${GAZEBO_LIBRARY_DIRS})
+add_library(roboticsgroup_upatras_gazebo_disable_link_plugin src/disable_link_plugin.cpp)
+target_compile_features(roboticsgroup_upatras_gazebo_disable_link_plugin PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_include_directories(roboticsgroup_upatras_gazebo_disable_link_plugin PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(
+  roboticsgroup_upatras_gazebo_disable_link_plugin
+  "rclcpp"
+  "control_toolbox"
+  "gazebo_ros"
+)

 
 

-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(roboticsgroup_upatras_gazebo_disable_link_plugin PRIVATE "ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_BUILDING_LIBRARY")

 
 

-add_library(roboticsgroup_upatras_gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp)
-target_link_libraries(roboticsgroup_upatras_gazebo_mimic_joint_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+install(
+  TARGETS
+  roboticsgroup_upatras_gazebo_mimic_joint_plugin
+  roboticsgroup_upatras_gazebo_disable_link_plugin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)

 
 

-add_library(roboticsgroup_upatras_gazebo_disable_link_plugin src/disable_link_plugin.cpp)
-target_link_libraries(roboticsgroup_upatras_gazebo_disable_link_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()

 
 

-install(TARGETS
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(

     roboticsgroup_upatras_gazebo_mimic_joint_plugin
     roboticsgroup_upatras_gazebo_disable_link_plugin
     roboticsgroup_upatras_gazebo_mimic_joint_plugin
     roboticsgroup_upatras_gazebo_disable_link_plugin

-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

 )
 
 )
 

-install(DIRECTORY include/${PROJECT_NAME}
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
+ament_package()

diff --git a/include/roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h b/include/roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h
index da9bf05..2102ff3 100644 (file)
--- a/include/roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h
+++ b/include/roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h
@@ -24,19 +24,26 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
 #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN
 
 // ROS includes
 #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN
 
 // ROS includes

-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>

 
 // Gazebo includes
 #include <gazebo/common/Plugin.hh>
 #include <gazebo/physics/physics.hh>
 
 
 // Gazebo includes
 #include <gazebo/common/Plugin.hh>
 #include <gazebo/physics/physics.hh>
 

+// Visibility control
+#include <roboticsgroup_upatras_gazebo_plugins/visibility_control.h>
+

 namespace gazebo {
 
     class DisableLinkPlugin : public ModelPlugin {
       public:
 namespace gazebo {
 
     class DisableLinkPlugin : public ModelPlugin {
       public:

+        ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC

         DisableLinkPlugin();
         DisableLinkPlugin();

+
+        ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC

         virtual ~DisableLinkPlugin() override;
 
         virtual ~DisableLinkPlugin() override;
 

+        ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC

         virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) override;
 
       private:
         virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) override;
 
       private:

diff --git a/include/roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h b/include/roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h
index 86a8da6..ce90354 100644 (file)
--- a/include/roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h
+++ b/include/roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h
@@ -24,22 +24,29 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
 #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN
 
 // ROS includes
 #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN
 
 // ROS includes

-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>

 
 // ros_control
 
 // ros_control

-#include <control_toolbox/pid.h>
+#include <control_toolbox/pid_ros.hpp>

 
 // Gazebo includes
 #include <gazebo/common/Plugin.hh>
 #include <gazebo/physics/physics.hh>
 
 
 // Gazebo includes
 #include <gazebo/common/Plugin.hh>
 #include <gazebo/physics/physics.hh>
 

+// Visibility control
+#include <roboticsgroup_upatras_gazebo_plugins/visibility_control.h>
+

 namespace gazebo {
 
     class MimicJointPlugin : public ModelPlugin {
       public:
 namespace gazebo {
 
     class MimicJointPlugin : public ModelPlugin {
       public:

+        ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC

         MimicJointPlugin();
         MimicJointPlugin();

+
+        ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC

         virtual ~MimicJointPlugin() override;
 
         virtual ~MimicJointPlugin() override;
 

+        ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC

         virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) override;
 
       private:
         virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) override;
 
       private:


@@ -51,7 +58,7 @@ namespace gazebo {
         bool has_pid_;
 
         // PID controller if needed
         bool has_pid_;
 
         // PID controller if needed

-        control_toolbox::Pid pid_;
+        std::unique_ptr<control_toolbox::PidROS> pid_;

 
         // Pointers to the joints
         physics::JointPtr joint_, mimic_joint_;
 
         // Pointers to the joints
         physics::JointPtr joint_, mimic_joint_;

diff --git a/include/roboticsgroup_upatras_gazebo_plugins/visibility_control.h b/include/roboticsgroup_upatras_gazebo_plugins/visibility_control.h
new file mode 100644 (file)
index 0000000..b3c82f2
--- /dev/null
+++ b/include/roboticsgroup_upatras_gazebo_plugins/visibility_control.h
@@ -0,0 +1,35 @@
+#ifndef ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS__VISIBILITY_CONTROL_H_
+#define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_EXPORT __attribute__ ((dllexport))
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_IMPORT __attribute__ ((dllimport))
+  #else
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_EXPORT __declspec(dllexport)
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_BUILDING_LIBRARY
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_EXPORT
+  #else
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_IMPORT
+  #endif
+  #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC_TYPE ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC
+  #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_LOCAL
+#else
+  #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_EXPORT __attribute__ ((visibility("default")))
+  #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_IMPORT
+  #if __GNUC__ >= 4
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC __attribute__ ((visibility("default")))
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC
+    #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_LOCAL
+  #endif
+  #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC_TYPE
+#endif
+
+#endif  // ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS__VISIBILITY_CONTROL_H_

diff --git a/package.xml b/package.xml
index bf45f47..fe30969 100644 (file)
--- a/package.xml
+++ b/package.xml
@@ -1,4 +1,5 @@
 <?xml version="1.0"?>
 <?xml version="1.0"?>

+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>

 <!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
 All rights reserved.
 
 <!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
 All rights reserved.
 


@@ -18,9 +19,9 @@ SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, I
 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -->
 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -->

-<package format="2">>
+<package format="3">

   <name>roboticsgroup_upatras_gazebo_plugins</name>
   <name>roboticsgroup_upatras_gazebo_plugins</name>

-  <version>0.2.0</version>
+  <version>0.2.1</version>

   <description>Collection of gazebo plugins</description>
 
   <author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
   <description>Collection of gazebo plugins</description>
 
   <author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>


@@ -32,18 +33,17 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
   <url type="repository">https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins</url>
   <url type="bugtracker">https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins/issues</url>
 
   <url type="repository">https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins</url>
   <url type="bugtracker">https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins/issues</url>
 

-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>control_toolbox</build_depend>
-  <build_depend>gazebo_ros</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_export_depend>control_toolbox</build_export_depend>
-  <build_export_depend>gazebo_ros</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <exec_depend>control_toolbox</exec_depend>
-  <exec_depend>gazebo_ros</exec_depend>
-  <exec_depend>roscpp</exec_depend>
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>control_toolbox</depend>
+  <depend>control_msgs</depend>
+  <depend>gazebo_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>

 
   <export>
 
   <export>

-    <gazebo_ros plugin_path="${prefix}/lib" />
+    <build_type>ament_cmake</build_type>

   </export>
 </package>
   </export>
 </package>

diff --git a/src/disable_link_plugin.cpp b/src/disable_link_plugin.cpp
index 5981846..b742f4b 100644 (file)
--- a/src/disable_link_plugin.cpp
+++ b/src/disable_link_plugin.cpp
@@ -20,6 +20,7 @@ INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT
 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **/
 
 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **/
 

+#include <rclcpp/logging.hpp>

 #include <roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h>
 
 namespace gazebo {
 #include <roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h>
 
 namespace gazebo {


@@ -40,7 +41,7 @@ namespace gazebo {
 
         // Check for link element
         if (!_sdf->HasElement("link")) {
 
         // Check for link element
         if (!_sdf->HasElement("link")) {

-            ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded.");
+            RCLCPP_ERROR(rclcpp::get_logger("disable_link_plugin"),"No link element present. DisableLinkPlugin could not be loaded.");

             return;
         }
 
             return;
         }
 


@@ -51,12 +52,12 @@ namespace gazebo {
         if (link_) {
             link_->SetEnabled(false);
             // Output some confirmation
         if (link_) {
             link_->SetEnabled(false);
             // Output some confirmation

-            ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: " << link_name_);
+            RCLCPP_INFO_STREAM(rclcpp::get_logger("disable_link_plugin"),"DisableLinkPlugin loaded! Link: " << link_name_);

         }
         else
         }
         else

-            ROS_ERROR_STREAM("Link " << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
+            RCLCPP_ERROR_STREAM(rclcpp::get_logger("disable_link_plugin"),"Link " << link_name_ << " not found! DisableLinkPlugin could not be loaded.");

     }
 
     }
 

-    GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin);
+    GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin)

 
 }  // namespace gazebo
 
 }  // namespace gazebo

diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp
index 48b588b..70f8c58 100644 (file)
--- a/src/mimic_joint_plugin.cpp
+++ b/src/mimic_joint_plugin.cpp
@@ -20,6 +20,13 @@ INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT
 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **/
 
 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **/
 

+#include <chrono>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logging.hpp>
+
+#include <gazebo_ros/node.hpp>
+

 #include <roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h>
 
 #if GAZEBO_MAJOR_VERSION >= 8
 #include <roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h>
 
 #if GAZEBO_MAJOR_VERSION >= 8


@@ -48,13 +55,13 @@ namespace gazebo {
 
         // Error message if the model couldn't be found
         if (!model_) {
 
         // Error message if the model couldn't be found
         if (!model_) {

-            ROS_ERROR("Parent model is NULL! MimicJointPlugin could not be loaded.");
+            RCLCPP_ERROR(rclcpp::get_logger("mimic_joint_plugin"),"Parent model is NULL! MimicJointPlugin could not be loaded.");

             return;
         }
 
         // Check that ROS has been initialized
             return;
         }
 
         // Check that ROS has been initialized

-        if (!ros::isInitialized()) {
-            ROS_ERROR("A ROS node for Gazebo has not been initialized, unable to load plugin.");
+        if (!rclcpp::ok()) {
+            RCLCPP_ERROR(rclcpp::get_logger("mimic_joint_plugin"),"A ROS node for Gazebo has not been initialized, unable to load plugin.");

             return;
         }
 
             return;
         }
 


@@ -62,11 +69,11 @@ namespace gazebo {
         if (_sdf->HasElement("robotNamespace")) {
             robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
         }
         if (_sdf->HasElement("robotNamespace")) {
             robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
         }

-        ros::NodeHandle model_nh(robot_namespace_);
+        rclcpp::Node::SharedPtr model_nh=gazebo_ros::Node::Get(_sdf);

 
         // Check for joint element
         if (!_sdf->HasElement("joint")) {
 
         // Check for joint element
         if (!_sdf->HasElement("joint")) {

-            ROS_ERROR("No joint element present. MimicJointPlugin could not be loaded.");
+            RCLCPP_ERROR(model_nh->get_logger(),"No joint element present. MimicJointPlugin could not be loaded.");

             return;
         }
 
             return;
         }
 


@@ -74,7 +81,7 @@ namespace gazebo {
 
         // Check for mimicJoint element
         if (!_sdf->HasElement("mimicJoint")) {
 
         // Check for mimicJoint element
         if (!_sdf->HasElement("mimicJoint")) {

-            ROS_ERROR("No mimicJoint element present. MimicJointPlugin could not be loaded.");
+            RCLCPP_ERROR(model_nh->get_logger(),"No mimicJoint element present. MimicJointPlugin could not be loaded.");

             return;
         }
 
             return;
         }
 


@@ -87,8 +94,9 @@ namespace gazebo {
             if (name.empty()) {
                 name = "gazebo_ros_control/pid_gains/" + mimic_joint_name_;
             }
             if (name.empty()) {
                 name = "gazebo_ros_control/pid_gains/" + mimic_joint_name_;
             }

-            const ros::NodeHandle nh(model_nh, name);
-            pid_.init(nh);
+            rclcpp::Node::SharedPtr nh=std::make_shared<rclcpp::Node>(name,model_nh->get_namespace());
+            pid_=std::make_unique<control_toolbox::PidROS>(nh);
+            pid_->initPid();

         }
 
         // Check for multiplier element
         }
 
         // Check for multiplier element


@@ -109,12 +117,12 @@ namespace gazebo {
         // Get pointers to joints
         joint_ = model_->GetJoint(joint_name_);
         if (!joint_) {
         // Get pointers to joints
         joint_ = model_->GetJoint(joint_name_);
         if (!joint_) {

-            ROS_ERROR_STREAM("No joint named \"" << joint_name_ << "\". MimicJointPlugin could not be loaded.");
+            RCLCPP_ERROR_STREAM(model_nh->get_logger(),"No joint named \"" << joint_name_ << "\". MimicJointPlugin could not be loaded.");

             return;
         }
         mimic_joint_ = model_->GetJoint(mimic_joint_name_);
         if (!mimic_joint_) {
             return;
         }
         mimic_joint_ = model_->GetJoint(mimic_joint_name_);
         if (!mimic_joint_) {

-            ROS_ERROR_STREAM("No (mimic) joint named \"" << mimic_joint_name_ << "\". MimicJointPlugin could not be loaded.");
+            RCLCPP_ERROR_STREAM(model_nh->get_logger(),"No (mimic) joint named \"" << mimic_joint_name_ << "\". MimicJointPlugin could not be loaded.");

             return;
         }
 
             return;
         }
 


@@ -143,17 +151,18 @@ namespace gazebo {
             boost::bind(&MimicJointPlugin::UpdateChild, this));
 
         // Output some confirmation
             boost::bind(&MimicJointPlugin::UpdateChild, this));
 
         // Output some confirmation

-        ROS_INFO_STREAM("MimicJointPlugin loaded! Joint: \"" << joint_name_ << "\", Mimic joint: \"" << mimic_joint_name_ << "\""
+        RCLCPP_INFO_STREAM(model_nh->get_logger(),"MimicJointPlugin loaded! Joint: \"" << joint_name_ << "\", Mimic joint: \"" << mimic_joint_name_ << "\""

                                                              << ", Multiplier: " << multiplier_ << ", Offset: " << offset_
                                                              << ", MaxEffort: " << max_effort_ << ", Sensitiveness: " << sensitiveness_);
     }
 
     void MimicJointPlugin::UpdateChild()
     {
                                                              << ", Multiplier: " << multiplier_ << ", Offset: " << offset_
                                                              << ", MaxEffort: " << max_effort_ << ", Sensitiveness: " << sensitiveness_);
     }
 
     void MimicJointPlugin::UpdateChild()
     {

+        using namespace std::chrono_literals;

 #if GAZEBO_MAJOR_VERSION >= 8
 #if GAZEBO_MAJOR_VERSION >= 8

-        static ros::Duration period(world_->Physics()->GetMaxStepSize());
+        static rclcpp::Duration period(world_->Physics()->GetMaxStepSize()*1s);

 #else
 #else

-        static ros::Duration period(world_->GetPhysicsEngine()->GetMaxStepSize());
+        static rclcpp::Duration period(world_->GetPhysicsEngine()->GetMaxStepSize()*1s);

 #endif
 
         // Set mimic joint's angle based on joint's angle
 #endif
 
         // Set mimic joint's angle based on joint's angle


@@ -170,17 +179,17 @@ namespace gazebo {
                 if (a != a)
                     a = angle;
                 double error = angle - a;
                 if (a != a)
                     a = angle;
                 double error = angle - a;

-                double effort = math::clamp(pid_.computeCommand(error, period), -max_effort_, max_effort_);
+                double effort = math::clamp(pid_->computeCommand(error, period), -max_effort_, max_effort_);

                 mimic_joint_->SetForce(0, effort);
             }
             else {
 #if GAZEBO_MAJOR_VERSION >= 9
                 mimic_joint_->SetPosition(0, angle, true);
 #elif GAZEBO_MAJOR_VERSION > 2
                 mimic_joint_->SetForce(0, effort);
             }
             else {
 #if GAZEBO_MAJOR_VERSION >= 9
                 mimic_joint_->SetPosition(0, angle, true);
 #elif GAZEBO_MAJOR_VERSION > 2

-                ROS_WARN_ONCE("The mimic_joint plugin is using the Joint::SetPosition method without preserving the link velocity.");
-                ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model.");
-                ROS_WARN_ONCE("Please set gazebo_pid parameters or upgrade to Gazebo 9.");
-                ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612");
+                RCLCPP_WARN_ONCE(model_nh->get_logger(),"The mimic_joint plugin is using the Joint::SetPosition method without preserving the link velocity.");
+                RCLCPP_WARN_ONCE(model_nh->get_logger(),"As a result, gravity will not be simulated correctly for your model.");
+                RCLCPP_WARN_ONCE(model_nh->get_logger(),"Please set gazebo_pid parameters or upgrade to Gazebo 9.");
+                RCLCPP_WARN_ONCE(model_nh->get_logger(),"For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612");

                 mimic_joint_->SetPosition(0, angle);
 #else
                 mimic_joint_->SetAngle(0, math::Angle(angle));
                 mimic_joint_->SetPosition(0, angle);
 #else
                 mimic_joint_->SetAngle(0, math::Angle(angle));


@@ -189,6 +198,6 @@ namespace gazebo {
         }
     }
 
         }
     }
 

-    GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin);
+    GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin)

 
 }  // namespace gazebo
 
 }  // namespace gazebo