Use word 'mock' instead of 'fake' in the nomenclature of ros2_control…

… templates. (StoglRobotics#77) * Use 'mock' instead of 'fake' in the nomenclature. * Update joint_state_controller name to broadcaster. * Add launch files for starting simulators.
Robotgir · Oct 17, 2022 · 3dccecb · 3dccecb
1 parent f3cf064
commit 3dccecb
Show file tree

Hide file tree

Showing 13 changed files with 363 additions and 72 deletions.
diff --git a/.github/workflows/ci-lint.yml b/.github/workflows/ci-lint.yml
@@ -14,7 +14,7 @@ jobs:
         arguments: [""]
         include:
           - linter: copyright
-            arguments: "--exclude conf.py test_robot_description.launch.py view_robot.launch.py robot_ros2_control.launch.py test_forward_position_controller.launch.py test_joint_trajectory_controller.launch.py"
+            arguments: "--exclude conf.py test_robot_description.launch.py view_robot.launch.py robot_ros2_control.launch.py robot_ros2_control_sim.launch.py test_forward_position_controller.launch.py test_joint_trajectory_controller.launch.py"
     steps:
     - uses: actions/checkout@v2
     - uses: ros-tooling/setup-ros@master

diff --git a/scripts/setup-robot-bringup.bash b/scripts/setup-robot-bringup.bash
@@ -70,14 +70,15 @@ cp -n $ROS2_CONTROL_TEMPLATES/test_goal_publishers_config.yaml $ROBOT_FPC_PUB_YA
 
 # Copy launch files
 ROBOT_LAUNCH="launch/${ROBOT_NAME}.launch.py"
-TEST_PUB_LAUNCH="launch/test_forward_position_controller.launch.py"
-TEST_JTC_PUB_LAUNCH="launch/test_joint_trajectory_controller.launch.py"
+TEST_FWD_POS_CTRL_LAUNCH="launch/test_forward_position_controller.launch.py"
+TEST_JTC_LAUNCH="launch/test_joint_trajectory_controller.launch.py"
 cp -n $ROS2_CONTROL_TEMPLATES/robot_ros2_control.launch.py ${ROBOT_LAUNCH}
-cp -n $ROS2_CONTROL_TEMPLATES/test_forward_position_controller.launch.py $TEST_PUB_LAUNCH
+cp -n $ROS2_CONTROL_TEMPLATES/test_forward_position_controller.launch.py $TEST_FWD_POS_CTRL_LAUNCH
+cp -n $ROS2_CONTROL_TEMPLATES/test_joint_trajectory_controller.launch.py $TEST_JTC_LAUNCH
 
 
 # sed all needed files
-FILES_TO_SED=($ROBOT_LAUNCH $TEST_PUB_LAUNCH)
+FILES_TO_SED=($ROBOT_LAUNCH $TEST_FWD_POS_CTRL_LAUNCH $TEST_JTC_LAUNCH)
 
 for SED_FILE in "${FILES_TO_SED[@]}"; do
   sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE
@@ -87,7 +88,7 @@ for SED_FILE in "${FILES_TO_SED[@]}"; do
 done
 
 # package.xml: Add dependencies if they not exist
-DEP_PKGS=("xacro" "rviz2" "robot_state_publisher" "joint_trajectory_controller" "joint_state_controller" "forward_command_controller" "controller_manager" "$DESCR_PKG_NAME")
+DEP_PKGS=("xacro" "rviz2" "ros2_controllers_test_nodes" "robot_state_publisher" "joint_trajectory_controller" "joint_state_broadcaster" "forward_command_controller" "controller_manager" "$DESCR_PKG_NAME")
 
 for DEP_PKG in "${DEP_PKGS[@]}"; do
   if `grep -q $DEP_PKG package.xml`; then

diff --git a/templates/package/_append_to_README_ROS_Intro.md b/templates/package/_append_to_README_ROS_Intro.md
@@ -82,7 +82,7 @@ These instructions assume you are running Ubuntu 20.04:
    Sometimes packages do not list all their dependencies so `rosdep` will not install everything.
    If you are getting missing dependency errors, try manually install the following packages:
    ```
-   sudo apt install ros2-foxy-forward_command_controller ros2-foxy-joint_state_controller ros2-foxy-joint_trajectory_controller ros2-foxy-xacro
+   sudo apt install ros2-foxy-forward_command_controller ros2-foxy-joint_state_broadcaster ros2-foxy-joint_trajectory_controller ros2-foxy-xacro
    ```
 
 ### Configure and Build Workspace:

diff --git a/templates/robot_description/robot.urdf.xacro b/templates/robot_description/robot.urdf.xacro
@@ -2,10 +2,14 @@
 <robot xmlns:xacro="http://wiki.ros.org/xacro" name="$ROBOT_NAME$">
 
   <!-- Use this if parameters are set from the launch file, otherwise delete -->
-  <xacro:arg name="use_fake_hardware" default="false" />
-  <xacro:arg name="fake_sensor_commands" default="false" />
   <xacro:arg name="prefix" default="" />
 
+  <xacro:arg name="use_mock_hardware" default="false" />
+  <xacro:arg name="mock_sensor_commands" default="false" />
+  <xacro:arg name="sim_gazebo_classic" default="false" />
+  <xacro:arg name="sim_gazebo" default="false" />
+  <xacro:arg name="simulation_controllers" default="" />
+
   <xacro:include filename="$(find $PKG_NAME$)/urdf/$ROBOT_NAME$/$ROBOT_NAME$_macro.xacro"/>
   <xacro:include filename="$(find $PKG_NAME$)/urdf/$ROBOT_NAME$/$ROBOT_NAME$_macro.ros2_control.xacro"/>
 
@@ -21,7 +25,32 @@
   <xacro:$ROBOT_NAME$_ros2_control
     name="$ROBOT_NAME$"
     prefix="$(arg prefix)"
-    use_fake_hardware="$(arg use_fake_hardware)"
-    fake_sensor_commands="$(arg fake_sensor_commands)" />
+    use_mock_hardware="$(arg use_mock_hardware)"
+    mock_sensor_commands="$(arg mock_sensor_commands)"
+    sim_gazebo_classic="$(arg sim_gazebo_classic)"
+    sim_gazebo="$(arg sim_gazebo)"/>
+
+  <xacro:if value="$(arg sim_gazebo_classic)">
+    <!-- Gazebo plugins -->
+    <gazebo reference="world">
+    </gazebo>
+    <gazebo>
+      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+        <parameters>$(arg simulation_controllers)</parameters>
+      </plugin>
+    </gazebo>
+  </xacro:if>
+
+  <xacro:if value="$(arg sim_gazebo)">
+    <!-- Gazebo plugins -->
+    <gazebo reference="world">
+    </gazebo>
+    <gazebo>
+      <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+        <parameters>$(arg simulation_controllers)</parameters>
+        <controller_manager_node_name>$(arg prefix)controller_manager</controller_manager_node_name>
+      </plugin>
+    </gazebo>
+  </xacro:if>
 
 </robot>
diff --git a/templates/robot_description/robot_macro.ros2_control.xacro b/templates/robot_description/robot_macro.ros2_control.xacro
@@ -1,16 +1,28 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://wiki.ros.org/xacro">
 
-  <xacro:macro name="$ROBOT_NAME$_ros2_control" params="name prefix use_fake_hardware:=^|false fake_sensor_commands:=^|false">
+  <xacro:macro name="$ROBOT_NAME$_ros2_control" params="
+               name
+               prefix
+               use_mock_hardware:=^|false
+               mock_sensor_commands:=^|false
+               sim_gazebo_classic:=^|false
+               sim_gazebo:=^|false"
+               >
 
     <ros2_control name="${name}" type="system">
       <hardware>
-        <xacro:if value="${use_fake_hardware}">
-          <plugin>fake_components/GenericSystem</plugin>
-          <param name="fake_sensor_commands">${fake_sensor_commands}</param>
-          <param name="state_following_offset">0.0</param>
+        <xacro:if value="${use_mock_hardware}">
+          <plugin>mock_components/GenericSystem</plugin>
+          <param name="mock_sensor_commands">${mock_sensor_commands}</param>
         </xacro:if>
-        <xacro:unless value="${use_fake_hardware}">
+        <xacro:if value="${sim_gazebo_classic}">
+          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        </xacro:if>
+        <xacro:if value="${sim_gazebo}">
+          <plugin>ign_ros2_control/IgnitionSystem</plugin>
+        </xacro:if>
+        <xacro:unless value="${use_mock_hardware or sim_gazebo_classic or sim_gazebo}">
           <plugin>robot_hardware_inteface/RobotHardwareInteface</plugin>
         </xacro:unless>
       </hardware>
@@ -19,60 +31,66 @@
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        <state_interface/>
         <state_interface name="velocity"/>
         <state_interface name="acceleration"/>
-        <param name="initial_position">0.0</param>
       </joint>
       <joint name="${prefix}joint2">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        <state_interface/>
         <state_interface name="velocity"/>
         <state_interface name="acceleration"/>
-        <param name="initial_position">0.0</param>
       </joint>
       <joint name="${prefix}joint3">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        <state_interface/>
         <state_interface name="velocity"/>
         <state_interface name="acceleration"/>
-        <param name="initial_position">0.0</param>
       </joint>
       <joint name="${prefix}joint4">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        <state_interface/>
         <state_interface name="velocity"/>
         <state_interface name="acceleration"/>
-        <param name="initial_position">0.0</param>
       </joint>
       <joint name="${prefix}joint5">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        <state_interface/>
         <state_interface name="velocity"/>
         <state_interface name="acceleration"/>
-        <param name="initial_position">0.0</param>
       </joint>
       <joint name="${prefix}joint6">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        <state_interface/>
         <state_interface name="velocity"/>
         <state_interface name="acceleration"/>
-        <param name="initial_position">0.0</param>
       </joint>
       <sensor name="tcp_fts_sensor">            <!-- Remove/change sensors as needed -->
         <state_interface name="fx"/>

diff --git a/templates/ros2_control/append_to_README.md b/templates/ros2_control/append_to_README.md
@@ -62,17 +62,17 @@ The sections below describe their usage.
 ### Joint State Controller
 Joint state Controllers provides output of robot's internal states to `/joint_states` and `/dynamic_joint_states` ROS2-topics.
 
-In a new terminal with sourced ROS2 environment load, configure and start `joint_state_controller`:
+In a new terminal with sourced ROS2 environment load, configure and start `joint_state_broadcaster`:
   ```
-  ros2 control load_start_controller joint_state_controller
+  ros2 control load_start_controller joint_state_broadcaster
   ```
 Check if controller is loaded properly:
  ```
  ros2 control list_controllers
  ```
 You should get similar response to:
  ```
- joint_state_controller[joint_state_controller/JointStateController] active
+ joint_state_broadcaster[joint_state_broadcaster/JointStateController] active
  ```
 
 Now you should also see your robot represented correctly in the `rviz2`.
@@ -89,7 +89,7 @@ Now you should also see your robot represented correctly in the `rviz2`.
    ```
    You should get the response:
    ```
-   joint_state_controller[joint_state_controller/JointStateController] active
+   joint_state_broadcaster[joint_state_broadcaster/JointStateController] active
    forward_<controller_type>_controller[forward_command_controller/ForwardCommandController] inactive
    ```
 
@@ -103,7 +103,7 @@ Now you should also see your robot represented correctly in the `rviz2`.
    ```
    You should get `active` in the response:
    ```
-   joint_state_controller[joint_state_controller/JointStateController] active
+   joint_state_broadcaster[joint_state_broadcaster/JointStateController] active
    forward_<controller_type>_controller[forward_command_controller/ForwardCommandController] active
    ```
 
@@ -138,7 +138,7 @@ Now you should also see your robot represented correctly in the `rviz2`.
    ```
    You should get `active` in the response:
    ```
-   joint_state_controller[joint_state_controller/JointStateController] active
+   joint_state_broadcaster[joint_state_broadcaster/JointStateController] active
    forward_<controller_type>_controller[forward_command_controller/ForwardCommandController] inactive
    ```
 
@@ -152,7 +152,7 @@ Now you should also see your robot represented correctly in the `rviz2`.
    ```
    You should get `active` in the response:
    ```
-   joint_state_controller[joint_state_controller/JointStateController] active
+   joint_state_broadcaster[joint_state_broadcaster/JointStateController] active
    joint_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] active
    ```
 

diff --git a/templates/ros2_control/hardware/robot_hardware_interface.cpp b/templates/ros2_control/hardware/robot_hardware_interface.cpp
@@ -38,7 +38,7 @@ hardware_interface::CallbackReturn DummyClassName::on_init(
 std::vector<hardware_interface::StateInterface> DummyClassName::export_state_interfaces()
 {
   std::vector<hardware_interface::StateInterface> state_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++) {
+  for (size_t i = 0; i < info_.joints.size(); ++i) {
     state_interfaces.emplace_back(hardware_interface::StateInterface(
       // TODO(anyone): insert correct interfaces
       info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
@@ -50,7 +50,7 @@ std::vector<hardware_interface::StateInterface> DummyClassName::export_state_int
 std::vector<hardware_interface::CommandInterface> DummyClassName::export_command_interfaces()
 {
   std::vector<hardware_interface::CommandInterface> command_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++) {
+  for (size_t i = 0; i < info_.joints.size(); ++i) {
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
       // TODO(anyone): insert correct interfaces
       info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));

diff --git a/templates/ros2_control/robot_controllers.yaml b/templates/ros2_control/robot_controllers.yaml
@@ -2,8 +2,8 @@ controller_manager:
   ros__parameters:
     update_rate: 100  # Hz
 
-    joint_state_controller:
-      type: joint_state_controller/JointStateController
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
 
     forward_position_controller:              # delete entry if controller is not applicable
       type: forward_command_controller/ForwardCommandController