Updated with ros2-control Foxy API

.github/workflows/ci.yaml

@@ -18,16 +18,15 @@ jobs:
         cd ..
         mkdir -p /home/ros2_ws/src
         cp -r gazebo_ros2_control /home/ros2_ws/src/
-        apt-get upgrade -q -y
+        # TODO(ahcorde): Remove this line when a new foxy released is launched (transmission_interface)
+        echo "deb http://packages.ros.org/ros2-testing/ubuntu focal main" > /etc/apt/sources.list.d/ros2-latest.list
+        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 +35,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

@@ -1,5 +1,8 @@
 FROM osrf/ros:foxy-desktop
 
+# TODO(ahcorde): Remove this line when a new foxy released is launched (transmission_interface)
+RUN echo "deb http://packages.ros.org/ros2-testing/ubuntu focal main" > /etc/apt/sources.list.d/ros2-latest.list
+
 # Make sure everything is up to date before building from source
 RUN apt-get update \
   && apt-get upgrade -y \
@@ -14,19 +17,16 @@ 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 \
+    && git clone https://github.com/ros-simulation/gazebo_ros2_control -b ahcorde/update/foxy \
     && 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"]
 
-CMD ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py
+CMD ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py

README.md

@@ -13,7 +13,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
 
@@ -63,9 +63,11 @@ ros2 run gazebo_ros2_control_demos example_position
 
 ## Add transmission elements 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.
+The `<transmission>` element is used to link actuators to joints, see the `<transmission>` spec for exact XML format.
 
-For the purposes of `gazebo_ros2_control` in its current implementation, the only important information in these transmission tags are:
+For the purposes of `gazebo_ros2_control` in its current implementation, the only important information
+in these transmission tags are:
 
  - `<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.
@@ -82,11 +84,12 @@ 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_sim_type>gazebo_ros2_control/GazeboSystem</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>
+      <control_period>0.01</control_period>
     </plugin>
 </gazebo>
 ```
@@ -96,8 +99,9 @@ The `gazebo_ros2_control` `<plugin>` tag also has the following optional child e
  - `<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`
+ - `<robot_sim_type>`: The pluginlib name of a custom robot ros2_control system plugin to be used, defaults to `gazebo_ros2_control/GazeboSystem`
  - `<parameters>`: YAML file with the configuration of the controllers
+ - `<e_stop_topic>`: Topic to publish the emergency stop
 
 #### Default gazebo_ros2_control Behavior
 
@@ -113,14 +117,17 @@ 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.
+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 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):
+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
+(same behavior as when using no `<robot_sim_type>` tag):
 
 ```xml
 <gazebo>
   <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
-    <robot_sim_type>gazebo_ros2_control/DefaultRobotHWSim</robot_sim_type>
+    <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
   </plugin>
 </gazebo>
 ```
@@ -138,13 +145,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`.
@@ -163,31 +170,6 @@ cart_pole_controller:
       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 +179,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 +191,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,12 +3,10 @@ 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)
@@ -19,12 +17,21 @@ find_package(yaml_cpp_vendor REQUIRED)
 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
@@ -36,27 +43,31 @@ ament_target_dependencies(${PROJECT_NAME}
   yaml_cpp_vendor
 )
 
-add_library(default_robot_hw_sim SHARED src/default_robot_hw_sim.cpp)
-ament_target_dependencies(default_robot_hw_sim
-  angles
-  control_toolbox
-  gazebo_dev
+add_library(gazebo_hardware_plugins SHARED
+  src/gazebo_system.cpp
+)
+ament_target_dependencies(gazebo_hardware_plugins
   hardware_interface
-  joint_limits_interface
-  rclcpp
+  gazebo_dev
   transmission_interface
+  angles
 )
 
 ## 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 +79,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>