From 9730af31f22793860c5509dc09f3f51382ac1d57 Mon Sep 17 00:00:00 2001
From: SimonR99 <simonroy99@hotmail.ca>
Date: Wed, 14 Jun 2023 09:00:50 +0200
Subject: [PATCH 1/2] add mux basic system

---
 markhor_bringup/config/twist_mux.yaml          | 10 ++++++++++
 markhor_bringup/launch/teleop_twist_joy.launch | 12 ++++++++++--
 markhor_control/package.xml                    |  1 +
 markhor_navigation/launch/markhor_nav.launch   |  4 ++--
 4 files changed, 23 insertions(+), 4 deletions(-)
 create mode 100644 markhor_bringup/config/twist_mux.yaml

diff --git a/markhor_bringup/config/twist_mux.yaml b/markhor_bringup/config/twist_mux.yaml
new file mode 100644
index 00000000..70148be8
--- /dev/null
+++ b/markhor_bringup/config/twist_mux.yaml
@@ -0,0 +1,10 @@
+twist_mux:
+  topics:
+    - name    : joy_cmd_vel
+      topic   : /markhor/joy/cmd_vel
+      timeout : 0.5
+      priority: 2
+    - name    : move_base_cmd_vel
+      topic   : /move_base/cmd_vel
+      timeout : 0.5
+      priority: 1
diff --git a/markhor_bringup/launch/teleop_twist_joy.launch b/markhor_bringup/launch/teleop_twist_joy.launch
index 009d940c..17ce68d7 100644
--- a/markhor_bringup/launch/teleop_twist_joy.launch
+++ b/markhor_bringup/launch/teleop_twist_joy.launch
@@ -1,9 +1,17 @@
 <launch>
     <arg name="joy_config" default="xbox" />
-    <arg name="config_filepath" default="$(find markhor_bringup)/config/$(arg joy_config).config.yaml" />
+    <arg name="config_filepath"
+        default="$(find markhor_bringup)/config/$(arg joy_config).config.yaml" />
 
     <node pkg="teleop_twist_joy" name="teleop_twist_joy" type="teleop_node">
         <rosparam command="load" file="$(arg config_filepath)" />
-        <remap from="cmd_vel" to="/markhor/tracks/diff_drive_controller/cmd_vel" />
+        <remap from="cmd_vel" to="/markhor/joy/cmd_vel" />
     </node>
+
+    <!-- Multiplexer to prioritize teleop commands over autonomous commands -->
+    <node pkg="twist_mux" type="twist_mux" name="twist_mux" respawn="false">
+        <rosparam file="$(find markhor_bringup)/config/twist_mux.yaml" command="load" />
+        <remap from="cmd_vel" to="/markhor/tracks/diff_drive_controller/cmd_vel"/>
+    </node>
+
 </launch>
\ No newline at end of file
diff --git a/markhor_control/package.xml b/markhor_control/package.xml
index b07247f1..ca05067f 100644
--- a/markhor_control/package.xml
+++ b/markhor_control/package.xml
@@ -17,6 +17,7 @@
   <exec_depend>effort_controllers</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>controller_manager</exec_depend>
+  <exec_depend>twist_mux</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/markhor_navigation/launch/markhor_nav.launch b/markhor_navigation/launch/markhor_nav.launch
index d282885e..44c31caf 100644
--- a/markhor_navigation/launch/markhor_nav.launch
+++ b/markhor_navigation/launch/markhor_nav.launch
@@ -6,8 +6,8 @@
   <!-- Launch the move_base node (Navigation core) -->
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
 
-    <!-- Remap the 'cmd_vel' and 'odom' topics to the appropriate topics for the robot -->
-    <remap from="cmd_vel" to="/markhor/tracks/diff_drive_controller/cmd_vel" />
+    <!-- Remap the 'cmd_vel' and 'odom' topics to the appropriate topics for the robot (multiplexer) -->
+    <remap from="cmd_vel" to="/move_base/cmd_vel"/>
     <remap from="odom" to="/markhor/tracks/diff_drive_controller/odom" />
 
     <!-- Set the global and local planners using the previously defined arguments -->

From fb6ab7254318a9cb914b4e63b935c0af3c7e8e95 Mon Sep 17 00:00:00 2001
From: SimonR99 <simonroy99@hotmail.ca>
Date: Wed, 14 Jun 2023 09:14:36 +0200
Subject: [PATCH 2/2] working mux

---
 markhor_bringup/config/twist_mux.yaml         | 21 ++++++++++---------
 .../launch/teleop_twist_joy.launch            |  2 +-
 2 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/markhor_bringup/config/twist_mux.yaml b/markhor_bringup/config/twist_mux.yaml
index 70148be8..d9617462 100644
--- a/markhor_bringup/config/twist_mux.yaml
+++ b/markhor_bringup/config/twist_mux.yaml
@@ -1,10 +1,11 @@
-twist_mux:
-  topics:
-    - name    : joy_cmd_vel
-      topic   : /markhor/joy/cmd_vel
-      timeout : 0.5
-      priority: 2
-    - name    : move_base_cmd_vel
-      topic   : /move_base/cmd_vel
-      timeout : 0.5
-      priority: 1
+topics:
+-
+  name    : navigation
+  topic   : /move_base/cmd_vel
+  timeout : 1 # Timeout in seconds before switching to lower priority topic
+  priority: 10 # Lower priority than joystick
+-
+  name    : joystick
+  topic   : /markhor/joy/cmd_vel
+  timeout : 1 # Timeout in seconds before switching to lower priority topic
+  priority: 100  # Higher priority than navigation
diff --git a/markhor_bringup/launch/teleop_twist_joy.launch b/markhor_bringup/launch/teleop_twist_joy.launch
index 17ce68d7..bdebb0c3 100644
--- a/markhor_bringup/launch/teleop_twist_joy.launch
+++ b/markhor_bringup/launch/teleop_twist_joy.launch
@@ -11,7 +11,7 @@
     <!-- Multiplexer to prioritize teleop commands over autonomous commands -->
     <node pkg="twist_mux" type="twist_mux" name="twist_mux" respawn="false">
         <rosparam file="$(find markhor_bringup)/config/twist_mux.yaml" command="load" />
-        <remap from="cmd_vel" to="/markhor/tracks/diff_drive_controller/cmd_vel"/>
+        <remap from="/cmd_vel_out" to="/markhor/tracks/diff_drive_controller/cmd_vel"/>
     </node>
 
 </launch>
\ No newline at end of file