Replace yocs_cmd_vel_mux with twist_mux, launch and yaml files

config/default_twist_mux_locks.yaml

+##########################################################
+### LOCKS
+##########################################################
+
+# Locks to stop the twist inputs.
+# For each lock:
+# - topic   : input topic that provides the lock; it must be of type std_msgs::Bool?!!!
+# - timeout : == 0.0 -> not used
+#              > 0.0 -> the lock is supposed to published at a certain frequency in order
+#                       to detect that the publisher is alive; the timeout in seconds allows
+#                       to detect that, and if the publisher dies we will enable the lock
+# - priority: priority in the range [0, 255], so all the topics with priority lower than it
+#             will be stopped/disabled
+
+locks:
+- name    : pause
+  topic   : pause_navigation
+  timeout : 0.0
+  # Same priority as joystick control, so it'll not block it.
+  priority: 100

config/default_twist_mux_topics.yaml

+##########################################################
+### TOPICS
+##########################################################
+
+# 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    : default
+  topic   : /ana/default/cmd_vel
+  timeout : 0.1
+  priority: 0
+- name    : navigation
+  topic   : /ana/navigation/cmd_vel
+  timeout : 0.5
+  priority: 5
+- name    : teleop
+  topic   : /ana/teleop/cmd_vel
+  timeout : 0.1
+  priority: 10

launch/include/cmd_vel_mux.launch

-<?xml version="1.0"?>
-<!-- -->
-<launch>
-
-  <arg name="ns"                   default="ana"/>
-  <arg name="nodelet_manager_name" default="nodelet_manager"/>
-  <arg name="config"               default="$(find iri_rosnav)/config/default_mux.yaml"/>
-  <arg name="output"               default="log" />
-  <arg name="launch_prefix"        default="" />
-  
-  <group ns="$(arg ns)">
-
-    <node pkg ="nodelet" 
-          type="nodelet"
-          name="$(arg nodelet_manager_name)"
-          args="manager"
-          output="$(arg output)"
-          launch-prefix="$(arg launch_prefix)"/>
-
-    <node pkg ="nodelet"
-          type="nodelet"
-          name="cmd_vel_mux"
-          args="load yocs_cmd_vel_mux/CmdVelMuxNodelet $(arg nodelet_manager_name)"
-          output="$(arg output)"
-          launch-prefix="$(arg launch_prefix)">
-      <param name="yaml_cfg_file" value="$(arg config)"/>
-    </node>
-  
-  </group>
-
-</launch>

launch/include/twist_mux.launch

+<?xml version="1.0"?>
+<!-- -->
+<launch>
+  <arg name="ns"          default="robot"/>
+  <arg name="node_name"   default="twist_mux"/>
+  <arg name="output"      default="log" />
+  <arg name="launch_prefix"  default="" />
+
+  <arg name="cmd_vel_out"   default="/$(arg ns)/cmd_vel"/>
+  <arg name="config_locks"  default="$(find iri_rosnav)/config/default_twist_mux_locks.yaml"/>
+  <arg name="config_topics" default="$(find iri_rosnav)/config/default_twist_mux_topics.yaml"/>
+
+  <group ns="$(arg ns)">
+
+    <node pkg="twist_mux"
+          type="twist_mux"
+          name="$(arg node_name)"
+          launch-prefix="$(arg launch_prefix)"
+          output="$(arg output)">
+      <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>
+
+  </group>
+
+</launch>

package.xml

@@ -52,7 +52,7 @@
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <exec_depend>amcl</exec_depend>
-  <exec_depend>yocs_cmd_vel_mux</exec_depend>
+  <exec_depend>twist_mux</exec_depend>
   <exec_depend>fake_localization</exec_depend>
   <exec_depend>gmapping</exec_depend>
   <exec_depend>map_server</exec_depend>