Improve joy telep support

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.

clearpath_bt_joy/README.md

@@ -0,0 +1,7 @@
+# clearpath_bt_joy
+
+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
+last-received input will not force the robot into an uncontrollable state.

clearpath_bt_joy/clearpath_bt_joy/clearpath_bt_joy_cutoff_node.py

@@ -0,0 +1,177 @@
+# Software License Agreement (BSD)
+#
+# @author    Chris Iverach-Brereton <[email protected]>
+# @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 QualityCutoffNode(Node):
+    """
+    Cuts off joy input if the controller link quality is too low.
+
+    Monitors the quality of a joy device and publishes 2 topics:
+        - quality (std_msgs/Int32) -- the raw link quality for the connection
+        - bt_quality_stop  (std_msgs/Bool) -- has the quality dropped too low to be consiered safe?
+    """
+
+    def __init__(self):
+        super().__init__('bt_cutoff_node')
+
+        # 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
+        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
+        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().warning('Unable to determine joy device')
+            self.joy_device = None
+
+        self.mac_addr = self.get_mac()
+
+        if self.mac_addr is not None:
+            self.quality_timer = self.create_timer(0.1, self.check_quality)
+        else:
+            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',
+                '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:
+                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
+        else:
+            self.get_logger().warning('Failed to read MAC address: no output')
+            return None
+
+    def check_quality(self):
+        """Check the quality of the link and publish it"""
+        hcitool_proc = subprocess.Popen(
+            [
+                'hcitool',
+                'lq',
+                self.mac_addr
+            ],
+            stdout=subprocess.PIPE,
+            stderr=subprocess.PIPE,
+        )
+        try:
+            result = hcitool_proc.communicate()
+            stdout = result[0].decode().strip()
+            stderr = result[1].decode().strip()
+
+            engage_stop = Bool()
+            quality_level = Int32()
+            if 'not connected' in stderr.lower():
+                engage_stop.data = True
+                quality_level.data = 0
+            else:
+                quality_level.data = int(stdout.split(':')[-1].strip())
+                engage_stop.data = quality_level.data <= self.quality_cutoff
+
+            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}')
+
+    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()
+    node = QualityCutoffNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

clearpath_bt_joy/debian/udev

@@ -0,0 +1,31 @@
+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"
+
+
+# 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"
+KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21d", SYMLINK+="input/f310"
+
+
+# Xbox One controller
+KERNEL=="js*", ATTRS{idVendor}=="1d6b", ATTRS{idProduct}=="0002", ATTRS{name}=="*Xbox*", MODE="0666", SYMLINK="input/xbox"

clearpath_bt_joy/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>clearpath_bt_joy</name>
+  <version>0.3.4</version>
+  <description>Clearpath bluetooth joy controller signal quality monitoring node</description>
+  <maintainer email="[email protected]">Chris Iverach-Brereton</maintainer>
+  <license>BSD-3</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <exec_depend>bluez</exec_depend>
+  <exec_depend>joy_linux</exec_depend>
+  <exec_depend>twist_mux</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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 = [email protected]
+maintainer = Chris Iverach-Brereton
+maintainer_email = [email protected]
+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

clearpath_bt_joy/setup.py

@@ -0,0 +1,63 @@
+# Software License Agreement (BSD)
+#
+# @author    Chris Iverach-Brereton <[email protected]>
+# @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']),
+    ],
+    install_requires=[
+        'setuptools',
+    ],
+    zip_safe=True,
+    maintainer='Chris Iverach-Brereton',
+    maintainer_email='[email protected]',
+    description='Clearpath joy controller link quality 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'
+        ],
+    },
+)

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
@@ -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