Updated with ros2-control Foxy API

Docker/Dockerfile

@@ -9,24 +9,34 @@ RUN apt-get update && apt-get install -q -y --no-install-recommends \
     dirmngr \
     gnupg2 \
     lsb-release \
+    wget \
     python3-colcon-ros \
     && apt-get clean
 
 RUN mkdir -p /home/ros2_ws/src \
     && cd /home/ros2_ws/src/ \
-    && git clone https://github.com/ros-simulation/gazebo_ros2_control \
+    # && wget https://raw.githubusercontent.com/ros-planning/moveit2/main/moveit2.repos \
+    # && vcs import < moveit2.repos \
+    && git clone https://github.com/ros-planning/moveit2 \
+    && git clone https://github.com/ros-planning/moveit_msgs -b ros2 \
+    && git clone https://github.com/ros-planning/moveit_resources -b ros2 \
+    && git clone https://github.com/ros-planning/geometric_shapes -b ros2 \
+    && git clone https://github.com/ros-planning/srdfdom -b ros2 \
+    && git clone https://github.com/ros-planning/warehouse_ros -b ros2 \
+    && git clone https://github.com/ros-planning/warehouse_ros_mongo -b ros2 \
+    && git clone https://github.com/ros-simulation/gazebo_ros2_control -b ahcorde/update/foxy \
     && 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_ros_demos -b ahcorde/port/ros2 \
     && 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

@@ -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 sim interface 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`.
@@ -166,7 +173,9 @@ cart_pole_controller:
 
 #### 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`.
+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 to
+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>
@@ -181,7 +190,7 @@ To set the PID gains for a specific joint you need to define them inside `<plugi
       <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>
+    <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
     <parameters>$(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml</parameters>
     ...
   </plugins>
@@ -200,6 +209,7 @@ You can run some of the configuration running the following commands:
 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,6 +219,7 @@ 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:
@@ -229,10 +240,15 @@ 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
 ```
 
-# NOTES
+#### Gazebo + Moveit2 + ROS 2
 
-`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.
+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/).
 
- - https://github.com/ros-controls/ros2_control/issues/26
- - https://github.com/ros-controls/ros2_control/issues/32
+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 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py
+```
+
+![](img/moveit2.gif)

gazebo_ros2_control/CMakeLists.txt

@@ -8,7 +8,6 @@ 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)
@@ -21,11 +20,18 @@ if(NOT CMAKE_CXX_STANDARD)
 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
+  control_toolbox
   gazebo_dev
   gazebo_ros
   hardware_interface
@@ -36,27 +42,32 @@ 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
+  control_toolbox
+  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>