Updated with ros2-control Foxy API (#44)

* Updated with ros2-control foxy API

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Fixed dependency

Signed-off-by: ahcorde <ahcorde@gmail.com>

* make compile on osx (#46)

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>

* Fixed velocity example

Signed-off-by: ahcorde <ahcorde@gmail.com>

* make linters happy

Signed-off-by: ahcorde <ahcorde@gmail.com>

* updated Dockerfile

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Added effort example

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Instructions to run with moveit2

Signed-off-by: ahcorde <ahcorde@gmail.com>

* updated readme

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Added PIMPL pattern and other fixes

Signed-off-by: ahcorde <ahcorde@gmail.com>

* updated Dockerfile

Signed-off-by: ahcorde <ahcorde@gmail.com>

* changed log level

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Fixed gazebo_period units

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Updated CI

Signed-off-by: ahcorde <ahcorde@gmail.com>

* updated CI

Signed-off-by: ahcorde <ahcorde@gmail.com>

* updated CI

Signed-off-by: ahcorde <ahcorde@gmail.com>

* update CI

Signed-off-by: ahcorde <ahcorde@gmail.com>

* updated Dockerfile

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Added feedback

Signed-off-by: ahcorde <ahcorde@gmail.com>

* make linters happy

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Removed PID controllers

Signed-off-by: ahcorde <ahcorde@gmail.com>

* make linters happy

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Removed transmission interface

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Removed e-stop

Signed-off-by: ahcorde <ahcorde@gmail.com>

* make linters happy

Signed-off-by: ahcorde <ahcorde@gmail.com>

* clean code

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Fix CI

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Changed log level of a trace

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Added rclcpp dependency to gazebo_hardware_plugin

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Fixed bug in control method

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Support more than one ros2_control tag

Signed-off-by: ahcorde <ahcorde@gmail.com>

* make linters happy

Signed-off-by: ahcorde <ahcorde@gmail.com>

* unify yaml files

Signed-off-by: ahcorde <ahcorde@gmail.com>

Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>

Author: ahcorde

.github/workflows/ci.yaml

@@ -18,16 +18,13 @@ jobs:
         cd ..
         mkdir -p /home/ros2_ws/src
         cp -r gazebo_ros2_control /home/ros2_ws/src/
-        apt-get upgrade -q -y
+        apt-get update && apt-get upgrade -q -y
         apt-get update && apt-get install -q -y --no-install-recommends \
           dirmngr \
           gnupg2 \
           lsb-release \
           python3-colcon-ros
-        apt-get upgrade -y
         cd /home/ros2_ws/src/
-        git clone https://github.com/ros-controls/ros2_control
-        git clone https://github.com/ros-controls/ros2_controllers
         rosdep update
         rosdep install --from-paths ./ -i -y --rosdistro foxy \
           --ignore-src
@@ -36,7 +33,6 @@ jobs:
       run: |
         cd /home/ros2_ws/
         . /opt/ros/foxy/local_setup.sh
-        export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH
         colcon build --packages-up-to gazebo_ros2_control_demos
     - name: Run tests
       id: test

Docker/Dockerfile

@@ -15,16 +15,13 @@ RUN apt-get update && apt-get install -q -y --no-install-recommends \
 RUN mkdir -p /home/ros2_ws/src \
     && cd /home/ros2_ws/src/ \
     && git clone https://github.com/ros-simulation/gazebo_ros2_control \
-    && git clone https://github.com/ros-controls/ros2_control \
-    && git clone https://github.com/ros-controls/ros2_controllers \
     && rosdep update \
     && rosdep install --from-paths ./ -i -y --rosdistro foxy \
       --ignore-src
 
 RUN cd /home/ros2_ws/ \
   && . /opt/ros/foxy/setup.sh \
-  && export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH \
-  && colcon build
+  && colcon build --merge-install
 
 COPY entrypoint.sh /entrypoint.sh
 ENTRYPOINT ["/entrypoint.sh"]

README.md

@@ -1,6 +1,5 @@
 # gazebo_ros2_control
 
-
 This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Gazebo](http://gazebosim.org/) simulator.
 
 This package provides a Gazebo plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model.
@@ -13,7 +12,7 @@ It is running Gazebo and some other ROS 2 nodes.
 
 ## Video + Pictures
 
-![](img/gazebo_ros2_control_position_pid.gif)
+![](img/gazebo_ros2_control_position.gif)
 
 ## Running
 
@@ -61,47 +60,59 @@ ros2 run gazebo_ros2_control_demos example_position
 ```
 
 
-## Add transmission elements to a URDF
+## Add ros2_control tag to a URDF
 
-To use `ros2_control` with your robot, you need to add some additional elements to your URDF. The `<transmission>` element is used to link actuators to joints, see the `<transmission>` spec for exact XML format.
+To use `ros2_control` with your robot, you need to add some additional elements to your URDF.
+You should include the tag `<ros2_control>` to access and control the robot interfaces. We should
+include
 
-For the purposes of `gazebo_ros2_control` in its current implementation, the only important information in these transmission tags are:
+ - a specific `<plugin>` for our robot
+ - `<joint>` tag including the robot controllers: commands and states.
 
- - `<joint name="">` the name must correspond to a joint else where in your URDF
- - `<type>` the type of transmission. Currently only `transmission_interface/SimpleTransmission` is implemented.
- - `<hardwareInterface>` within the `<actuator>` and `<joint>` tags, this tells the `gazebo_ros2_control` plugin what hardware interface to load (position, velocity or effort interfaces).
+```xml
+<ros2_control name="GazeboSystem" type="system">
+  <hardware>
+    <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+  </hardware>
+  <joint name="slider_to_cart">
+    <command_interface name="effort">
+      <param name="min">-1000</param>
+      <param name="max">1000</param>
+    </command_interface>
+    <state_interface name="position"/>
+    <state_interface name="velocity"/>
+    <state_interface name="effort"/>
+  </joint>
+</ros2_control>
+```
 
 ## Add the gazebo_ros2_control plugin
 
-In addition to the transmission tags, a Gazebo plugin needs to be added to your URDF that
-actually parses the transmission tags and loads the appropriate hardware interfaces and
+In addition to the `ros2_control` tags, a Gazebo plugin needs to be added to your URDF that
+actually parses the `ros2_control` tags and loads the appropriate hardware interfaces and
 controller manager. By default the `gazebo_ros2_control` plugin is very simple, though it is also
 extensible via an additional plugin architecture to allow power users to create their own custom
 robot hardware interfaces between `ros2_control` and Gazebo.
 
 ```xml
 <gazebo>
     <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-      <robot_sim_type>gazebo_ros2_control/DefaultRobotHWSim</robot_sim_type>
       <robot_param>robot_description</robot_param>
       <robot_param_node>robot_state_publisher</robot_param_node>
       <parameters>$(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml</parameters>
-      <e_stop_topic>False</e_stop_topic>
     </plugin>
 </gazebo>
 ```
 
 The `gazebo_ros2_control` `<plugin>` tag also has the following optional child elements:
 
- - `<control_period>`: The period of the controller update (in seconds), defaults to Gazebo's period
  - `<robot_param>`: The location of the `robot_description` (URDF) on the parameter server, defaults to `robot_description`
  - `<robot_param_node>`: Name of the node where the `robot_param` is located, defauls to `robot_state_publisher`
- - `<robot_sim_type>`: The pluginlib name of a custom robot sim interface to be used, defaults to `gazebo_ros2_control/DefaultRobotHWSim`
  - `<parameters>`: YAML file with the configuration of the controllers
 
 #### Default gazebo_ros2_control Behavior
 
-By default, without a `<robot_sim_type>` tag, `gazebo_ros2_control` will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started.
+By default, without a `<plugin>` tag, `gazebo_ros2_control` will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started.
 
 The default behavior provides the following ros2_control interfaces:
 
@@ -113,14 +124,21 @@ The default behavior provides the following ros2_control interfaces:
 
 The `gazebo_ros2_control` Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between Gazebo and `ros2_control` for simulating more complex mechanisms (nonlinear springs, linkages, etc).
 
-These plugins must inherit `gazebo_ros2_control::RobotHWSim` which implements a simulated `ros2_control` `hardware_interface::RobotHW`. RobotHWSim provides API-level access to read and command joint properties in the Gazebo simulator.
-
-The respective RobotHWSim sub-class is specified in a URDF model and is loaded when the robot model is loaded. For example, the following XML will load the default plugin (same behavior as when using no `<robot_sim_type>` tag):
+These plugins must inherit `gazebo_ros2_control::GazeboSystemInterface` which implements a simulated `ros2_control`
+`hardware_interface::SystemInterface`. SystemInterface provides API-level access to read and command joint properties.
 
+The respective GazeboSystemInterface sub-class is specified in a URDF model and is loaded when the
+robot model is loaded. For example, the following XML will load the default plugin:
 ```xml
+<ros2_control name="GazeboSystem" type="system">
+  <hardware>
+    <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+  </hardware>
+  ...
+<ros2_control>
 <gazebo>
   <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
-    <robot_sim_type>gazebo_ros2_control/DefaultRobotHWSim</robot_sim_type>
+    ...
   </plugin>
 </gazebo>
 ```
@@ -138,13 +156,13 @@ Use the tag `<parameters>` inside `<plugin>` to set the YAML file with the contr
 ```
 
 This controller publishes the state of all resources registered to a
-`hardware_interface::JointStateInterface` to a topic of type `sensor_msgs/msg/JointState`. The following is a basic configuration of the controller.
+`hardware_interface::StateInterface` to a topic of type `sensor_msgs/msg/JointState`.
+The following is a basic configuration of the controller.
 
 ```yaml
 joint_state_controller:
   ros__parameters:
     type: joint_state_controller/JointStateController
-    publish_rate: 50
 ```
 
 This controller creates an action called `/cart_pole_controller/follow_joint_trajectory` of type `control_msgs::action::FollowJointTrajectory`.
@@ -157,37 +175,7 @@ cart_pole_controller:
        - slider_to_cart
     write_op_modes:
        - slider_to_cart
-    state_publish_rate: 25
-    action_monitor_rate: 20
-    constraints:
-      stopped_velocity_tolerance: 0.05
-      goal_time: 5
-```
-
-#### Setting PID gains
-
-To set the PID gains for a specific joint you need to define them inside `<plugin><ros></plugin></ros>`. Using the generic way of defining parameters with `gazebo_ros`. The name of the parameter correspond the name of the joint followed by a dot and the name of the parameter: `p`, `i`, `d`, `i_clamp_max`, `i_clamp_min` and `antiwindup`.
-
-```xml
-<gazebo>
-  <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-    <ros>
-      <namespace>/my_robot</namespace>
-      <parameter name="slider_to_cart.p" type="double">50.0</parameter>
-      <parameter name="slider_to_cart.i" type="double">10.0</parameter>
-      <parameter name="slider_to_cart.d" type="double">15.0</parameter>
-      <parameter name="slider_to_cart.i_clamp_max" type="double">3.0</parameter>
-      <parameter name="slider_to_cart.i_clamp_min" type="double">-3.0</parameter>
-      <parameter name="slider_to_cart.antiwindup" type="bool">false</parameter>
-      <remapping>e_stop_topic:=emergency_stop</remapping>
-    </ros>
-    <robot_sim_type>gazebo_ros2_control/DefaultRobotHWSim</robot_sim_type>
-    <parameters>$(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml</parameters>
-    ...
-  </plugins>
-</gazebo>
 ```
-
 #### Executing the examples
 
 There are some examples in the `gazebo_ros2_control_demos` package. These examples allow to launch a cart in a 30 meter rail.
@@ -197,9 +185,9 @@ There are some examples in the `gazebo_ros2_control_demos` package. These exampl
 You can run some of the configuration running the following commands:
 
 ```bash
-ros2 launch gazebo_ros2_control_demos cart_example_position_pid.launch.py
 ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py
 ros2 launch gazebo_ros2_control_demos cart_example_velocity.launch.py
+ros2 launch gazebo_ros2_control_demos cart_example_effort.launch.py
 ```
 
 Send example commands:
@@ -209,30 +197,18 @@ When the Gazebo world is launched you can run some of the following commads to m
 ```bash
 ros2 run gazebo_ros2_control_demos example_position
 ros2 run gazebo_ros2_control_demos example_velocity
+ros2 run gazebo_ros2_control_demos example_effort
 ```
 
-To get or modify the values of the PID controller you can run the following commands:
+#### Gazebo + Moveit2 + ROS 2
 
-```bash
-ros2 param get /gazebo_ros2_control slider_to_cart.p
-ros2 param get /gazebo_ros2_control slider_to_cart.i
-ros2 param get /gazebo_ros2_control slider_to_cart.d
-ros2 param get /gazebo_ros2_control slider_to_cart.i_clamp_max
-ros2 param get /gazebo_ros2_control slider_to_cart.i_clamp_min
-```
+This example works with [ROS 2 Foxy](https://index.ros.org/doc/ros2/Installation/Foxy/).
+You should install Moveit2 from sources, the instructions are available in this [link](https://moveit.ros.org/install-moveit2/source/).
+
+The repository with all the required packages are in the [gazebo_ros_demos](https://github.com/ros-simulation/gazebo_ros_demos/tree/ahcorde/port/ros2).
 
 ```bash
-ros2 param set /gazebo_ros2_control slider_to_cart.p 50.0
-ros2 param set /gazebo_ros2_control slider_to_cart.i 10.0
-ros2 param set /gazebo_ros2_control slider_to_cart.d 15.0
-ros2 param set /gazebo_ros2_control slider_to_cart.i_clamp_max 3.0
-ros2 param set /gazebo_ros2_control slider_to_cart.i_clamp_min -3.0
+ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py
 ```
 
-# NOTES
-
-`ros2_control` and `ros2_controllers` are in a redesign phase. Some of the packages are unofficial.
-In the following links you can track some of the missing features.
-
- - https://github.com/ros-controls/ros2_control/issues/26
- - https://github.com/ros-controls/ros2_control/issues/32
+![](img/moveit2.gif)

gazebo_ros2_control/CMakeLists.txt

@@ -3,60 +3,69 @@ project(gazebo_ros2_control)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
-find_package(control_toolbox REQUIRED)
 find_package(controller_manager REQUIRED)
 find_package(gazebo_dev REQUIRED)
 find_package(gazebo_ros REQUIRED)
 find_package(hardware_interface REQUIRED)
-find_package(joint_limits_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(transmission_interface REQUIRED)
 find_package(urdf REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wconversion -Wno-sign-conversion -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual)
+endif()
 
 include_directories(include)
+link_directories(
+  ${gazebo_dev_LIBRARY_DIRS}
+)
 
 # Libraries
-add_library(${PROJECT_NAME} SHARED src/gazebo_ros2_control_plugin.cpp)
+add_library(${PROJECT_NAME} SHARED
+  src/gazebo_ros2_control_plugin.cpp
+)
 ament_target_dependencies(${PROJECT_NAME}
+  angles
   controller_manager
   gazebo_dev
   gazebo_ros
   hardware_interface
   pluginlib
   rclcpp
-  transmission_interface
   urdf
   yaml_cpp_vendor
 )
 
-add_library(default_robot_hw_sim SHARED src/default_robot_hw_sim.cpp)
-ament_target_dependencies(default_robot_hw_sim
+add_library(gazebo_hardware_plugins SHARED
+  src/gazebo_system.cpp
+)
+ament_target_dependencies(gazebo_hardware_plugins
   angles
-  control_toolbox
   gazebo_dev
   hardware_interface
-  joint_limits_interface
   rclcpp
-  transmission_interface
 )
 
 ## Install
-install(TARGETS default_robot_hw_sim ${PROJECT_NAME}
+install(TARGETS
+  ${PROJECT_NAME}
+  gazebo_hardware_plugins
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
 ament_export_dependencies(ament_cmake)
 ament_export_dependencies(rclcpp)
-ament_export_libraries(${PROJECT_NAME} default_robot_hw_sim)
+ament_export_libraries(
+  ${PROJECT_NAME}
+  gazebo_hardware_plugins
+)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
@@ -68,6 +77,6 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION include/${PROJECT_NAME}
 )
 
-pluginlib_export_plugin_description_file(gazebo_ros2_control robot_hw_sim_plugins.xml)
+pluginlib_export_plugin_description_file(gazebo_ros2_control gazebo_hardware_plugins.xml)
 
 ament_package()

gazebo_ros2_control/gazebo_hardware_plugins.xml

@@ -0,0 +1,10 @@
+<library path="gazebo_hardware_plugins">
+  <class
+    name="gazebo_ros2_control/GazeboSystem"
+    type="gazebo_ros2_control::GazeboSystem"
+    base_class_type="gazebo_ros2_control::GazeboSystemInterface">
+    <description>
+      GazeboPositionJoint
+    </description>
+  </class>
+</library>