From e969ad761ad3deb9b80e1dd64fac804a98da206d Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 16 Dec 2024 10:47:58 -0500 Subject: [PATCH 01/21] First attempt at an overhaul to the joy controller interface. Turns out the HCI RSSI feedback latches when the controller goes out of range, so it's useless as a metric. --- clearpath_bt_joy/LICENSE | 28 ++++ clearpath_bt_joy/README.md | 7 + clearpath_bt_joy/clearpath_bt_joy/__init__.py | 0 .../clearpath_bt_joy_cutoff_node.py | 154 ++++++++++++++++++ .../launch/clearpath_bt_joy.launch.py | 141 ++++++++++++++++ clearpath_bt_joy/package.xml | 21 +++ clearpath_bt_joy/resource/clearpath_bt_joy | 0 clearpath_bt_joy/setup.cfg | 19 +++ clearpath_bt_joy/setup.py | 65 ++++++++ clearpath_control/config/a200/teleop_ps4.yaml | 3 + 10 files changed, 438 insertions(+) create mode 100644 clearpath_bt_joy/LICENSE create mode 100644 clearpath_bt_joy/README.md create mode 100644 clearpath_bt_joy/clearpath_bt_joy/__init__.py create mode 100644 clearpath_bt_joy/clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py create mode 100644 clearpath_bt_joy/launch/clearpath_bt_joy.launch.py create mode 100644 clearpath_bt_joy/package.xml create mode 100644 clearpath_bt_joy/resource/clearpath_bt_joy create mode 100644 clearpath_bt_joy/setup.cfg create mode 100644 clearpath_bt_joy/setup.py diff --git a/clearpath_bt_joy/LICENSE b/clearpath_bt_joy/LICENSE new file mode 100644 index 00000000..62bd137d --- /dev/null +++ b/clearpath_bt_joy/LICENSE @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2024, clearpathrobotics + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. 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. + +3. Neither the name of the copyright holder 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. diff --git a/clearpath_bt_joy/README.md b/clearpath_bt_joy/README.md new file mode 100644 index 00000000..f7663ebe --- /dev/null +++ b/clearpath_bt_joy/README.md @@ -0,0 +1,7 @@ +# clearpath_bt_joy + +Monitors a bluetooth joy controller's RSSI and cuts it off if the signal strength drops +below a given threshold. + +This provides a safety mechanism by which controllers that go out-of-range and latch their +last-received input will not force the robot into an uncontrollable state. \ No newline at end of file diff --git a/clearpath_bt_joy/clearpath_bt_joy/__init__.py b/clearpath_bt_joy/clearpath_bt_joy/__init__.py new file mode 100644 index 00000000..e69de29b 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 new file mode 100644 index 00000000..d49da242 --- /dev/null +++ b/clearpath_bt_joy/clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py @@ -0,0 +1,154 @@ +# 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. +import os +import subprocess + +import rclpy +from rclpy.node import Node +from rcl_interfaces.srv import GetParameters +from std_msgs.msg import Bool, Int32 + + +class RssiCutoffNode(Node): + """ + Cuts off joy input if the controller RSSI is too low. + + Monitors the RSSI of a joy device and publishes 2 topics: + - rssi (std_msgs/Int32) -- the raw RSSI reading for the device + - rssi_ok (std_msgs/Bool) -- is the RSSI strong enough to accept the joy inputs? + """ + + def __init__(self): + super().__init__('rssi_cutoff_node') + + self.declare_parameter('rssi_cutoff', -80) + self.rssi_cutoff = self.get_parameter('rssi_cutoff').value + + # Create our publishers + self.rssi_ok_pub = self.create_publisher(Bool, 'rssi_ok', 10) + self.rssi_pub = self.create_publisher(Int32, 'rssi', 10) + + # Get the 'dev' parameter from the joy_node to determine what device we're using + cli = self.create_client(GetParameters, 'joy_node/get_parameters') + cli.wait_for_service() + req = GetParameters.Request() + req.names = ['dev'] + future = cli.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + self.joy_device = future.result().values[0].string_value + else: + self.get_logger().warn('Unable to determine joy device') + self.joy_device = None + + self.mac_addr = self.get_mac() + + if self.mac_addr is not None: + self.rssi_timer = self.create_timer(0.1, self.check_rssi) + else: + self.get_logger().warn(f'Unable to determine MAC address for {self.joy_device}') + + def get_mac(self): + if self.joy_device is None: + return None + + # wait until the joy device appears on the local file system + rate = self.create_rate(1) + while not os.path.exists(self.joy_device): + rate.sleep() + + udev_proc = subprocess.Popen( + [ + 'udevadm', + 'info', + '--attribute-walk', + self.joy_device, + ], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + grep_proc = subprocess.Popen( + [ + 'grep', + 'ATTRS{uniq}==' + ], + stdin=udev_proc.stdout, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + result = grep_proc.communicate() + if result[0] is not None: + try: + return result[0].decode().strip().split('==')[1].replace('"', '') + except Exception as err: + self.get_logger().warning(f'Failed to read MAC address: {err}') + return None + else: + self.get_logger().warning('Failed to read MAC address: no output') + return None + + def check_rssi(self): + hcitool_proc = subprocess.Popen( + [ + 'hcitool', + 'rssi', + self.mac_addr + ], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + try: + result = hcitool_proc.communicate() + stdout = result[0].decode().strip() + stderr = result[1].decode().strip() + + rssi_ok = Bool() + rssi_level = Int32() + if 'not connected' in stderr.lower(): + rssi_ok.data = False + rssi_level.data = -1_000_000 # arbitrarily huge to indicate no connection + else: + rssi_level.data = int(stdout.split(':')[-1].strip()) + rssi_ok.data = rssi_level.data >= self.rssi_cutoff + + self.rssi_ok_pub.publish(rssi_ok) + self.rssi_pub.publish(rssi_level) + except Exception as err: + self.get_logger().warning(f'Failed to read RSSI: {err}') + + +def main(): + rclpy.init() + node = RssiCutoffNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py b/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py new file mode 100644 index 00000000..b9f68bfa --- /dev/null +++ b/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py @@ -0,0 +1,141 @@ +# 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='rssi_cutoff_node', + parameters=[ + config_teleop_joy, + {'use_sim_time': use_sim_time}, + ], + remappings=[ + ('rssi_ok', 'joy_teleop/rssi_ok'), + ('rssi', 'joy_teleop/rssi'), + ], + ) + + 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.rssi.topic': 'joy_teleop/rssi_ok'}, + {'locks.rssi.timeout': 0.0}, + {'locks.rssi.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/package.xml b/clearpath_bt_joy/package.xml new file mode 100644 index 00000000..bf7ddab9 --- /dev/null +++ b/clearpath_bt_joy/package.xml @@ -0,0 +1,21 @@ + + + + clearpath_bt_joy + 0.3.4 + Clearpath Bluetooth Joy controller driver + Chris Iverach-Brereton + BSD-3 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + joy_linux + twist_mux + + + ament_python + + diff --git a/clearpath_bt_joy/resource/clearpath_bt_joy b/clearpath_bt_joy/resource/clearpath_bt_joy new file mode 100644 index 00000000..e69de29b diff --git a/clearpath_bt_joy/setup.cfg b/clearpath_bt_joy/setup.cfg new file mode 100644 index 00000000..8f1bfd96 --- /dev/null +++ b/clearpath_bt_joy/setup.cfg @@ -0,0 +1,19 @@ +[metadata] +name=clearpath_bt_joy +version = attr: clearpath_bt_joy.__version__ +author = Chris Iverach-Brereton +author_email = civerachb@clearpathrobotics.com +maintainer = Chris Iverach-Brereton +maintainer_email = civerachb@clearpathrobotics.com +description = Clearpath bluetooth joy node + +[options] +install_requires = + requests + importlib-metadata; python_version < "3.8" + +[develop] +script_dir=$base/lib/clearpath_bt_joy + +[install] +install_scripts=$base/lib/clearpath_bt_joy diff --git a/clearpath_bt_joy/setup.py b/clearpath_bt_joy/setup.py new file mode 100644 index 00000000..7c18fde7 --- /dev/null +++ b/clearpath_bt_joy/setup.py @@ -0,0 +1,65 @@ +# 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 glob import glob +import os + +from setuptools import setup + + +package_name = 'clearpath_bt_joy' + +setup( + name=package_name, + version='0.3.4', + packages=[ + package_name, + ], + data_files=[ + # Install marker file in the package index + ('share/ament_index/resource_index/packages', + ['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', + ], + zip_safe=True, + maintainer='Chris Iverach-Brereton', + maintainer_email='civerachb@clearpathrobotics.com', + description='Clearpath joy controller RSSI monitor', + license='BSD-3', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'clearpath_bt_joy_cutoff_node = clearpath_bt_joy.clearpath_bt_joy_cutoff_node:main' + ], + }, +) diff --git a/clearpath_control/config/a200/teleop_ps4.yaml b/clearpath_control/config/a200/teleop_ps4.yaml index 80acc0af..306499df 100644 --- a/clearpath_control/config/a200/teleop_ps4.yaml +++ b/clearpath_control/config/a200/teleop_ps4.yaml @@ -72,3 +72,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +rssi_cutoff_node: + ros__parameters: + rssi_cutoff: -60 \ No newline at end of file From eaabc0eea55110a283a3c44d3878887dc6de0837 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 16 Dec 2024 12:01:43 -0500 Subject: [PATCH 02/21] Use link quality instead of RSSI. It appears to respond faster, and has a more predictable range of values --- clearpath_bt_joy/README.md | 2 +- .../clearpath_bt_joy_cutoff_node.py | 46 +++++++++---------- .../launch/clearpath_bt_joy.launch.py | 12 ++--- clearpath_bt_joy/setup.py | 2 +- clearpath_control/config/a200/teleop_ps4.yaml | 4 +- 5 files changed, 33 insertions(+), 33 deletions(-) diff --git a/clearpath_bt_joy/README.md b/clearpath_bt_joy/README.md index f7663ebe..dd05c8f3 100644 --- a/clearpath_bt_joy/README.md +++ b/clearpath_bt_joy/README.md @@ -1,6 +1,6 @@ # clearpath_bt_joy -Monitors a bluetooth joy controller's RSSI and cuts it off if the signal strength drops +Monitors a bluetooth joy controller's link quality and cuts it off if the signal strength drops below a given threshold. This provides a safety mechanism by which controllers that go out-of-range and latch their 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 d49da242..efb35337 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 @@ -34,24 +34,24 @@ from std_msgs.msg import Bool, Int32 -class RssiCutoffNode(Node): +class qualityCutoffNode(Node): """ - Cuts off joy input if the controller RSSI is too low. + Cuts off joy input if the controller link quality is too low. - Monitors the RSSI of a joy device and publishes 2 topics: - - rssi (std_msgs/Int32) -- the raw RSSI reading for the device - - rssi_ok (std_msgs/Bool) -- is the RSSI strong enough to accept the joy inputs? + Monitors the quality of a joy device and publishes 2 topics: + - quality (std_msgs/Int32) -- the raw link quality for the connection + - quality_ok (std_msgs/Bool) -- is the quality good enough to accept the joy inputs? """ def __init__(self): - super().__init__('rssi_cutoff_node') + super().__init__('bt_cutoff_node') - self.declare_parameter('rssi_cutoff', -80) - self.rssi_cutoff = self.get_parameter('rssi_cutoff').value + self.declare_parameter('quality_cutoff', -80) + self.quality_cutoff = self.get_parameter('quality_cutoff').value # Create our publishers - self.rssi_ok_pub = self.create_publisher(Bool, 'rssi_ok', 10) - self.rssi_pub = self.create_publisher(Int32, 'rssi', 10) + self.quality_ok_pub = self.create_publisher(Bool, 'quality_ok', 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 cli = self.create_client(GetParameters, 'joy_node/get_parameters') @@ -69,7 +69,7 @@ def __init__(self): self.mac_addr = self.get_mac() if self.mac_addr is not None: - self.rssi_timer = self.create_timer(0.1, self.check_rssi) + self.quality_timer = self.create_timer(0.1, self.check_quality) else: self.get_logger().warn(f'Unable to determine MAC address for {self.joy_device}') @@ -112,11 +112,11 @@ def get_mac(self): self.get_logger().warning('Failed to read MAC address: no output') return None - def check_rssi(self): + def check_quality(self): hcitool_proc = subprocess.Popen( [ 'hcitool', - 'rssi', + 'lq', self.mac_addr ], stdout=subprocess.PIPE, @@ -127,24 +127,24 @@ def check_rssi(self): stdout = result[0].decode().strip() stderr = result[1].decode().strip() - rssi_ok = Bool() - rssi_level = Int32() + quality_ok = Bool() + quality_level = Int32() if 'not connected' in stderr.lower(): - rssi_ok.data = False - rssi_level.data = -1_000_000 # arbitrarily huge to indicate no connection + quality_ok.data = False + quality_level.data = 0 else: - rssi_level.data = int(stdout.split(':')[-1].strip()) - rssi_ok.data = rssi_level.data >= self.rssi_cutoff + quality_level.data = int(stdout.split(':')[-1].strip()) + quality_ok.data = quality_level.data >= self.quality_cutoff - self.rssi_ok_pub.publish(rssi_ok) - self.rssi_pub.publish(rssi_level) + self.quality_ok_pub.publish(quality_ok) + self.quality_pub.publish(quality_level) except Exception as err: - self.get_logger().warning(f'Failed to read RSSI: {err}') + self.get_logger().warning(f'Failed to read quality: {err}') def main(): rclpy.init() - node = RssiCutoffNode() + node = qualityCutoffNode() rclpy.spin(node) node.destroy_node() rclpy.shutdown() diff --git a/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py b/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py index b9f68bfa..e35512ac 100644 --- a/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py +++ b/clearpath_bt_joy/launch/clearpath_bt_joy.launch.py @@ -62,14 +62,14 @@ def generate_launch_description(): node_bt_cutoff = Node( package='clearpath_bt_joy', executable='clearpath_bt_joy_cutoff_node', - name='rssi_cutoff_node', + name='bt_cutoff_node', parameters=[ config_teleop_joy, {'use_sim_time': use_sim_time}, ], remappings=[ - ('rssi_ok', 'joy_teleop/rssi_ok'), - ('rssi', 'joy_teleop/rssi'), + ('quality_ok', 'joy_teleop/quality_ok'), + ('quality', 'joy_teleop/quality'), ], ) @@ -123,9 +123,9 @@ def generate_launch_description(): {'topics.joy.topic': 'joy_teleop/_/cmd_vel_src'}, {'topics.joy.timeout': 0.5}, {'topics.joy.priority': 10}, - {'locks.rssi.topic': 'joy_teleop/rssi_ok'}, - {'locks.rssi.timeout': 0.0}, - {'locks.rssi.priority': 255}, + {'locks.bt_quality.topic': 'joy_teleop/quality_ok'}, + {'locks.bt_quality.timeout': 0.0}, + {'locks.bt_quality.priority': 255}, ] ) diff --git a/clearpath_bt_joy/setup.py b/clearpath_bt_joy/setup.py index 7c18fde7..7308f5f6 100644 --- a/clearpath_bt_joy/setup.py +++ b/clearpath_bt_joy/setup.py @@ -54,7 +54,7 @@ zip_safe=True, maintainer='Chris Iverach-Brereton', maintainer_email='civerachb@clearpathrobotics.com', - description='Clearpath joy controller RSSI monitor', + description='Clearpath joy controller link quality monitor', license='BSD-3', tests_require=['pytest'], entry_points={ diff --git a/clearpath_control/config/a200/teleop_ps4.yaml b/clearpath_control/config/a200/teleop_ps4.yaml index 306499df..75430da8 100644 --- a/clearpath_control/config/a200/teleop_ps4.yaml +++ b/clearpath_control/config/a200/teleop_ps4.yaml @@ -72,6 +72,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 -rssi_cutoff_node: +bt_cutoff_node: ros__parameters: - rssi_cutoff: -60 \ No newline at end of file + bt_cutoff: 20 \ No newline at end of file From 16543ceff8e79f9267c60bf9eda89995a6c19e98 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 16 Dec 2024 12:06:20 -0500 Subject: [PATCH 03/21] Fix the default value for the cutoff --- .../clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 efb35337..8b022107 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 @@ -46,7 +46,9 @@ class qualityCutoffNode(Node): def __init__(self): super().__init__('bt_cutoff_node') - self.declare_parameter('quality_cutoff', -80) + # link quality is a 0-255 value, as reported by hcitool lq + # by default use a quality of 20 or less to indicate a poor connection + self.declare_parameter('quality_cutoff', 20) self.quality_cutoff = self.get_parameter('quality_cutoff').value # Create our publishers From 1908a7f3c5277a5a2fd5271888fd088c70f26cc4 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 16 Dec 2024 12:11:53 -0500 Subject: [PATCH 04/21] Fix case of class name --- .../clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 8b022107..50b87686 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 @@ -34,7 +34,7 @@ from std_msgs.msg import Bool, Int32 -class qualityCutoffNode(Node): +class QualityCutoffNode(Node): """ Cuts off joy input if the controller link quality is too low. @@ -146,7 +146,7 @@ def check_quality(self): def main(): rclpy.init() - node = qualityCutoffNode() + node = QualityCutoffNode() rclpy.spin(node) node.destroy_node() rclpy.shutdown() From 428fd0dca8698d4fc4858301a1a7c9d821534dbd Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 16 Dec 2024 12:25:09 -0500 Subject: [PATCH 05/21] Update description, add bluez dependency --- clearpath_bt_joy/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/clearpath_bt_joy/package.xml b/clearpath_bt_joy/package.xml index bf7ddab9..e87d51b6 100644 --- a/clearpath_bt_joy/package.xml +++ b/clearpath_bt_joy/package.xml @@ -3,7 +3,7 @@ clearpath_bt_joy 0.3.4 - Clearpath Bluetooth Joy controller driver + Clearpath bluetooth joy controller signal quality monitoring node Chris Iverach-Brereton BSD-3 @@ -12,6 +12,7 @@ ament_pep257 python3-pytest + bluez joy_linux twist_mux From c4fedd45177838a2aa6158d8f0c700dcd6673106 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 16 Dec 2024 12:45:20 -0500 Subject: [PATCH 06/21] Add the quality_cutoff parameter to all of the PS4 configuration files --- clearpath_control/config/a200/teleop_ps4.yaml | 2 +- clearpath_control/config/a300/teleop_ps4.yaml | 3 +++ clearpath_control/config/dd100/teleop_ps4.yaml | 3 +++ clearpath_control/config/dd150/teleop_ps4.yaml | 3 +++ clearpath_control/config/do100/teleop_ps4.yaml | 3 +++ clearpath_control/config/do150/teleop_ps4.yaml | 3 +++ clearpath_control/config/generic/teleop_ps4.yaml | 3 +++ clearpath_control/config/j100/teleop_ps4.yaml | 3 +++ clearpath_control/config/r100/teleop_ps4.yaml | 3 +++ clearpath_control/config/w200/teleop_ps4.yaml | 3 +++ 10 files changed, 28 insertions(+), 1 deletion(-) diff --git a/clearpath_control/config/a200/teleop_ps4.yaml b/clearpath_control/config/a200/teleop_ps4.yaml index 75430da8..24280d69 100644 --- a/clearpath_control/config/a200/teleop_ps4.yaml +++ b/clearpath_control/config/a200/teleop_ps4.yaml @@ -74,4 +74,4 @@ joy_node: dev: /dev/input/ps4 bt_cutoff_node: ros__parameters: - bt_cutoff: 20 \ No newline at end of file + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/a300/teleop_ps4.yaml b/clearpath_control/config/a300/teleop_ps4.yaml index d108f14d..dbc0c926 100644 --- a/clearpath_control/config/a300/teleop_ps4.yaml +++ b/clearpath_control/config/a300/teleop_ps4.yaml @@ -72,3 +72,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/dd100/teleop_ps4.yaml b/clearpath_control/config/dd100/teleop_ps4.yaml index 47aaa145..e2b5e2b9 100644 --- a/clearpath_control/config/dd100/teleop_ps4.yaml +++ b/clearpath_control/config/dd100/teleop_ps4.yaml @@ -72,3 +72,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/dd150/teleop_ps4.yaml b/clearpath_control/config/dd150/teleop_ps4.yaml index 47aaa145..e2b5e2b9 100644 --- a/clearpath_control/config/dd150/teleop_ps4.yaml +++ b/clearpath_control/config/dd150/teleop_ps4.yaml @@ -72,3 +72,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/do100/teleop_ps4.yaml b/clearpath_control/config/do100/teleop_ps4.yaml index b51a3457..5c9f80f6 100644 --- a/clearpath_control/config/do100/teleop_ps4.yaml +++ b/clearpath_control/config/do100/teleop_ps4.yaml @@ -75,3 +75,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/do150/teleop_ps4.yaml b/clearpath_control/config/do150/teleop_ps4.yaml index dc362f0c..4f2e9327 100644 --- a/clearpath_control/config/do150/teleop_ps4.yaml +++ b/clearpath_control/config/do150/teleop_ps4.yaml @@ -75,3 +75,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/generic/teleop_ps4.yaml b/clearpath_control/config/generic/teleop_ps4.yaml index 80acc0af..24280d69 100644 --- a/clearpath_control/config/generic/teleop_ps4.yaml +++ b/clearpath_control/config/generic/teleop_ps4.yaml @@ -72,3 +72,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/j100/teleop_ps4.yaml b/clearpath_control/config/j100/teleop_ps4.yaml index ddc6a318..5b13cec2 100644 --- a/clearpath_control/config/j100/teleop_ps4.yaml +++ b/clearpath_control/config/j100/teleop_ps4.yaml @@ -72,3 +72,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/r100/teleop_ps4.yaml b/clearpath_control/config/r100/teleop_ps4.yaml index 2c20d692..edaa766a 100644 --- a/clearpath_control/config/r100/teleop_ps4.yaml +++ b/clearpath_control/config/r100/teleop_ps4.yaml @@ -75,3 +75,6 @@ joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/w200/teleop_ps4.yaml b/clearpath_control/config/w200/teleop_ps4.yaml index eec07b8d..a26761eb 100644 --- a/clearpath_control/config/w200/teleop_ps4.yaml +++ b/clearpath_control/config/w200/teleop_ps4.yaml @@ -78,3 +78,6 @@ joy_teleop/joy_node: deadzone: 0.1 autorepeat_rate: 20.0 dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 From a2ffddf85a55a99c17a70522a993d6eef9965ad9 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 16 Dec 2024 12:45:42 -0500 Subject: [PATCH 07/21] Update the launch file to start the new cutoff node and additional twist_mux --- clearpath_control/launch/teleop_joy.launch.py | 62 +++++++++++++++---- 1 file changed, 49 insertions(+), 13 deletions(-) diff --git a/clearpath_control/launch/teleop_joy.launch.py b/clearpath_control/launch/teleop_joy.launch.py index 1db04334..e35512ac 100644 --- a/clearpath_control/launch/teleop_joy.launch.py +++ b/clearpath_control/launch/teleop_joy.launch.py @@ -1,9 +1,7 @@ -#!/usr/bin/env python3 - # Software License Agreement (BSD) # -# @author Roni Kreinin -# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# @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: @@ -28,10 +26,6 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -# Redistribution and use in source and binary forms, with or without -# modification, is not permitted without the express permission -# of Clearpath Robotics. - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution @@ -43,7 +37,6 @@ def generate_launch_description(): setup_path = LaunchConfiguration('setup_path') use_sim_time = LaunchConfiguration('use_sim_time') - # Launch Arguments arg_setup_path = DeclareLaunchArgument( 'setup_path', default_value='/etc/clearpath/' @@ -66,6 +59,20 @@ def generate_launch_description(): '/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', @@ -73,14 +80,15 @@ def generate_launch_description(): name='joy_node', parameters=[ config_teleop_joy, - {'use_sim_time': use_sim_time}], + {'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( @@ -90,16 +98,44 @@ def generate_launch_description(): name='teleop_twist_joy_node', parameters=[ config_teleop_joy, - {'use_sim_time': use_sim_time}], + {'use_sim_time': use_sim_time}, + {'publish_stamped_twist': True}, + ], remappings=[ ('joy', 'joy_teleop/joy'), - ('cmd_vel', 'joy_teleop/cmd_vel'), + ('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 From e0f22f0695195059c74c1f85cb3ee21cc65c1c5c Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Tue, 17 Dec 2024 11:30:09 -0500 Subject: [PATCH 08/21] Add udev rules for various controllers --- clearpath_bt_joy/debian/udev | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 clearpath_bt_joy/debian/udev diff --git a/clearpath_bt_joy/debian/udev b/clearpath_bt_joy/debian/udev new file mode 100644 index 00000000..d3bbb343 --- /dev/null +++ b/clearpath_bt_joy/debian/udev @@ -0,0 +1,26 @@ +KERNEL=="uinput", MODE="0666" +KERNEL=="event*", MODE="0666" + + +# Sony PlayStation DualShock 4 +# USB +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="054c", ATTRS{idProduct}=="05c4", MODE="0666" +# Bluetooth +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", KERNELS=="*:054C:05C4.*", MODE="0666" + +# Sony PlayStation DualShock 4 Slim +# USB +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="054c", ATTRS{idProduct}=="09cc", MODE="0666" +# Bluetooth +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", KERNELS=="*:054C:09CC.*", MODE="0666" +KERNEL=="js*", SUBSYSTEM=="input", ATTRS{name}=="Wireless Controller", MODE="0666", SYMLINK+="input/ps4" + + +# Logitech game controllers +KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21f", SYMLINK+="input/f710" +KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c219", SYMLINK+="input/f710" +KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21d", SYMLINK+="input/f310" + + +# Xbox One controller +KERNEL=="usb*", ATTRS{idVendor}=="1d6b", ATTRS{idProduct}=="0002", MODE="0666", SYMLINK="input/xbox" \ No newline at end of file From 5eeabe6e0eae752aa8186d3c8b9aa9344a8aa949 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Tue, 17 Dec 2024 11:30:19 -0500 Subject: [PATCH 09/21] Add dependency on the new bt cutoff package --- clearpath_control/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/clearpath_control/package.xml b/clearpath_control/package.xml index 2263c8b8..712b0453 100644 --- a/clearpath_control/package.xml +++ b/clearpath_control/package.xml @@ -18,6 +18,7 @@ ament_cmake + clearpath_bt_joy controller_manager diff_drive_controller From 346651a6d931b6f164ab596940002ac1e494abd0 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Tue, 17 Dec 2024 11:46:57 -0500 Subject: [PATCH 10/21] Add xbox controller parameters for all platforms --- .../config/a200/teleop_xbox.yaml | 66 ++++++++++++++++++ .../config/a300/teleop_xbox.yaml | 66 ++++++++++++++++++ .../config/dd100/teleop_xbox.yaml | 66 ++++++++++++++++++ .../config/dd150/teleop_xbox.yaml | 66 ++++++++++++++++++ .../config/do100/teleop_xbox.yaml | 69 +++++++++++++++++++ .../config/do150/teleop_xbox.yaml | 69 +++++++++++++++++++ .../config/generic/teleop_xbox.yaml | 66 ++++++++++++++++++ .../config/j100/teleop_xbox.yaml | 66 ++++++++++++++++++ .../config/r100/teleop_xbox.yaml | 69 +++++++++++++++++++ .../config/w200/teleop_xbox.yaml | 66 ++++++++++++++++++ 10 files changed, 669 insertions(+) create mode 100644 clearpath_control/config/a200/teleop_xbox.yaml create mode 100644 clearpath_control/config/a300/teleop_xbox.yaml create mode 100644 clearpath_control/config/dd100/teleop_xbox.yaml create mode 100644 clearpath_control/config/dd150/teleop_xbox.yaml create mode 100644 clearpath_control/config/do100/teleop_xbox.yaml create mode 100644 clearpath_control/config/do150/teleop_xbox.yaml create mode 100644 clearpath_control/config/generic/teleop_xbox.yaml create mode 100644 clearpath_control/config/j100/teleop_xbox.yaml create mode 100644 clearpath_control/config/r100/teleop_xbox.yaml create mode 100644 clearpath_control/config/w200/teleop_xbox.yaml diff --git a/clearpath_control/config/a200/teleop_xbox.yaml b/clearpath_control/config/a200/teleop_xbox.yaml new file mode 100644 index 00000000..c85ad09d --- /dev/null +++ b/clearpath_control/config/a200/teleop_xbox.yaml @@ -0,0 +1,66 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 1.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.2 + enable_button: 6 + enable_turbo_button: 7 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/a300/teleop_xbox.yaml b/clearpath_control/config/a300/teleop_xbox.yaml new file mode 100644 index 00000000..8da9ad84 --- /dev/null +++ b/clearpath_control/config/a300/teleop_xbox.yaml @@ -0,0 +1,66 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.3 + scale_linear_turbo.x: 2.0 + axis_angular.yaw: 0 + scale_angular.yaw: 1.0 + scale_angular_turbo.yaw: 2.0 + enable_button: 5 + enable_turbo_button: 4 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/dd100/teleop_xbox.yaml b/clearpath_control/config/dd100/teleop_xbox.yaml new file mode 100644 index 00000000..0af64808 --- /dev/null +++ b/clearpath_control/config/dd100/teleop_xbox.yaml @@ -0,0 +1,66 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 1.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.0 + enable_button: 6 + enable_turbo_button: 7 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/dd150/teleop_xbox.yaml b/clearpath_control/config/dd150/teleop_xbox.yaml new file mode 100644 index 00000000..0af64808 --- /dev/null +++ b/clearpath_control/config/dd150/teleop_xbox.yaml @@ -0,0 +1,66 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 1.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.0 + enable_button: 6 + enable_turbo_button: 7 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/do100/teleop_xbox.yaml b/clearpath_control/config/do100/teleop_xbox.yaml new file mode 100644 index 00000000..14f14541 --- /dev/null +++ b/clearpath_control/config/do100/teleop_xbox.yaml @@ -0,0 +1,69 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + axis_linear.y: 0 + scale_linear.x: 0.4 + scale_linear.y: 0.4 + scale_linear_turbo.x: 1.0 + scale_linear_turbo.y: 1.0 + axis_angular.yaw: 3 + scale_angular.yaw: 0.5 + scale_angular_turbo.yaw: 1.0 + enable_button: 6 + enable_turbo_button: 7 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/do150/teleop_xbox.yaml b/clearpath_control/config/do150/teleop_xbox.yaml new file mode 100644 index 00000000..14f14541 --- /dev/null +++ b/clearpath_control/config/do150/teleop_xbox.yaml @@ -0,0 +1,69 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + axis_linear.y: 0 + scale_linear.x: 0.4 + scale_linear.y: 0.4 + scale_linear_turbo.x: 1.0 + scale_linear_turbo.y: 1.0 + axis_angular.yaw: 3 + scale_angular.yaw: 0.5 + scale_angular_turbo.yaw: 1.0 + enable_button: 6 + enable_turbo_button: 7 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/generic/teleop_xbox.yaml b/clearpath_control/config/generic/teleop_xbox.yaml new file mode 100644 index 00000000..c85ad09d --- /dev/null +++ b/clearpath_control/config/generic/teleop_xbox.yaml @@ -0,0 +1,66 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 1.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.2 + enable_button: 6 + enable_turbo_button: 7 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/j100/teleop_xbox.yaml b/clearpath_control/config/j100/teleop_xbox.yaml new file mode 100644 index 00000000..4e48938d --- /dev/null +++ b/clearpath_control/config/j100/teleop_xbox.yaml @@ -0,0 +1,66 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 2.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.4 + enable_button: 6 + enable_turbo_button: 7 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/r100/teleop_xbox.yaml b/clearpath_control/config/r100/teleop_xbox.yaml new file mode 100644 index 00000000..5fbf4ece --- /dev/null +++ b/clearpath_control/config/r100/teleop_xbox.yaml @@ -0,0 +1,69 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + axis_linear.y: 0 + scale_linear.x: 0.4 + scale_linear.y: 0.4 + scale_linear_turbo.x: 0.4 + scale_linear_turbo.y: 0.4 + axis_angular.yaw: 3 + scale_angular.yaw: 0.5 + scale_angular_turbo.yaw: 0.5 + enable_button: 6 + enable_turbo_button: 7 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/w200/teleop_xbox.yaml b/clearpath_control/config/w200/teleop_xbox.yaml new file mode 100644 index 00000000..be9e6a7b --- /dev/null +++ b/clearpath_control/config/w200/teleop_xbox.yaml @@ -0,0 +1,66 @@ +# Layout for Microsoft Xbox controllers +# +# (LB) (RB) +# (LT) (RT) +# _=====_ _=====_ +# / \ / \ +# / ---------------------------- \ +# / ___ (Y) \ +# / / L \ \ +# / \___/ (X) (B) \ +# | # = | +# | (A) | +# | _ ___ | +# | __| |__ / R \ | +# | |__ __| \___/ | +# | |_| | +# | /------------------------\ | +# | / \ | +# | / \ | +# \ / \ / +# \________/ \________/ +# +# BUTTON Value +# LB 6 +# RB 7 +# A 0 +# B 1 +# X 3 +# Y 4 +# L (click) 13 +# R (click) 14 +# menu (=) 11 +# window (#) unmapped + +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 2 +# Right Vert. 3 +# LT 5 +# RT 4 # yes, really, RT is 4, not 5 +# D-pad Horiz. 6 +# D-pad Vert. 7 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.5 + scale_linear_turbo.x: 1.0 + axis_angular.yaw: 0 + scale_angular.yaw: 2.4 + scale_angular_turbo.yaw: 2.4 + enable_button: 6 + enable_turbo_button: 7 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/xbox +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file From f4e294276b33aaa13431965cd049aa4bf0a4201a Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Tue, 17 Dec 2024 12:23:52 -0500 Subject: [PATCH 11/21] 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_cutoff_node.py | 10 +- .../launch/clearpath_bt_joy.launch.py | 141 ------------------ clearpath_bt_joy/setup.py | 2 - clearpath_control/launch/teleop_joy.launch.py | 5 +- 4 files changed, 8 insertions(+), 150 deletions(-) delete mode 100644 clearpath_bt_joy/launch/clearpath_bt_joy.launch.py 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 50b87686..abd67ab1 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 e35512ac..00000000 --- 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 7308f5f6..e2fd91dd 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 e35512ac..b690e668 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}, ] From 39dab6a8f633b7c7112a3ce2ddee740c23803227 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 19 Dec 2024 13:57:24 -0500 Subject: [PATCH 12/21] Fix the PS4/5 axis ordering; the left stick shows up as axis 3/4, with the l/r analogue triggers being 2 & 5. Update omni control configurations accordingly --- clearpath_control/config/a200/teleop_ps4.yaml | 8 ++++---- clearpath_control/config/a300/teleop_ps4.yaml | 8 ++++---- clearpath_control/config/dd100/teleop_ps4.yaml | 8 ++++---- clearpath_control/config/dd150/teleop_ps4.yaml | 8 ++++---- clearpath_control/config/do100/teleop_ps4.yaml | 10 +++++----- clearpath_control/config/do150/teleop_ps4.yaml | 10 +++++----- clearpath_control/config/generic/teleop_ps4.yaml | 8 ++++---- clearpath_control/config/r100/teleop_ps4.yaml | 10 +++++----- clearpath_control/config/w200/teleop_ps4.yaml | 8 ++++---- 9 files changed, 39 insertions(+), 39 deletions(-) diff --git a/clearpath_control/config/a200/teleop_ps4.yaml b/clearpath_control/config/a200/teleop_ps4.yaml index 24280d69..8ccd76a9 100644 --- a/clearpath_control/config/a200/teleop_ps4.yaml +++ b/clearpath_control/config/a200/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 diff --git a/clearpath_control/config/a300/teleop_ps4.yaml b/clearpath_control/config/a300/teleop_ps4.yaml index dbc0c926..6a5d9d6d 100644 --- a/clearpath_control/config/a300/teleop_ps4.yaml +++ b/clearpath_control/config/a300/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 diff --git a/clearpath_control/config/dd100/teleop_ps4.yaml b/clearpath_control/config/dd100/teleop_ps4.yaml index e2b5e2b9..b114d439 100644 --- a/clearpath_control/config/dd100/teleop_ps4.yaml +++ b/clearpath_control/config/dd100/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 diff --git a/clearpath_control/config/dd150/teleop_ps4.yaml b/clearpath_control/config/dd150/teleop_ps4.yaml index e2b5e2b9..b114d439 100644 --- a/clearpath_control/config/dd150/teleop_ps4.yaml +++ b/clearpath_control/config/dd150/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 diff --git a/clearpath_control/config/do100/teleop_ps4.yaml b/clearpath_control/config/do100/teleop_ps4.yaml index 5c9f80f6..2b359ab3 100644 --- a/clearpath_control/config/do100/teleop_ps4.yaml +++ b/clearpath_control/config/do100/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 @@ -64,7 +64,7 @@ teleop_twist_joy_node: scale_linear.y: 0.4 scale_linear_turbo.x: 1.0 scale_linear_turbo.y: 1.0 - axis_angular.yaw: 3 + axis_angular.yaw: 4 scale_angular.yaw: 0.5 scale_angular_turbo.yaw: 1.4 enable_button: 4 diff --git a/clearpath_control/config/do150/teleop_ps4.yaml b/clearpath_control/config/do150/teleop_ps4.yaml index 4f2e9327..11935758 100644 --- a/clearpath_control/config/do150/teleop_ps4.yaml +++ b/clearpath_control/config/do150/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 @@ -64,7 +64,7 @@ teleop_twist_joy_node: scale_linear.y: 0.4 scale_linear_turbo.x: 1.0 scale_linear_turbo.y: 1.0 - axis_angular.yaw: 3 + axis_angular.yaw: 4 scale_angular.yaw: 0.5 scale_angular_turbo.yaw: 1.0 enable_button: 4 diff --git a/clearpath_control/config/generic/teleop_ps4.yaml b/clearpath_control/config/generic/teleop_ps4.yaml index 24280d69..8ccd76a9 100644 --- a/clearpath_control/config/generic/teleop_ps4.yaml +++ b/clearpath_control/config/generic/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 diff --git a/clearpath_control/config/r100/teleop_ps4.yaml b/clearpath_control/config/r100/teleop_ps4.yaml index edaa766a..50203de5 100644 --- a/clearpath_control/config/r100/teleop_ps4.yaml +++ b/clearpath_control/config/r100/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 @@ -64,7 +64,7 @@ teleop_twist_joy_node: scale_linear.y: 0.4 scale_linear_turbo.x: 0.4 scale_linear_turbo.y: 0.4 - axis_angular.yaw: 3 + axis_angular.yaw: 4 scale_angular.yaw: 0.5 scale_angular_turbo.yaw: 0.5 enable_button: 4 diff --git a/clearpath_control/config/w200/teleop_ps4.yaml b/clearpath_control/config/w200/teleop_ps4.yaml index a26761eb..f2fb61b3 100644 --- a/clearpath_control/config/w200/teleop_ps4.yaml +++ b/clearpath_control/config/w200/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 From 848f0cafce795783e2c9c5bd6fa2761e56145eca Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 19 Dec 2024 14:00:17 -0500 Subject: [PATCH 13/21] Missed updating the Jackal configuration --- clearpath_control/config/j100/teleop_ps4.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/clearpath_control/config/j100/teleop_ps4.yaml b/clearpath_control/config/j100/teleop_ps4.yaml index 5b13cec2..1e84b7bc 100644 --- a/clearpath_control/config/j100/teleop_ps4.yaml +++ b/clearpath_control/config/j100/teleop_ps4.yaml @@ -44,10 +44,10 @@ # AXIS Value # Left Horiz. 0 # Left Vert. 1 -# Right Horiz. 2 -# Right Vert. 5 -# L2 3 -# R2 4 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 # D-pad Horiz. 9 # D-pad Vert. 10 # Accel x 7 From 0504329f9275846715d1549fa9c605eef4745798 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 19 Dec 2024 15:41:45 -0500 Subject: [PATCH 14/21] Add PS5 udev rules, config files --- clearpath_bt_joy/debian/udev | 7 +- clearpath_control/config/a200/teleop_ps5.yaml | 77 +++++++++++++++++ clearpath_control/config/a300/teleop_ps5.yaml | 77 +++++++++++++++++ .../config/dd100/teleop_ps5.yaml | 77 +++++++++++++++++ .../config/dd150/teleop_ps5.yaml | 77 +++++++++++++++++ .../config/do100/teleop_ps5.yaml | 80 ++++++++++++++++++ .../config/do150/teleop_ps5.yaml | 80 ++++++++++++++++++ .../config/generic/teleop_ps5.yaml | 77 +++++++++++++++++ clearpath_control/config/j100/teleop_ps5.yaml | 77 +++++++++++++++++ clearpath_control/config/r100/teleop_ps5.yaml | 80 ++++++++++++++++++ clearpath_control/config/w200/teleop_ps5.yaml | 83 +++++++++++++++++++ 11 files changed, 791 insertions(+), 1 deletion(-) create mode 100644 clearpath_control/config/a200/teleop_ps5.yaml create mode 100644 clearpath_control/config/a300/teleop_ps5.yaml create mode 100644 clearpath_control/config/dd100/teleop_ps5.yaml create mode 100644 clearpath_control/config/dd150/teleop_ps5.yaml create mode 100644 clearpath_control/config/do100/teleop_ps5.yaml create mode 100644 clearpath_control/config/do150/teleop_ps5.yaml create mode 100644 clearpath_control/config/generic/teleop_ps5.yaml create mode 100644 clearpath_control/config/j100/teleop_ps5.yaml create mode 100644 clearpath_control/config/r100/teleop_ps5.yaml create mode 100644 clearpath_control/config/w200/teleop_ps5.yaml diff --git a/clearpath_bt_joy/debian/udev b/clearpath_bt_joy/debian/udev index d3bbb343..6295c546 100644 --- a/clearpath_bt_joy/debian/udev +++ b/clearpath_bt_joy/debian/udev @@ -16,6 +16,11 @@ KERNEL=="hidraw*", SUBSYSTEM=="hidraw", KERNELS=="*:054C:09CC.*", MODE="0666" KERNEL=="js*", SUBSYSTEM=="input", ATTRS{name}=="Wireless Controller", MODE="0666", SYMLINK+="input/ps4" +# Sony PlayStation DualShock 5 +# Bluetooth +KERNEL=="js*", SUBSYSTEM=="input", KERNELS=="*:054C:0CE6.*", MODE="0666", SYMLINK+="input/ps5" + + # Logitech game controllers KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21f", SYMLINK+="input/f710" KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c219", SYMLINK+="input/f710" @@ -23,4 +28,4 @@ KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21d", SYMLINK+="inpu # Xbox One controller -KERNEL=="usb*", ATTRS{idVendor}=="1d6b", ATTRS{idProduct}=="0002", MODE="0666", SYMLINK="input/xbox" \ No newline at end of file +KERNEL=="js*", ATTRS{idVendor}=="1d6b", ATTRS{idProduct}=="0002", ATTRS{name}=="*Xbox*", MODE="0666", SYMLINK="input/xbox" \ No newline at end of file diff --git a/clearpath_control/config/a200/teleop_ps5.yaml b/clearpath_control/config/a200/teleop_ps5.yaml new file mode 100644 index 00000000..3cdac3f2 --- /dev/null +++ b/clearpath_control/config/a200/teleop_ps5.yaml @@ -0,0 +1,77 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 1.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.2 + enable_button: 4 + enable_turbo_button: 5 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps5 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/a300/teleop_ps5.yaml b/clearpath_control/config/a300/teleop_ps5.yaml new file mode 100644 index 00000000..cdd92cdc --- /dev/null +++ b/clearpath_control/config/a300/teleop_ps5.yaml @@ -0,0 +1,77 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +teleop_twist_joy_node: + ros__parameters: + use_sim_time: False + publish_stamped_twist: True + axis_linear.x: 1 + scale_linear.x: 0.3 + scale_linear_turbo.x: 2.0 + axis_angular.yaw: 0 + scale_angular.yaw: 1.0 + scale_angular_turbo.yaw: 2.0 + enable_button: 4 + enable_turbo_button: 5 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps5 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/dd100/teleop_ps5.yaml b/clearpath_control/config/dd100/teleop_ps5.yaml new file mode 100644 index 00000000..383c26ad --- /dev/null +++ b/clearpath_control/config/dd100/teleop_ps5.yaml @@ -0,0 +1,77 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 1.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.0 + enable_button: 4 + enable_turbo_button: 5 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps5 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/dd150/teleop_ps5.yaml b/clearpath_control/config/dd150/teleop_ps5.yaml new file mode 100644 index 00000000..383c26ad --- /dev/null +++ b/clearpath_control/config/dd150/teleop_ps5.yaml @@ -0,0 +1,77 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 1.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.0 + enable_button: 4 + enable_turbo_button: 5 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps5 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/do100/teleop_ps5.yaml b/clearpath_control/config/do100/teleop_ps5.yaml new file mode 100644 index 00000000..7b14dccc --- /dev/null +++ b/clearpath_control/config/do100/teleop_ps5.yaml @@ -0,0 +1,80 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + axis_linear.y: 0 + scale_linear.x: 0.4 + scale_linear.y: 0.4 + scale_linear_turbo.x: 1.0 + scale_linear_turbo.y: 1.0 + axis_angular.yaw: 4 + scale_angular.yaw: 0.5 + scale_angular_turbo.yaw: 1.4 + enable_button: 4 + enable_turbo_button: 5 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps5 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/do150/teleop_ps5.yaml b/clearpath_control/config/do150/teleop_ps5.yaml new file mode 100644 index 00000000..148a366a --- /dev/null +++ b/clearpath_control/config/do150/teleop_ps5.yaml @@ -0,0 +1,80 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + axis_linear.y: 0 + scale_linear.x: 0.4 + scale_linear.y: 0.4 + scale_linear_turbo.x: 1.0 + scale_linear_turbo.y: 1.0 + axis_angular.yaw: 4 + scale_angular.yaw: 0.5 + scale_angular_turbo.yaw: 1.0 + enable_button: 4 + enable_turbo_button: 5 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps5 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/generic/teleop_ps5.yaml b/clearpath_control/config/generic/teleop_ps5.yaml new file mode 100644 index 00000000..3cdac3f2 --- /dev/null +++ b/clearpath_control/config/generic/teleop_ps5.yaml @@ -0,0 +1,77 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 1.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.2 + enable_button: 4 + enable_turbo_button: 5 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps5 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 \ No newline at end of file diff --git a/clearpath_control/config/j100/teleop_ps5.yaml b/clearpath_control/config/j100/teleop_ps5.yaml new file mode 100644 index 00000000..40af924b --- /dev/null +++ b/clearpath_control/config/j100/teleop_ps5.yaml @@ -0,0 +1,77 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + scale_linear.x: 0.4 + scale_linear_turbo.x: 2.0 + axis_angular.yaw: 0 + scale_angular.yaw: 0.6 + scale_angular_turbo.yaw: 1.4 + enable_button: 4 + enable_turbo_button: 5 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/r100/teleop_ps5.yaml b/clearpath_control/config/r100/teleop_ps5.yaml new file mode 100644 index 00000000..b9749fbb --- /dev/null +++ b/clearpath_control/config/r100/teleop_ps5.yaml @@ -0,0 +1,80 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear.x: 1 + axis_linear.y: 0 + scale_linear.x: 0.4 + scale_linear.y: 0.4 + scale_linear_turbo.x: 0.4 + scale_linear_turbo.y: 0.4 + axis_angular.yaw: 4 + scale_angular.yaw: 0.5 + scale_angular_turbo.yaw: 0.5 + enable_button: 4 + enable_turbo_button: 5 +joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 diff --git a/clearpath_control/config/w200/teleop_ps5.yaml b/clearpath_control/config/w200/teleop_ps5.yaml new file mode 100644 index 00000000..90d1840c --- /dev/null +++ b/clearpath_control/config/w200/teleop_ps5.yaml @@ -0,0 +1,83 @@ +# Teleop configuration for PS5 joystick using the x-pad configuration. +# Left thumb-stick up/down for velocity, left/right for twist +# Left shoulder button for enable +# Right shoulder button for enable-turbo +# +# L1 R1 +# L2 R2 +# _=====_ _=====_ +# / _____ \ / _____ \ +# +.-'_____'-.------------------------------.-'_____'-.+ +# / | | '. S O N Y .' | _ | \ +# / ___| /|\ |___ \ / ___| /_\ |___ \ (Y) +# / | | | ; ; | _ _ || +# | | <--- ---> | | | ||_| (_)|| (X) (B) +# | |___ | ___| ; ; |___ ___|| +# |\ | \|/ | / _ ____ _ \ | (X) | /| (A) +# | \ |_____| .','" "', (_PS_) ,'" "', '. |_____| .' | +# | '-.______.-' / \ / \ '-._____.-' | +# | | LJ |--------| RJ | | +# | /\ / \ /\ | +# | / '.___.' '.___.' \ | +# | / \ | +# \ / \ / +# \________/ \_________/ +# +# ^ x +# | +# | +# y <-----+ Accelerometer axes +# \ +# \ +# > z (out) +# +# BUTTON Value +# L1 4 +# L2 6 +# R1 5 +# R2 7 +# A 1 +# B 2 +# X 0 +# Y 3 +# +# AXIS Value +# Left Horiz. 0 +# Left Vert. 1 +# Right Horiz. 3 +# Right Vert. 4 +# L2 2 +# R2 5 +# D-pad Horiz. 9 +# D-pad Vert. 10 +# Accel x 7 +# Accel y 6 +# Accel z 8 + +joy_teleop/teleop_twist_joy_node: + ros__parameters: + publish_stamped_twist: True + use_sim_time: False + axis_linear: + x: 1 + scale_linear: + x: 0.5 + scale_linear_turbo: + x: 1.0 + axis_angular: + yaw: 0 + scale_angular: + yaw: 2.4 + scale_angular_turbo: + yaw: 2.4 + enable_button: 4 + enable_turbo_button: 5 +joy_teleop/joy_node: + ros__parameters: + use_sim_time: False + deadzone: 0.1 + autorepeat_rate: 20.0 + dev: /dev/input/ps4 +bt_cutoff_node: + ros__parameters: + quality_cutoff: 20 From 507c4a8a769d42cf515f8514c3699db6e58d042d Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 19 Dec 2024 15:45:59 -0500 Subject: [PATCH 15/21] Fix devices for PS5 controllers --- clearpath_control/config/j100/teleop_ps5.yaml | 2 +- clearpath_control/config/r100/teleop_ps5.yaml | 2 +- clearpath_control/config/w200/teleop_ps5.yaml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/clearpath_control/config/j100/teleop_ps5.yaml b/clearpath_control/config/j100/teleop_ps5.yaml index 40af924b..8748faa9 100644 --- a/clearpath_control/config/j100/teleop_ps5.yaml +++ b/clearpath_control/config/j100/teleop_ps5.yaml @@ -71,7 +71,7 @@ joy_node: use_sim_time: False deadzone: 0.1 autorepeat_rate: 20.0 - dev: /dev/input/ps4 + dev: /dev/input/ps5 bt_cutoff_node: ros__parameters: quality_cutoff: 20 diff --git a/clearpath_control/config/r100/teleop_ps5.yaml b/clearpath_control/config/r100/teleop_ps5.yaml index b9749fbb..77dd18bd 100644 --- a/clearpath_control/config/r100/teleop_ps5.yaml +++ b/clearpath_control/config/r100/teleop_ps5.yaml @@ -74,7 +74,7 @@ joy_node: use_sim_time: False deadzone: 0.1 autorepeat_rate: 20.0 - dev: /dev/input/ps4 + dev: /dev/input/ps5 bt_cutoff_node: ros__parameters: quality_cutoff: 20 diff --git a/clearpath_control/config/w200/teleop_ps5.yaml b/clearpath_control/config/w200/teleop_ps5.yaml index 90d1840c..a004be44 100644 --- a/clearpath_control/config/w200/teleop_ps5.yaml +++ b/clearpath_control/config/w200/teleop_ps5.yaml @@ -77,7 +77,7 @@ joy_teleop/joy_node: use_sim_time: False deadzone: 0.1 autorepeat_rate: 20.0 - dev: /dev/input/ps4 + dev: /dev/input/ps5 bt_cutoff_node: ros__parameters: quality_cutoff: 20 From 401846961111256497825a6432768942f5487501 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 6 Jan 2025 16:55:11 -0500 Subject: [PATCH 16/21] Rename the hidden cmd_vel topic --- clearpath_control/launch/teleop_joy.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/clearpath_control/launch/teleop_joy.launch.py b/clearpath_control/launch/teleop_joy.launch.py index b690e668..9bcaee07 100644 --- a/clearpath_control/launch/teleop_joy.launch.py +++ b/clearpath_control/launch/teleop_joy.launch.py @@ -103,7 +103,7 @@ def generate_launch_description(): ], remappings=[ ('joy', 'joy_teleop/joy'), - ('cmd_vel', 'joy_teleop/_/cmd_vel_src'), + ('cmd_vel', 'joy_teleop/_cmd_vel'), ] ) @@ -121,7 +121,7 @@ def generate_launch_description(): parameters=[ {'use_sim_time': use_sim_time}, {'use_stamped': True}, - {'topics.joy.topic': 'joy_teleop/_/cmd_vel_src'}, + {'topics.joy.topic': 'joy_teleop/_cmd_vel'}, {'topics.joy.timeout': 0.5}, {'topics.joy.priority': 10}, {'locks.bt_quality.topic': 'joy_teleop/bt_quality_stop'}, From 0ee3d68f0820670d1516093586b6aacc24d0b0d1 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 6 Jan 2025 17:00:34 -0500 Subject: [PATCH 17/21] Add a timeout to the quality lock so we lock out the controller if the quality-checker node crashes --- clearpath_control/launch/teleop_joy.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_control/launch/teleop_joy.launch.py b/clearpath_control/launch/teleop_joy.launch.py index 9bcaee07..1501f975 100644 --- a/clearpath_control/launch/teleop_joy.launch.py +++ b/clearpath_control/launch/teleop_joy.launch.py @@ -125,7 +125,7 @@ def generate_launch_description(): {'topics.joy.timeout': 0.5}, {'topics.joy.priority': 10}, {'locks.bt_quality.topic': 'joy_teleop/bt_quality_stop'}, - {'locks.bt_quality.timeout': 0.0}, + {'locks.bt_quality.timeout': 1.0}, {'locks.bt_quality.priority': 255}, ] ) From a226c7b040dac37f7ec65ee68db6fe0f5046241c Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 6 Jan 2025 17:13:41 -0500 Subject: [PATCH 18/21] Set the respawn flag for the BT cutoff node and the joy_linux node --- clearpath_control/launch/teleop_joy.launch.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/clearpath_control/launch/teleop_joy.launch.py b/clearpath_control/launch/teleop_joy.launch.py index 1501f975..c353d558 100644 --- a/clearpath_control/launch/teleop_joy.launch.py +++ b/clearpath_control/launch/teleop_joy.launch.py @@ -71,6 +71,7 @@ def generate_launch_description(): ('bt_quality_stop', 'joy_teleop/bt_quality_stop'), ('quality', 'joy_teleop/quality'), ], + respawn=True, ) node_joy = Node( @@ -89,6 +90,7 @@ def generate_launch_description(): ('joy', 'joy_teleop/joy'), ('joy/set_feedback', 'joy_teleop/joy/set_feedback'), ], + respawn=True, ) node_teleop_twist_joy = Node( From cd9c8862f9d055fd2b30d174afe9c336f0df611c Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 6 Jan 2025 17:30:21 -0500 Subject: [PATCH 19/21] Update topics in docstring --- .../clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 abd67ab1..6d09d92a 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 @@ -40,7 +40,7 @@ class QualityCutoffNode(Node): Monitors the quality of a joy device and publishes 2 topics: - quality (std_msgs/Int32) -- the raw link quality for the connection - - quality_ok (std_msgs/Bool) -- is the quality good enough to accept the joy inputs? + - bt_quality_stop (std_msgs/Bool) -- has the quality dropped too low to be consiered safe? """ def __init__(self): From 1381b7496592449840d3fdf52cb3240b77d32d60 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 6 Jan 2025 17:44:11 -0500 Subject: [PATCH 20/21] Add additional logging when blocking for services & IPC. Since we've added a timeout to the lock, publish fake quality data when using wired controllers. Log when this happens --- .../clearpath_bt_joy_cutoff_node.py | 27 ++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) 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 6d09d92a..cc8eb710 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 @@ -56,16 +56,18 @@ def __init__(self): self.quality_pub = self.create_publisher(Int32, 'quality', 10) # Get the 'dev' parameter from the joy_node to determine what device we're using + self.get_logger().info('Waiting for joy_node parameter service...') cli = self.create_client(GetParameters, 'joy_node/get_parameters') cli.wait_for_service() req = GetParameters.Request() req.names = ['dev'] + self.get_logger().info('Getting joy_device parameter...') future = cli.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: self.joy_device = future.result().values[0].string_value else: - self.get_logger().warn('Unable to determine joy device') + self.get_logger().warning('Unable to determine joy device') self.joy_device = None self.mac_addr = self.get_mac() @@ -73,17 +75,20 @@ def __init__(self): if self.mac_addr is not None: self.quality_timer = self.create_timer(0.1, self.check_quality) else: - self.get_logger().warn(f'Unable to determine MAC address for {self.joy_device}') + self.get_logger().info(f'Assuming {self.joy_device} is wired; quality check will be bypassed') # noqa: E501 + self.quality_timer = self.create_timer(0.1, self.fake_quality) def get_mac(self): if self.joy_device is None: return None # wait until the joy device appears on the local file system + self.get_logger().info(f'Waiting for {self.joy_device} to appear on the local filesystem...') # noqa: E501 rate = self.create_rate(1) while not os.path.exists(self.joy_device): rate.sleep() + self.get_logger().info('Getting MAC address from udev...') udev_proc = subprocess.Popen( [ 'udevadm', @@ -106,7 +111,13 @@ def get_mac(self): result = grep_proc.communicate() if result[0] is not None: try: - return result[0].decode().strip().split('==')[1].replace('"', '') + mac = result[0].decode().strip().split('==')[1].replace('"', '') + if mac: + self.get_logger().info(f'MAC address of {self.joy_device} is {mac}') + return mac + else: + self.get_logger().warning(f'{self.joy_device} has no MAC. Is it a wired controller?') # noqa: E501 + return None except Exception as err: self.get_logger().warning(f'Failed to read MAC address: {err}') return None @@ -115,6 +126,7 @@ def get_mac(self): return None def check_quality(self): + """Check the quality of the link and publish it""" hcitool_proc = subprocess.Popen( [ 'hcitool', @@ -143,6 +155,15 @@ def check_quality(self): except Exception as err: self.get_logger().warning(f'Failed to read quality: {err}') + def fake_quality(self): + """Callback for the quality check for wired controllers to avoid locking out the mux.""" + engage_stop = Bool() + engage_stop.data = False + quality_level = Int32() + quality_level.data = 255 + + self.stop_pub.publish(engage_stop) + self.quality_pub.publish(quality_level) def main(): rclpy.init() From 4043e7eec7162908bfe34ef5c9860c3101e0ebd5 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Tue, 7 Jan 2025 13:26:24 -0500 Subject: [PATCH 21/21] Add missing newline at EOF --- .../clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 cc8eb710..64f6c405 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 @@ -174,4 +174,4 @@ def main(): if __name__ == '__main__': - main() \ No newline at end of file + main()