From abdac96943831892dc0c682d121a37dc0f199baf Mon Sep 17 00:00:00 2001
From: ilithebutterfly <iliana.decarufel@gmail.com>
Date: Fri, 21 Jun 2024 10:33:31 -0400
Subject: [PATCH] Implemented twist_mux

---
 .../config/twist_mux.yaml                            | 12 ++++++++----
 src/rove_bringup/launch/common.launch.py             | 12 +++++++++++-
 .../launch/rove_controller_bluetooth.launch.py       |  4 +---
 .../launch/rove_controller_usb.launch.py             |  4 +---
 src/rove_bringup/package.xml                         |  2 ++
 src/rove_description/config/default_bridge.yaml      |  2 +-
 src/rove_description/urdf/rove.urdf.xacro            |  1 +
 7 files changed, 25 insertions(+), 12 deletions(-)
 rename src/{rove_navigation => rove_bringup}/config/twist_mux.yaml (53%)

diff --git a/src/rove_navigation/config/twist_mux.yaml b/src/rove_bringup/config/twist_mux.yaml
similarity index 53%
rename from src/rove_navigation/config/twist_mux.yaml
rename to src/rove_bringup/config/twist_mux.yaml
index e960825..893dd7c 100644
--- a/src/rove_navigation/config/twist_mux.yaml
+++ b/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
\ No newline at end of file
diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py
index 87781ff..e2ebf97 100644
--- a/src/rove_bringup/launch/common.launch.py
+++ b/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,
diff --git a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py b/src/rove_bringup/launch/rove_controller_bluetooth.launch.py
index 3c4a870..d42ffec 100644
--- a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py
+++ b/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(
diff --git a/src/rove_bringup/launch/rove_controller_usb.launch.py b/src/rove_bringup/launch/rove_controller_usb.launch.py
index 9d34d47..0cb7e79 100644
--- a/src/rove_bringup/launch/rove_controller_usb.launch.py
+++ b/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(
diff --git a/src/rove_bringup/package.xml b/src/rove_bringup/package.xml
index 808c303..6e2a0cf 100644
--- a/src/rove_bringup/package.xml
+++ b/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>
 
diff --git a/src/rove_description/config/default_bridge.yaml b/src/rove_description/config/default_bridge.yaml
index 822cf55..fe31e4b 100644
--- a/src/rove_description/config/default_bridge.yaml
+++ b/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"
diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro
index 82334f1..9dd637d 100644
--- a/src/rove_description/urdf/rove.urdf.xacro
+++ b/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 -->