Refactor the cutoff node to publish a stop topic that goes true when …

…the quality is low. Remove the (unused) launch file from the cutoff package. Update topic remappings in the teleop launch file

clearpath_bt_joy/clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py

@@ -52,7 +52,7 @@ def __init__(self):
         self.quality_cutoff = self.get_parameter('quality_cutoff').value
 
         # Create our publishers
-        self.quality_ok_pub = self.create_publisher(Bool, 'quality_ok', 10)
+        self.stop_pub = self.create_publisher(Bool, 'bt_quality_stop', 10)
         self.quality_pub = self.create_publisher(Int32, 'quality', 10)
 
         # Get the 'dev' parameter from the joy_node to determine what device we're using
@@ -129,16 +129,16 @@ def check_quality(self):
             stdout = result[0].decode().strip()
             stderr = result[1].decode().strip()
 
-            quality_ok = Bool()
+            engage_stop = Bool()
             quality_level = Int32()
             if 'not connected' in stderr.lower():
-                quality_ok.data = False
+                engage_stop.data = True
                 quality_level.data = 0
             else:
                 quality_level.data = int(stdout.split(':')[-1].strip())
-                quality_ok.data = quality_level.data >= self.quality_cutoff
+                engage_stop.data = quality_level.data <= self.quality_cutoff
 
-            self.quality_ok_pub.publish(quality_ok)
+            self.stop_pub.publish(engage_stop)
             self.quality_pub.publish(quality_level)
         except Exception as err:
             self.get_logger().warning(f'Failed to read quality: {err}')

clearpath_bt_joy/setup.py

@@ -45,8 +45,6 @@
             ['resource/' + package_name]),
         # Include the package.xml file
         (os.path.join('share', package_name), ['package.xml']),
-        (os.path.join('share', package_name, 'launch'),
-            glob(os.path.join('launch', '*.launch.py'))),
     ],
     install_requires=[
         'setuptools',

clearpath_control/launch/teleop_joy.launch.py

@@ -68,7 +68,7 @@ def generate_launch_description():
             {'use_sim_time': use_sim_time},
         ],
         remappings=[
-            ('quality_ok', 'joy_teleop/quality_ok'),
+            ('bt_quality_stop', 'joy_teleop/bt_quality_stop'),
             ('quality', 'joy_teleop/quality'),
         ],
     )
@@ -111,6 +111,7 @@ def generate_launch_description():
         package='twist_mux',
         executable='twist_mux',
         output='screen',
+        name='teleop_cutoff_mux',
         remappings={
             ('cmd_vel_out', 'joy_teleop/cmd_vel'),
             ('/diagnostics', 'diagnostics'),
@@ -123,7 +124,7 @@ def generate_launch_description():
             {'topics.joy.topic': 'joy_teleop/_/cmd_vel_src'},
             {'topics.joy.timeout': 0.5},
             {'topics.joy.priority': 10},
-            {'locks.bt_quality.topic': 'joy_teleop/quality_ok'},
+            {'locks.bt_quality.topic': 'joy_teleop/bt_quality_stop'},
             {'locks.bt_quality.timeout': 0.0},
             {'locks.bt_quality.priority': 255},
         ]