update

CMakeLists.txt

@@ -95,6 +95,11 @@ install(FILES README.md LICENSE.md
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
+# Install configuration files
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  FILES_MATCHING PATTERN "*.yaml"
+)
 ################################################################################
 # Test
 ################################################################################

config/twist_mux.yaml

@@ -1,19 +1,28 @@
-twist_mux:
-  ros__parameters:
-    topics:
-      # common:
-      #   topic   : cmd_vel_common
-      #   timeout : 0.5
-      #   priority: 0
-      navigation:
-        topic   : cmd_vel
+# Input topics handled/muxed.
+# For each topic:
+# - name    : name identifier to select the topic
+# - topic   : input topic of geometry_msgs::Twist type
+# - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead
+# - priority: priority in the range [0, 255]; the higher the more priority over other topics
+
+topics:
+-
+  name    : navigation
+  topic   : nav_vel
   timeout : 0.5
-        priority: 10
-      # tracker:
-      #   topic   : cmd_vel_tracker
-      #   timeout : 0.5
-      #   priority: 20
-      joystick:
-        topic   : cmd_vel_joy
+  priority: 100
+-
+  name    : joystick
+  topic   : joy_vel
   timeout : 0.5
-        priority: 50
+  priority: 90
+-
+  name    : keyboard
+  topic   : key_vel
+  timeout : 0.5
+  priority: 90
+-
+  name    : tablet
+  topic   : tab_vel
+  timeout : 0.5
+  priority: 90

launch/twist_mux.launch

@@ -0,0 +1,30 @@
+<launch>
+  <arg name="joy_vel_in"  default="input_joy/cmd_vel"/>
+  <arg name="joy_vel_out" default="joy_vel"/>
+
+  <arg name="cmd_vel_out" default="twist_mux/cmd_vel"/>
+
+  <arg name="config_locks"  default="$(find twist_mux)/config/twist_mux_locks.yaml"/>
+  <arg name="config_topics" default="$(find cps_rmp220_Support)/config/twist_mux_topics.yaml"/>
+
+  <arg name="config_joy" default="$(find twist_mux)/config/joystick.yaml"/>
+
+  <node pkg="twist_mux" type="twist_mux" name="twist_mux" output="screen">
+    <remap from="cmd_vel_out" to="$(arg cmd_vel_out)"/>
+
+    <rosparam file="$(arg config_locks)"  command="load"/>
+    <rosparam file="$(arg config_topics)" command="load"/>
+  </node>
+
+  <node pkg="twist_mux" type="twist_marker" name="twist_marker">
+    <remap from="twist"  to="$(arg cmd_vel_out)"/>
+    <remap from="marker" to="twist_marker"/>
+  </node>
+
+  <node pkg="twist_mux" type="joystick_relay.py" name="joystick_relay" output="screen">
+    <remap from="joy_vel_in"  to="$(arg joy_vel_in)"/>
+    <remap from="joy_vel_out" to="$(arg joy_vel_out)"/>
+
+    <rosparam file="$(arg config_joy)" command="load"/>
+  </node>
+</launch>