Fix simulation and rename drive_controller node (#146)

rafal-gorecki · web-flow · commit c20fca85a03d · 2025-09-04T15:55:15.000+02:00
* Launch RViz with simulation

* Fix simulation and rename drive_controller node
diff --git a/README.md b/README.md
@@ -116,6 +116,7 @@ ros2 launch rosbot_gazebo simulation.launch.py robot_model:=<rosbot/rosbot_xl>
 | ❌  | ✅  | `gz_headless_mode`  | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations. <br/> **_bool:_** `False`                                                            |
 | ❌  | ✅  | `gz_log_level`      | Adjust the level of console output. <br/> **_int:_** `1` (choices: `0`, `1`, `2`, `3`, `4`)                                                                                                        |
 | ❌  | ✅  | `gz_world`          | Absolute path to SDF world file. <br/> **_string:_** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf)                                    |
+| ❌  | ✅  | `rviz`          | Run RViz simultaneously. <br/> **_bool:_** `True`                                    |
 | ❌  | ✅  | `x`                 | Initial robot position in the global 'x' axis. <br/> **_float:_** `0.0`                                                                                                                            |
 | ❌  | ✅  | `y`                 | Initial robot position in the global 'y' axis. <br/> **_float:_** `2.0`                                                                                                                            |
 | ❌  | ✅  | `z`                 | Initial robot position in the global 'z' axis. <br/> **_float:_** `0.0`                                                                                                                            |
diff --git a/ROS_API.md b/ROS_API.md
@@ -22,6 +22,7 @@ Detailed information about content of rosbot package for ROS2.
 | 🤖  | 🖥️  | NODE                          | DESCRIPTION                                                                                                                                                                                                                                                                                                                                         |
 | --- | --- | ----------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
 | ✅  | ✅  | **`controller_manager`**      | Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. <br /> _[controller_manager/controller_manager]_                             |
+| ✅  | ✅  | **`drive_controller`**  | The controller managing a mobile robot with a differential or omni drive (mecanum wheels). Converts speed commands for the robot body to wheel commands for the base. It also calculates odometry based on hardware feedback and shares it.`DiffDriveController` or `MecanumDriveController` <br /> _[diff_drive_controller/diff_drive_controller]_ |
 | ✅  | ✅  | **`ekf_node`**                | Used to fuse wheel odometry and IMU data. Parameters are defined in `rosbot_bringup/config/ekf.yaml` <br /> _[robot_localization/ekf_node]_                                                                                                                                                                                                         |
 | ❌  | ✅  | **`/gz_bridge`**              | Transmits Gazebo simulation data to the ROS layer <br /> _[ros_gz_bridge/parameter_bridge]_                                                                                                                                                                                                                                         |
 | ❌  | ✅  | **`gz_ros_control`**         | Responsible for integrating the ros2_control controller architecture with the Gazebo simulator. <br /> _[gz_ros2_control/gz_ros2_control]_                                                                                                                                                                                                                                         |
@@ -31,7 +32,6 @@ Detailed information about content of rosbot package for ROS2.
 | ✅  | ✅  | **`laser_filter`**            | This is a filter that removes points in a laser scan inside of a cartesian box <br /> _[laser_filters/scan_to_scan_filter_chain]_                                                                                                                                                                                                                   |
 | ✅  | ✅  | **`robot_state_publisher`**   | Uses the URDF specified by the parameter robot\*description and the joint positions from the topic joint\*states to calculate the forward kinematics of the robot and publish the results using tf <br /> _[robot_state_publisher/robot_state_publisher]_                                                                                             |
 | ✅  | ❌  | **`rosbot_system_node`**      | The node communicating with the hardware responsible for receiving and sending data related to engine control <br /> _[rosbot_hardware_interfaces/rosbot_system]_                                                                                                                                                                                   |
-| ✅  | ✅  | **`rosbot_base_controller`**  | The controller managing a mobile robot with a differential or omni drive (mecanum wheels). Converts speed commands for the robot body to wheel commands for the base. It also calculates odometry based on hardware feedback and shares it.`DiffDriveController` or `MecanumDriveController` <br /> _[diff_drive_controller/diff_drive_controller]_ |
 | ❌  | ✅  | **`rosbot_gz_bridge`**        | Transmits data about the robot between the Gazebo simulator and ROS. <br /> _[ros_gz_bridge/parameter_bridge]_ |
 | ✅  | ❌  | **`/stm32_node`**             | Node responsible for communication with hardware. <br /> _[micro_ros_agent/micro_ros_agent]_                                                                                                                                                                                                                      |
 
@@ -41,7 +41,6 @@ Detailed information about content of rosbot package for ROS2.
 [diagnostic_msgs/DiagnosticArray]: https://docs.ros2.org/foxy/api/diagnostic_msgs/msg/DiagnosticArray.html
 [geometry_msgs/PoseWithCovarianceStamped]: https://docs.ros2.org/foxy/api/geometry_msgs/msg/PoseWithCovarianceStamped.html
 [geometry_msgs/Twist]: https://docs.ros2.org/foxy/api/geometry_msgs/msg/Twist.html
-[lifecycle_msgs/TransitionEvent]: https://docs.ros2.org/foxy/api/lifecycle_msgs/msg/TransitionEvent.html
 [nav_msgs/Odometry]: https://docs.ros2.org/foxy/api/nav_msgs/msg/Odometry.html
 [sensor_msgs/BatteryState]: https://docs.ros2.org/foxy/api/sensor_msgs/msg/BatteryState.html
 [sensor_msgs/Imu]: https://docs.ros2.org/foxy/api/sensor_msgs/msg/Imu.html
@@ -58,13 +57,10 @@ Detailed information about content of rosbot package for ROS2.
 | ✅  | ✅  | **`diagnostics`**                              | Contains diagnostic information about the robot's systems. <br /> _[diagnostic_msgs/DiagnosticArray]_                         |
 | ✅  | ✅  | **`dynamic_joint_states`**                     | Publishes information about the dynamic state of joints. <br /> _[control_msgs/DynamicJointState]_                            |
 | ✅  | ✅  | **`imu_broadcaster/imu`**                      | Broadcasts IMU (Inertial Measurement Unit) data. <br /> _[sensor_msgs/Imu]_                                                   |
-| ✅  | ✅  | **`imu_broadcaster/transition_event`**         | Signals transition events in the lifecycle of the IMU broadcaster node. <br /> _[lifecycle_msgs/TransitionEvent]_             |
-| ✅  | ✅  | **`joint_state_broadcaster/transition_event`** | Indicates transition events in the lifecycle of the joint state broadcaster node. <br /> _[lifecycle_msgs/TransitionEvent]_   |
 | ✅  | ✅  | **`joint_states`**                             | Publishes information about the state of robot joints. <br /> _[sensor_msgs/JointState]_                                      |
 | ✅  | ✅  | **`odometry/filtered`**                        | Publishes filtered odometry data. <br /> _[nav_msgs/Odometry]_                                                                |
+| ✅  | ✅  | **`odometry/wheels`**              | Provides odometry data from the base controller of the ROSbot XL. <br /> _[nav_msgs/Odometry]_                                |
 | ✅  | ✅  | **`robot_description`**                        | Publishes the robot's description. <br /> _[std_msgs/String]_                                                                 |
-| ✅  | ✅  | **`rosbot_base_controller/odom`**              | Provides odometry data from the base controller of the ROSbot XL. <br /> _[nav_msgs/Odometry]_                                |
-| ✅  | ✅  | **`rosbot_base_controller/transition_event`**  | Indicates transition events in the lifecycle of the ROSbot XL base controller node. <br /> _[lifecycle_msgs/TransitionEvent]_ |
 | ✅  | ✅  | **`scan`**                                     | Publishes raw laser scan data. <br /> _[sensor_msgs/LaserScan]_                                                               |
 | ✅  | ✅  | **`scan_filtered`**                            | Publishes filtered laser scan data. <br /> _[sensor_msgs/LaserScan]_                                                          |
 | ✅  | ✅  | **`set_pose`**                                 | Sets the robot's pose with covariance. <br /> _[geometry_msgs/PoseWithCovarianceStamped]_                                     |
diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos
@@ -10,7 +10,7 @@ repositories:
   husarion_components_description:
     type: git
     url: https://github.com/husarion/husarion_components_description.git
-    version: 3495ff0933cabc1437d85b152af4ffc2c77ef866
+    version: 5ad18c27f53b129a99bf9df513d16fda5a4bca6f
   rosbot_hardware_interfaces:
     type: git
     url: https://github.com/husarion/rosbot_hardware_interfaces.git
diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos
@@ -10,4 +10,4 @@ repositories:
   husarion_components_description:
     type: git
     url: https://github.com/husarion/husarion_components_description.git
-    version: 3495ff0933cabc1437d85b152af4ffc2c77ef866
+    version: 5ad18c27f53b129a99bf9df513d16fda5a4bca6f
diff --git a/rosbot_bringup/test/test_utils.py b/rosbot_bringup/test/test_utils.py
@@ -56,7 +56,7 @@ def create_test_subscribers_and_publishers(self):
             JointState, "joint_states", self.joint_states_callback, 10
         )
         self.controller_odom_sub = self.create_subscription(
-            Odometry, "rosbot_base_controller/odom", self.controller_odometry_callback, 10
+            Odometry, "odometry/wheels", self.controller_odometry_callback, 10
         )
         self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10)
         self.ekf_odom_sub = self.create_subscription(
diff --git a/rosbot_controller/config/rosbot/diff_drive_controller.yaml b/rosbot_controller/config/rosbot/diff_drive_controller.yaml
@@ -7,7 +7,7 @@
       type: joint_state_broadcaster/JointStateBroadcaster
     imu_broadcaster:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
-    rosbot_base_controller:
+    drive_controller:
       type: diff_drive_controller/DiffDriveController
 
 /**/imu_broadcaster:
@@ -19,7 +19,7 @@
     static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally
     static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally
 
-/**/rosbot_base_controller:
+/**/drive_controller:
   ros__parameters:
     tf_frame_prefix_enable: false
 
diff --git a/rosbot_controller/config/rosbot/mecanum_drive_controller.yaml b/rosbot_controller/config/rosbot/mecanum_drive_controller.yaml
@@ -7,7 +7,7 @@
       type: joint_state_broadcaster/JointStateBroadcaster
     imu_broadcaster:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
-    rosbot_base_controller:
+    drive_controller:
       type: mecanum_drive_controller/MecanumDriveController
 
 /**/imu_broadcaster:
@@ -19,7 +19,7 @@
     static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally
     static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally
 
-/**/rosbot_base_controller:
+/**/drive_controller:
   ros__parameters:
     tf_frame_prefix_enable: false
 
diff --git a/rosbot_controller/config/rosbot_xl/diff_drive_controller.yaml b/rosbot_controller/config/rosbot_xl/diff_drive_controller.yaml
@@ -8,7 +8,7 @@
       type: joint_state_broadcaster/JointStateBroadcaster
     imu_broadcaster:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
-    rosbot_base_controller:
+    drive_controller:
       type: diff_drive_controller/DiffDriveController
 
 /**/imu_broadcaster:
@@ -20,7 +20,7 @@
     static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally
     static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally
 
-/**/rosbot_base_controller:
+/**/drive_controller:
   ros__parameters:
     tf_frame_prefix_enable: false
     left_wheel_names: [fl_wheel_joint, rl_wheel_joint]
diff --git a/rosbot_controller/config/rosbot_xl/diff_drive_manipulator_controller.yaml b/rosbot_controller/config/rosbot_xl/diff_drive_manipulator_controller.yaml
@@ -8,7 +8,7 @@
       type: joint_state_broadcaster/JointStateBroadcaster
     imu_broadcaster:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
-    rosbot_base_controller:
+    drive_controller:
       type: diff_drive_controller/DiffDriveController
     manipulator_controller:
       type: joint_trajectory_controller/JointTrajectoryController
@@ -24,7 +24,7 @@
     static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally
     static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally
 
-/**/rosbot_base_controller:
+/**/drive_controller:
   ros__parameters:
     tf_frame_prefix_enable: false
     left_wheel_names: [fl_wheel_joint, rl_wheel_joint]
diff --git a/rosbot_controller/config/rosbot_xl/mecanum_drive_controller.yaml b/rosbot_controller/config/rosbot_xl/mecanum_drive_controller.yaml
@@ -8,7 +8,7 @@
       type: joint_state_broadcaster/JointStateBroadcaster
     imu_broadcaster:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
-    rosbot_base_controller:
+    drive_controller:
       type: mecanum_drive_controller/MecanumDriveController
 
 /**/imu_broadcaster:
@@ -20,7 +20,7 @@
     static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally
     static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally
 
-/**/rosbot_base_controller:
+/**/drive_controller:
   ros__parameters:
     tf_frame_prefix_enable: false
 
diff --git a/rosbot_controller/config/rosbot_xl/mecanum_drive_manipulator_controller.yaml b/rosbot_controller/config/rosbot_xl/mecanum_drive_manipulator_controller.yaml
@@ -8,7 +8,7 @@
       type: joint_state_broadcaster/JointStateBroadcaster
     imu_broadcaster:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
-    rosbot_base_controller:
+    drive_controller:
       type: mecanum_drive_controller/MecanumDriveController
     manipulator_controller:
       type: joint_trajectory_controller/JointTrajectoryController
@@ -24,7 +24,7 @@
     static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally
     static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally
 
-/**/rosbot_base_controller:
+/**/drive_controller:
   ros__parameters:
     tf_frame_prefix_enable: false
 
diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py
@@ -122,11 +122,17 @@ def generate_launch_description():
         executable="ros2_control_node",
         parameters=[ns_controller_config],
         remappings=[
+            ("drive_controller/cmd_vel", "cmd_vel"),
+            ("drive_controller/odom", "odometry/wheels"),
+            ("drive_controller/transition_event", "_drive_controller/transition_event"),
             ("imu_sensor_node/imu", "/_imu/data_raw"),
+            ("imu_broadcaster/transition_event", "_imu_broadcaster/transition_event"),
+            (
+                "joint_state_broadcaster/transition_event",
+                "_joint_state_broadcaster/transition_event",
+            ),
             ("~/motors_cmd", "/_motors_cmd"),
             ("~/motors_response", "/_motors_response"),
-            ("rosbot_base_controller/cmd_vel_unstamped", "cmd_vel"),
-            ("~/robot_description", "robot_description"),
         ],
         condition=UnlessCondition(use_sim),
     )
@@ -143,11 +149,11 @@ def generate_launch_description():
         ],
     )
 
-    robot_controller = Node(
+    drive_controller = Node(
         package="controller_manager",
         executable="spawner",
         arguments=[
-            "rosbot_base_controller",
+            "drive_controller",
             "-c",
             "controller_manager",
             "--controller-manager-timeout",
@@ -176,7 +182,7 @@ def generate_launch_description():
         condition=IfCondition(manipulator),
     )
 
-    controllers = [joint_state_broadcaster, imu_broadcaster, robot_controller]
+    controllers = [joint_state_broadcaster, imu_broadcaster, drive_controller]
 
     # spawners expect ros2_control_node to be running
     delayed_controllers = TimerAction(period=4.0, actions=controllers)
diff --git a/rosbot_description/urdf/common/gazebo.urdf.xacro b/rosbot_description/urdf/common/gazebo.urdf.xacro
@@ -7,6 +7,11 @@
         <parameters>${config_file}</parameters>
         <ros>
           <namespace>${namespace}</namespace>
+          <remapping>drive_controller/cmd_vel:=cmd_vel</remapping>
+          <remapping>drive_controller/odom:=odometry/wheels</remapping>
+          <remapping>drive_controller/transition_event:=_drive_controller/transition_event</remapping>
+          <remapping>imu_broadcaster/transition_event:=_imu_broadcaster/transition_event</remapping>
+          <remapping>joint_state_broadcaster/transition_event:=_joint_state_broadcaster/transition_event</remapping>
           <remapping>/diagnostics:=diagnostics</remapping>
           <remapping>rosbot_base_controller/cmd_vel_unstamped:=cmd_vel</remapping>
           <remapping>/tf:=tf</remapping>
diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py
@@ -13,14 +13,23 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node, SetParameter, SetRemap
 from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
+    rviz = LaunchConfiguration("rviz")
+    declare_rviz_arg = DeclareLaunchArgument(
+        "rviz",
+        default_value="True",
+        description="Run RViz simultaneously.",
+        choices=["True", "true", "False", "false"],
+    )
+
     gz_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -52,14 +61,30 @@ def generate_launch_description():
         ),
     )
 
+    rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("rosbot_description"),
+                    "launch",
+                    "rviz.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={"namespace": ""}.items(),
+        condition=IfCondition(rviz),
+    )
+
     return LaunchDescription(
         [
+            declare_rviz_arg,
             SetRemap("/diagnostics", "diagnostics"),
             SetRemap("/tf", "tf"),
             SetRemap("/tf_static", "tf_static"),
             SetParameter(name="use_sim_time", value=True),
             gz_sim,
             gz_bridge,
             spawn_robot,
+            rviz_launch,
         ]
     )
diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml
diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py
diff --git a/rosbot_localization/config/ekf.yaml b/rosbot_localization/config/ekf.yaml

Original file line number	Diff line number	Diff line change
`@@ -56,7 +56,7 @@ def create_test_subscribers_and_publishers(self):`
`56`	`56`	`JointState, "joint_states", self.joint_states_callback, 10`
`57`	`57`	`)`
`58`	`58`	`self.controller_odom_sub = self.create_subscription(`
`59`		`- Odometry, "rosbot_base_controller/odom", self.controller_odometry_callback, 10`
	`59`	`+ Odometry, "odometry/wheels", self.controller_odometry_callback, 10`
`60`	`60`	`)`
`61`	`61`	`self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10)`
`62`	`62`	`self.ekf_odom_sub = self.create_subscription(`