Implemented twist_mux

src/rove_navigation/config/twist_mux.yaml → src/rove_bringup/config/twist_mux.yaml

@@ -1,11 +1,15 @@
-twist_mux:
-  ros_parameters:
+/twist_mux:
+  ros__parameters:
     topics:
       navigation:
-        topic: cmd_vel
+        topic: nav_vel
         timeout: 0.5
         priority: 10
       joystick:
-        topic: cmd_vel_joy
+        topic: joy_vel
         timeout: 0.5
         priority: 20
+      others:
+        topic: cmd_vel
+        timeout: 0.5
+        priority: 1

src/rove_bringup/launch/common.launch.py

@@ -97,12 +97,22 @@ def generate_launch_description():
             "use_slam3d": "false",
         }.items(),
     )
+
+    twist_mux = Node(
+      package='twist_mux',
+      executable='twist_mux',
+      output='screen',
+      parameters=[os.path.join(pkg_rove_bringup, 'config/twist_mux.yaml')],
+      remappings={
+          ('/cmd_vel_out', '/diff_drive_controller/cmd_vel_unstamped')
+      })
 
     return LaunchDescription([
             robot_state_publisher,
             robot_localization_node_local,
-            #robot_localization_node_global,
+            # robot_localization_node_global,
             #navsat_transform,
+            twist_mux,
             rviz,
             teleop,
             autonomy,

src/rove_bringup/launch/rove_controller_bluetooth.launch.py

@@ -25,9 +25,7 @@ def generate_launch_description():
             parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
-                # Remap needs to happen here since the diff_drive_controller node
-                # is started via a spawner, thus remappings do not work on it
-                ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped') 
+                ('/cmd_vel', '/joy_vel') 
             ],
         ),
         Node(

src/rove_bringup/launch/rove_controller_usb.launch.py

@@ -25,9 +25,7 @@ def generate_launch_description():
             parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
-                # Remap needs to happen here since the diff_drive_controller node
-                # is started via a spawner, thus remappings do not work on it
-                ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped')
+                ('/cmd_vel', '/joy_vel') 
             ],
         ),
         Node(

src/rove_bringup/package.xml

@@ -9,11 +9,13 @@
 
   <build_depend>joy</build_depend>
   <build_depend>teleop_twist_joy</build_depend>
+  <build_depend>twist_mux</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
 
   <exec_depend>joy</exec_depend>
   <exec_depend>teleop_twist_joy</exec_depend>
+  <exec_depend>twist_mux</exec_depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

src/rove_description/config/default_bridge.yaml

@@ -6,7 +6,7 @@
   direction: GZ_TO_ROS
 
 # gz topic subscribed to by Tracked Vehicle plugin
-- ros_topic_name: "cmd_vel"
+- ros_topic_name: "diff_drive_controller/cmd_vel_unstamped"
   gz_topic_name: "cmd_vel"
   ros_type_name: "geometry_msgs/msg/Twist"
   gz_type_name: "gz.msgs.Twist"

src/rove_description/urdf/rove.urdf.xacro

@@ -44,6 +44,7 @@
     <!-- <xacro:rove_ros2_control/> -->
      <!-- Put these at -1 to mock the motors if working without the motors -->
     <xacro:odrive_control fl_id="21" rl_id="22" fr_id="23" rr_id="24"/>
+    <!-- <xacro:odrive_control fl_id="-1" rl_id="-1" fr_id="-1" rr_id="-1"/> -->
 
 
     <!-- Insert ovis on top of rove -->