Kart commander

src/amp_kart_bringup/README.md

@@ -4,36 +4,25 @@ Contains files needed to start the kart.
 
 ## Mux structure
 
-The muxes used are [`twist_mux`](http://wiki.ros.org/twist_mux).
-They are laid out as can be seen below.
+There are two control sources that both produce `twist_mux` messages to `cmd_vel`: joy_vel (teleop controller) and nav_vel (autnomous navigation).
+The RCS provides some track states (safe stop, estop, teleop, auto) that control which input we select. The kart_commander uses these states to determine which control source to let through, or neither.
+
+The priorities are implemented with `switch_mux`, a node defined in `kart_commander`
 
 ```
-                   /joy_disable      /joy_enable
-                  |                 |
-                  |                 |   /stop
-                 ____               |  |
-                |    |  /mux1_vel   ____
-   /joy_vel ____|    |_____________|    |
-                |    |             |    |
-                |____|             |    |____ /cmd_vel
-                 mux1              |    |
-                       /nav_vel ___|    |
-                                   |____|
-                                    mux2
+              /topic_select
+                   |
+                   |
+                  _|___
+     /joy_vel____|     |
+                 |     |____ /cmd_vel
+                 |     |
+                switch_mux
+                 |     |
+     /nav_vel ___|     |
+                 |_____|
 ```
 
-Priorities for the muxes are listed below, as well as the corresponding topic
-and (expected) topic publishers:
-
-- `mux1` → `/mux1_vel`
-  - `joy_disable` ~ `kart_commander`: 150
-  - `joy_vel` ~ `teleop_twist_joy_node`: 100
-- `mux2` → `/cmd_vel`
-  - `stop` ~ `kart_commander`: 255
-  - `joy_enable` ~ `kart_commander`: 50
-  - `nav_vel` ~ `nav2`: 10
-  - `mux1_vel` ~ `mux1`: 100
-
 ## Directory Tree
 
 - `config/`
@@ -48,7 +37,7 @@ and (expected) topic publishers:
   - `rviz.launch.py` _Initialize RViz with Nav2 configuration_
   - `slam.launch.py` _Initialize map_server and slam_toolbox_
   - `teleop.launch.py` _Launch joy teleop nodes_
-  - `twist_mux.launch.py` _Launch both twist muxex_
+  - `kart_commander.launch.py` _Launch kart_commander and the twist muxes_
   - `VLP16.launch.py` _Initialize Velodyne LIDAR VLP16 nodes_
   - `zed.launch.py` _Zed camera launch file_
 - `map/`
@@ -58,7 +47,6 @@ and (expected) topic publishers:
   - `VLP16db.params.yaml` _VLP16 calibration parameters_
   - `nav2.params.yaml` _Nav2 parameters_
   - `patchworkpp.params.yaml` _Patchwork++ parameters_
-  - `twist_mux_1.params.yaml`
-  - `twist_mux_2.params.yaml`
+  - `switch_mux.params.yaml`
 - `rviz/`
   - `nav2_default_view.rviz.yaml` _RViz config for Nav2_

src/amp_kart_bringup/launch/kart_commander.launch.py

@@ -0,0 +1,31 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    params_file = os.path.join(get_package_share_directory('amp_kart_bringup'),
+                               'params', 'switch_mux.params.yaml')
+
+    switch_mux_action = Node(package='amp_kart_commander',
+                             namespace='',
+                             executable='switch_mux',
+                             name='switch_mux',
+                             parameters=[params_file])
+
+    kart_commander_action = Node(package='amp_kart_commander',
+                                 namespace='',
+                                 executable='kart_commander',
+                                 name='kart_commander')
+
+    ld = LaunchDescription()
+    ld.add_action(switch_mux_action)
+    ld.add_action(kart_commander_action)
+
+    return ld

src/amp_kart_bringup/params/switch_mux.params.yaml

@@ -0,0 +1,6 @@
+switch_mux:
+  ros__parameters:
+    input_topics:
+      - joy_vel
+      - nav_vel
+    initial_topic: __none

src/amp_kart_commander/README.md

@@ -0,0 +1,24 @@
+# Kart Commander
+
+Kart Commander hosts a Service Server that listens for TrackState changes from the RCS node and manipulates the switch_mux accordingly.
+
+Might consider moving the mux diagram from `bringup/launch/twist_mux` here. There's more elaboration on this node in particular there. Somebody fix the rest,
+
+Note that while you can reach `Stop` from any other state, returning to `Autonomous` from `Stop` is not allowed; switch to RC first.
+
+## `switch_mux`
+
+An one-in, one-out mux for `Twist` messages, since ROS2 does not support generic message types in Foxy.
+Publishes to a `Twist` topic named "cmd_vel" and takes in a configurable list of `Twist` topics
+
+- Parameters
+  - `initial_topic`: (optional) name of the initial `Twist` topic to pass through
+  - `input_topics`: list of `Twist` topics to listen for
+- Subscribed Topics
+  - `select_topic`: `String` name of topic from `input_topics` to pass through, or `__none`, ehich explicitly turns off the mux
+- Published Topics
+  - `cmd_vel`: Twist topic output chosen from the inputs, or nothing at all
+
+## Tests
+
+See the README in the test folder

src/amp_kart_commander/amp_kart_commander/kart_commander.py

@@ -0,0 +1,62 @@
+'''Server that receives updates to TrackState from RCS and sets the kart cmd_vel mux accordingly'''
+
+#usr/bin/env python3
+from amp_msgs.srv import TrackState
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy
+
+# Create high-reliability QoS for mux toggling
+qos_profile = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,  # Last always has priority anyways
+    reliability=ReliabilityPolicy.RELIABLE,  # Guaranteed delivery
+    durability=DurabilityPolicy.
+    TRANSIENT_LOCAL  # Re-send msg to late-joining subscribers
+)
+
+
+class ServiceServer(Node):
+
+    def __init__(self):
+        super().__init__('service_server')
+        self.server = self.create_service(TrackState, 'track_state',
+                                          self.track_state_callback)
+        self.mux_publisher = self.create_publisher(String,
+                                                   'select_topic',
+                                                   qos_profile=qos_profile)
+
+    def track_state_callback(self, request, response):
+        response.state = request.state
+
+        self.get_logger().info('Recieved: %s Response: %s' %
+                               (request.state, response.state))
+
+        # self.get_logger().info('eeee' + str(dir(TrackState.Request)))
+
+        # TODO More complex states in TrackState.srv have transition rules not yet implemented
+
+        if response.state == TrackState.Request.EMERGENCY or response.state == TrackState.Request.SAFESTOP:
+            self.mux_publisher.publish(String(data='__none'))
+            self.get_logger().info('STOP: nav2 and teleop disabled')
+        if response.state == TrackState.Request.RC or response.state == TrackState.Request.RACEDONE:
+            self.mux_publisher.publish(String(data='joy_vel'))
+            self.get_logger().info('RC: joy only')
+        if response.state == TrackState.Request.AUTONOMOUS_SLOW or response.state == TrackState.Request.AUTONOMOUS_OVERTAKE:
+            self.mux_publisher.publish(String(data='nav_vel'))
+            self.get_logger().info('AUTO: nav2 only')
+
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ServiceServer()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+
+if __name__ == 'main':
+    main()

src/amp_kart_commander/amp_kart_commander/switch_mux.py

@@ -0,0 +1,82 @@
+''' Mux the allows choosing a single topic to let through '''
+#usr/bin/env python3
+import rclpy
+import functools
+
+from rclpy.node import Node
+from std_msgs.msg import String
+from geometry_msgs.msg import Twist
+from rclpy.exceptions import ParameterNotDeclaredException
+
+from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy
+# Create lossy QoS that is compatible with any publisher
+subscriber_any = QoSProfile(history=HistoryPolicy.KEEP_LAST,
+                            depth=5,
+                            reliability=ReliabilityPolicy.BEST_EFFORT,
+                            durability=DurabilityPolicy.VOLATILE)
+
+# Create high-reliability QoS for topic selection
+reliable = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,  # Last always has priority anyways
+    reliability=ReliabilityPolicy.RELIABLE,  # Guaranteed delivery
+    durability=DurabilityPolicy.TRANSIENT_LOCAL  # Re-send msg to late-joiners
+)
+
+EMPTY_TOPIC = "__none"
+
+
+class SwitchMux(Node):
+
+    def __init__(self):
+        super().__init__('switch_mux', allow_undeclared_parameters=True)
+
+        self.declare_parameter('initial_topic', String)
+        self.declare_parameter('input_topics')
+
+        # `initial_topic` is optionally defined at initialization
+        try:
+            self.current_topic = self.get_parameter('initial_topic').value
+        except ParameterNotDeclaredException:
+            self.current_topic = EMPTY_TOPIC
+
+        # `input_topics` must be defined at initialization
+        self.input_topics = self._parameters['input_topics'].value
+        if self.input_topics is None:
+            raise ParameterNotDeclaredException('input_topics')
+
+        for topic_name in self.input_topics:
+            # This specific name is disallowed to guarantee that it disables the mux
+            if topic_name == EMPTY_TOPIC:
+                continue
+            # All input topics share the same base callback (input_topic_callback), but we
+            # bind a third `topic_name` parameter that changes with the topic
+            named_callback = functools.partial(self.input_topic_callback,
+                                               topic_name)
+            self.create_subscription(Twist, topic_name, named_callback,
+                                     subscriber_any)
+
+        # Topic name
+        self.subscription = self.create_subscription(String, 'select_topic',
+                                                     self.set_topic_callback,
+                                                     reliable)
+        self.publisher = self.create_publisher(Twist, 'cmd_vel', reliable)
+
+    def set_topic_callback(self, topic_name_msg):
+        self.current_topic = str(topic_name_msg.data)
+
+    def input_topic_callback(self, topic_name, twist_msg):
+        ''' Upon receiving a message to any registered input of the mux'''
+        if topic_name == self.current_topic:
+            self.publisher.publish(twist_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SwitchMux()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+
+if __name__ == 'main':
+    main()

src/amp_kart_commander/package.xml

@@ -0,0 +1,13 @@
+<?xml version="1.0" ?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>amp_kart_commander</name>
+  <version>0.0.0</version>
+  <description>Updates cmd_vel max based on TrackState changes</description>
+  <maintainer email="[email protected]">lucy</maintainer>
+  <license>TODO: License declaration</license>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/amp_kart_commander/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/amp_kart_commander
+[install]
+install_scripts=$base/lib/amp_kart_commander