Update .pre-commit-config.yaml (backport #2805)

.codespell_words

@@ -5,6 +5,7 @@ ba
 dof
 dur
 iff
+lins
 keyserver
 nto
 ot

.pre-commit-config.yaml

@@ -15,7 +15,11 @@
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
+<<<<<<< HEAD
     rev: v3.4.0
+=======
+    rev: v5.0.0
+>>>>>>> 130a7ecb8 (Update pre-commit-config (#2805))
     hooks:
       - id: check-added-large-files
       - id: check-case-conflict
@@ -33,7 +37,11 @@ repos:
       - id: trailing-whitespace
 
   - repo: https://github.com/psf/black
+<<<<<<< HEAD
     rev: 22.3.0
+=======
+    rev: 25.1.0
+>>>>>>> 130a7ecb8 (Update pre-commit-config (#2805))
     hooks:
       - id: black
 
@@ -47,8 +55,25 @@ repos:
         files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
         args: ['-fallback-style=none', '-i']
   - repo: https://github.com/codespell-project/codespell
+<<<<<<< HEAD
     rev: v2.0.0
+=======
+    rev: v2.4.1
+>>>>>>> 130a7ecb8 (Update pre-commit-config (#2805))
     hooks:
       - id: codespell
         args: ['--write-changes', '--ignore-words=.codespell_words']
         exclude: CHANGELOG.rst
+<<<<<<< HEAD
+=======
+
+  - repo: https://github.com/cheshirekow/cmake-format-precommit
+    rev: v0.6.13
+    hooks:
+      - id: cmake-format
+      - id: cmake-lint
+        args: [--line-width  220,
+               --disabled-codes=C0307,
+               --suppress-decorations
+              ]
+>>>>>>> 130a7ecb8 (Update pre-commit-config (#2805))

moveit/scripts/create_readme_table.py

@@ -62,9 +62,9 @@ def define_urls(target, params):
         )
         params["url"] = "{base_url}/view/{R}src_u{U}/job/{job}".format(**params)
     elif target == "bin":
-        params[
-            "job"
-        ] = "{R}bin_u{U}64__{package}__ubuntu_{ubuntu}_amd64__binary".format(**params)
+        params["job"] = (
+            "{R}bin_u{U}64__{package}__ubuntu_{ubuntu}_amd64__binary".format(**params)
+        )
         params["url"] = "{base_url}/view/{R}bin_u{U}64/job/{job}".format(**params)
 
 

moveit_configs_utils/moveit_configs_utils/substitutions/xacro.py

@@ -67,8 +67,8 @@ def perform(self, context: LaunchContext) -> Text:
         for (key, value) in self.__mappings.items():
             normalized_key = normalize_to_list_of_substitutions(key)
             normalized_value = normalize_to_list_of_substitutions(value)
-            expanded_mappings[
-                perform_substitutions(context, normalized_key)
-            ] = perform_substitutions(context, normalized_value)
+            expanded_mappings[perform_substitutions(context, normalized_key)] = (
+                perform_substitutions(context, normalized_value)
+            )
 
         return load_xacro(Path(expanded_file_path), mappings=expanded_mappings)

moveit_core/version/version.cmake

@@ -0,0 +1,38 @@
+# Retrieve (active) branch name
+execute_process(
+  COMMAND git rev-parse --abbrev-ref HEAD
+  WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+  OUTPUT_VARIABLE MOVEIT_GIT_NAME
+  OUTPUT_STRIP_TRAILING_WHITESPACE ERROR_QUIET)
+
+if("${MOVEIT_GIT_NAME}" STREQUAL "HEAD")
+  # Retrieve any associated name (tag or branch)
+  execute_process(
+    COMMAND git describe --contains --all HEAD
+    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+    OUTPUT_VARIABLE MOVEIT_GIT_NAME
+    OUTPUT_STRIP_TRAILING_WHITESPACE ERROR_QUIET)
+endif()
+
+# Retrieve (short) commit hash
+execute_process(
+  COMMAND git rev-parse --short HEAD
+  WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+  OUTPUT_VARIABLE MOVEIT_GIT_COMMIT_HASH
+  OUTPUT_STRIP_TRAILING_WHITESPACE ERROR_QUIET)
+
+string(REGEX REPLACE "^([0-9]+)\\..*" "\\1" MOVEIT_VERSION_MAJOR
+                     "${moveit_core_VERSION}")
+string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_MINOR
+                     "${moveit_core_VERSION}")
+string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_PATCH
+                     "${moveit_core_VERSION}")
+set(MOVEIT_VERSION
+    "${MOVEIT_VERSION_MAJOR}.${MOVEIT_VERSION_MINOR}.${MOVEIT_VERSION_PATCH}")
+
+if(NOT "${MOVEIT_VERSION_EXTRA}" STREQUAL "")
+  string(APPEND MOVEIT_VERSION "-${MOVEIT_VERSION_EXTRA}")
+endif()
+
+configure_file("version.hpp.in"
+               "${VERSION_FILE_PATH}/moveit_core/moveit/version.hpp")

moveit_py/moveit/servo_client/devices/__init__.py

@@ -0,0 +1 @@
+"""Teleoperation device implementations."""

moveit_py/moveit/servo_client/devices/ps4_dualshock.py

@@ -0,0 +1,144 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2022, Peter David Fagan
+# 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 Willow Garage, Inc. 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 OWNER 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.
+#
+# Author: Peter David Fagan
+"""PS4 dualshock teleop device implementation."""
+
+import rclpy
+from multiprocessing import Process
+
+from geometry_msgs.msg import TwistStamped
+from sensor_msgs.msg import Joy
+from std_srvs.srv import Trigger
+
+from dataclasses import dataclass
+from moveit.servo_client.teleop import TeleopDevice
+
+
+###############################################################################
+# DualShock4 Device Mappings
+###############################################################################
+
+
+@dataclass
+class DualShockAxes:
+    LEFT_STICK_X: int = 0
+    LEFT_STICK_Y: int = 1
+    LEFT_TRIGGER: int = 2
+    RIGHT_STICK_X: int = 3
+    RIGHT_STICK_Y: int = 4
+    RIGHT_TRIGGER: int = 5
+    D_PAD_X: int = 6
+    D_PAD_Y: int = 7
+
+
+@dataclass
+class DualShockButtons:
+    X: int = 0
+    O: int = 1
+    TRIANGLE: int = 2
+    SQUARE: int = 3
+    L1: int = 4
+    R1: int = 5
+    L2: int = 6
+    R2: int = 7
+    SHARE: int = 8
+    OPTIONS: int = 9
+    HOME: int = 10
+    LEFT_STICK_TRIGGER: int = 11
+    RIGHT_STICK_TRIGGER: int = 12
+
+
+@dataclass
+class PS4DualShock:
+    """
+    A dataclass to store device config. This class follows the conventions of Joy message (http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Joy.html)
+    """
+
+    Axes: DualShockAxes = DualShockAxes()
+
+    Buttons: DualShockButtons = DualShockButtons()
+
+
+###############################################################################
+# DualShock4 Device Teleop
+###############################################################################
+
+
+class PS4DualShockTeleop(TeleopDevice):
+    "A class which encapsulates teleoperation functionalities for ps4 dualshock device."
+
+    def __init__(
+        self,
+        ee_frame_name: str,
+        node_name: str = "ps4_dualshock_teleop",
+        device_name: str = "ps4_dualshock",
+        device_config: PS4DualShock = PS4DualShock(),
+    ):
+        super().__init__(
+            node_name=node_name,
+            device_name=device_name,
+            device_config=device_config,
+            ee_frame_name=ee_frame_name,
+        )
+        self.logger = rclpy.logging.get_logger("ps4_dualshock_teleop")
+
+    def publish_command(self, data):
+        """
+        Publishes the teleop command.
+        """
+        try:
+            # convert joy data to twist command
+            twist = TwistStamped()
+            twist.twist.linear.z = data.axes[self.device_config.Axes.RIGHT_STICK_Y]
+            twist.twist.linear.y = data.axes[self.device_config.Axes.RIGHT_STICK_X]
+
+            lin_x_right = -0.5 * (data.axes[self.device_config.Axes.RIGHT_TRIGGER])
+            lin_x_left = 0.5 * (data.axes[self.device_config.Axes.LEFT_TRIGGER])
+            twist.twist.linear.x = lin_x_right + lin_x_left
+
+            twist.twist.angular.y = data.axes[self.device_config.Axes.LEFT_STICK_Y]
+            twist.twist.angular.x = data.axes[self.device_config.Axes.LEFT_STICK_X]
+
+            roll_positive = data.buttons[self.device_config.Buttons.R1]
+            roll_negative = -1 * data.buttons[self.device_config.Buttons.L1]
+            twist.twist.angular.z = float(roll_positive + roll_negative)
+
+            twist.header.frame_id = self.ee_frame_name
+            twist.header.stamp = self.get_clock().now().to_msg()
+            self.twist_publisher.publish(twist)
+        except Exception as e:
+            self.logger.info.error(e)
+            print(e)
+
+    def record():
+        pass

moveit_py/moveit/servo_client/teleop.py

@@ -0,0 +1,118 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2022, Peter David Fagan
+# 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 Willow Garage, Inc. 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 OWNER 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.
+#
+# Author: Peter David Fagan
+"""Definition of an abstract base class for device teleoperation."""
+
+import rclpy
+
+from abc import ABC, abstractmethod
+import threading
+from rclpy.node import Node
+
+# messages/services
+from geometry_msgs.msg import TwistStamped
+from sensor_msgs.msg import Joy
+from std_srvs.srv import Trigger
+
+
+class TeleopDevice(ABC, Node):
+    """
+    An abstract base class for a teleoperation device.
+
+    This class expects both the `publish_command` and `record` methods to be overridden by inheriting classes.
+    """
+
+    def __init__(self, node_name, device_name, device_config, ee_frame_name):
+        super().__init__(node_name=node_name)
+        self.device_name = device_name
+        self.device_config = device_config
+        self.ee_frame_name = ee_frame_name
+
+        # default subscriber and publisher setup
+        self.joy_subscriber = self.create_subscription(
+            Joy, "/joy", self.publish_command, 10
+        )
+        self.twist_publisher = self.create_publisher(
+            TwistStamped, "/servo_node/delta_twist_cmds", 10
+        )
+        self.servo_node_start_client = self.create_client(
+            Trigger, "/servo_node/start_servo"
+        )
+        self.servo_node_stop_client = self.create_client(
+            Trigger, "/servo_node/stop_servo"
+        )
+
+        self.teleop_thread = None  # gets created when teleop starts
+
+    def start_teleop(self):
+        """
+        Starts servo client and teleop process.
+        """
+        try:
+            # start servo client
+            self.servo_node_start_client.wait_for_service(10.0)
+            self.servo_node_start_client.call_async(Trigger.Request())
+
+            # spin the teleop node
+            # use multithreaded because Servo has concurrent processes for moving the robot and avoiding collisions
+            ex = rclpy.executors.MultiThreadedExecutor()
+            ex.add_node(self)
+            self.teleop_thread = threading.Thread(target=ex.spin, daemon=True)
+            self.teleop_thread.start()
+        except Exception as e:
+            print(e)
+
+    def stop_teleop(self):
+        """
+        Stops servo client and teleop process.
+        """
+        try:
+            self.servo_node_stop_client.wait_for_service(10.0)
+            self.servo_node_stop_client.call_async(Trigger.Request())
+            self.teleop_thread.join()
+        except Exception as e:
+            print(e)
+
+    @abstractmethod
+    def publish_command(self):
+        """
+        Publishes the teleop command.
+        """
+        pass
+
+    @abstractmethod
+    def record(self):
+        """
+        Records trajectory data.
+        """
+        pass