diff --git a/clearpath_bt_joy/clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py b/clearpath_bt_joy/clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py index 50b8768..abd67ab 100644 --- a/clearpath_bt_joy/clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py +++ b/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}') diff --git a/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py b/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py deleted file mode 100644 index e35512a..0000000 --- a/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py +++ /dev/null @@ -1,141 +0,0 @@ -# Software License Agreement (BSD) -# -# @author Chris Iverach-Brereton -# @copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# * Neither the name of Clearpath Robotics nor the names of its contributors -# may be used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node - - -def generate_launch_description(): - # Launch Configurations - setup_path = LaunchConfiguration('setup_path') - use_sim_time = LaunchConfiguration('use_sim_time') - - arg_setup_path = DeclareLaunchArgument( - 'setup_path', - default_value='/etc/clearpath/' - ) - - arg_use_sim_time = DeclareLaunchArgument( - 'use_sim_time', - choices=['true', 'false'], - default_value='false', - description='Use simulation time' - ) - - # Paths - dir_platform_config = PathJoinSubstitution([ - setup_path, 'platform/config']) - - # Configs - config_teleop_joy = [ - dir_platform_config, - '/teleop_joy.yaml' - ] - - node_bt_cutoff = Node( - package='clearpath_bt_joy', - executable='clearpath_bt_joy_cutoff_node', - name='bt_cutoff_node', - parameters=[ - config_teleop_joy, - {'use_sim_time': use_sim_time}, - ], - remappings=[ - ('quality_ok', 'joy_teleop/quality_ok'), - ('quality', 'joy_teleop/quality'), - ], - ) - - node_joy = Node( - package='joy_linux', - executable='joy_linux_node', - output='screen', - name='joy_node', - parameters=[ - config_teleop_joy, - {'use_sim_time': use_sim_time}, - ], - remappings=[ - ('/diagnostics', 'diagnostics'), - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ('joy', 'joy_teleop/joy'), - ('joy/set_feedback', 'joy_teleop/joy/set_feedback'), - ], - ) - - node_teleop_twist_joy = Node( - package='teleop_twist_joy', - executable='teleop_node', - output='screen', - name='teleop_twist_joy_node', - parameters=[ - config_teleop_joy, - {'use_sim_time': use_sim_time}, - {'publish_stamped_twist': True}, - ], - remappings=[ - ('joy', 'joy_teleop/joy'), - ('cmd_vel', 'joy_teleop/_/cmd_vel_src'), - ] - ) - - node_twist_mux = Node( - package='twist_mux', - executable='twist_mux', - output='screen', - remappings={ - ('cmd_vel_out', 'joy_teleop/cmd_vel'), - ('/diagnostics', 'diagnostics'), - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - }, - parameters=[ - {'use_sim_time': use_sim_time}, - {'use_stamped': True}, - {'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.timeout': 0.0}, - {'locks.bt_quality.priority': 255}, - ] - ) - - - # Create launch description and add actions - ld = LaunchDescription() - ld.add_action(arg_setup_path) - ld.add_action(arg_use_sim_time) - ld.add_action(node_bt_cutoff) - ld.add_action(node_joy) - ld.add_action(node_teleop_twist_joy) - ld.add_action(node_twist_mux) - return ld diff --git a/clearpath_bt_joy/setup.py b/clearpath_bt_joy/setup.py index 7308f5f..e2fd91d 100644 --- a/clearpath_bt_joy/setup.py +++ b/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', diff --git a/clearpath_control/launch/teleop_joy.launch.py b/clearpath_control/launch/teleop_joy.launch.py index e35512a..b690e66 100644 --- a/clearpath_control/launch/teleop_joy.launch.py +++ b/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}, ]