From a15c0853a1caa4d8fc74c22d2940b632213bac65 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Wed, 21 Feb 2024 14:55:17 -0500 Subject: [PATCH 1/4] Merged in ROS2 version so the code works with ROS1 and ROS2 --- CMakeLists.txt | 56 ++++---- package.xml | 17 ++- src/crtk/__init__.py | 11 +- src/crtk/measured_cp.py | 19 +-- src/crtk/{ral.py => ral_ros1.py} | 0 src/crtk/ral_ros2.py | 238 +++++++++++++++++++++++++++++++ 6 files changed, 288 insertions(+), 53 deletions(-) rename src/crtk/{ral.py => ral_ros1.py} (100%) create mode 100644 src/crtk/ral_ros2.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 2cc9613..3129802 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,31 +1,29 @@ -cmake_minimum_required (VERSION 3.1) +cmake_minimum_required (VERSION 3.10) project (crtk_python_client VERSION 1.2.0) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package (catkin REQUIRED COMPONENTS - rospy - ) - -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package ( -# INCLUDE_DIRS include -# LIBRARIES my_pkg - CATKIN_DEPENDS rospy -# DEPENDS system_lib -) - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -catkin_python_setup () +# first test for ROS1 +find_package (catkin QUIET + COMPONENTS rospy) + +if (catkin_FOUND) + + catkin_package (CATKIN_DEPENDS rospy) + catkin_python_setup () + +else (catkin_FOUND) + + # look for ROS2 + find_package (ament_cmake QUIET) + if (ament_cmake_FOUND) + find_package (ament_cmake_python REQUIRED) + ament_python_install_package (crtk + PACKAGE_DIR ${crtk_python_client_SOURCE_DIR}/src/crtk) + + file (GLOB crtk_SCRIPTS scripts/*.py) + install (PROGRAMS ${crtk_SCRIPTS} + DESTINATION lib/${PROJECT_NAME}) + + ament_package () + endif (ament_cmake_FOUND) + +endif (catkin_FOUND) diff --git a/package.xml b/package.xml index 3c7ff3b..9628348 100644 --- a/package.xml +++ b/package.xml @@ -1,19 +1,24 @@ - + crtk_python_client 1.2.0 crtk Python client - Anton Deguet Adnan Munawar MIT - catkin - rospy - rospy + catkin + rospy + rospy + crtk_msgs + + ament_cmake + ament_cmake_python + rclpy + crtk_msgs - + ament_cmake diff --git a/src/crtk/__init__.py b/src/crtk/__init__.py index 092f1df..92446ef 100755 --- a/src/crtk/__init__.py +++ b/src/crtk/__init__.py @@ -4,10 +4,17 @@ # Copyright (c) 2016-2023 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute # Released under MIT License -__all__ = ["ral", "wait_move_handle", "utils", "joystick_button", "measured_cp"] +__all__ = ['wait_move_handle', 'utils', 'joystick_button', 'measured_cp'] # ros abstraction layer -from .ral import ral +import os +__ros_version_string = os.environ['ROS_VERSION'] +if __ros_version_string == '1': + __all__.append('ros_ral1') + from .ral_ros1 import ral +elif __ros_version_string == '2': + __all__.append('ros_ral2') + from .ral_ros2 import ral # handle classes from .wait_move_handle import wait_move_handle diff --git a/src/crtk/measured_cp.py b/src/crtk/measured_cp.py index da96b5f..a628188 100644 --- a/src/crtk/measured_cp.py +++ b/src/crtk/measured_cp.py @@ -12,34 +12,21 @@ # --- end cisst license --- import crtk -import rospy -import PyKDL class measured_cp(object): """Simple class to get measured_cp over ROS """ # initialize the arm - def __init__(self, ros_namespace, expected_interval = 0.01): - # base class constructor in separate method so it can be called in derived classes - self.__init_arm(ros_namespace, expected_interval) - - - def __init_arm(self,ros_namespace, expected_interval): + def __init__(self, ral, expected_interval = 0.01): """Constructor. This initializes a few data members. It requires a ros namespace, this will be used to find the ROS topic `measured_cp`.""" # data members, event based - self.__ros_namespace = ros_namespace + self.__ral = ral # crtk features - self.__crtk_utils = crtk.utils(self, self.__ros_namespace, expected_interval) + self.__crtk_utils = crtk.utils(self, self.__ral, expected_interval) # add crtk features that we need self.__crtk_utils.add_measured_cp() - - # create node - if not rospy.get_node_uri(): - rospy.init_node('measured_cp_client', anonymous = True, log_level = rospy.WARN) - else: - rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized') diff --git a/src/crtk/ral.py b/src/crtk/ral_ros1.py similarity index 100% rename from src/crtk/ral.py rename to src/crtk/ral_ros1.py diff --git a/src/crtk/ral_ros2.py b/src/crtk/ral_ros2.py new file mode 100644 index 0000000..5ceb236 --- /dev/null +++ b/src/crtk/ral_ros2.py @@ -0,0 +1,238 @@ +# Author(s): Anton Deguet +# Created on: 2023-05-08 +# +# Copyright (c) 2023 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute +# Released under MIT License + +import rclpy +import rclpy.publisher +import rclpy.qos +import rclpy.subscription +import rclpy.time +import rclpy.utilities +import threading + + +class ral: + """RAL: ROS abstraction layer + + Provides many common parts of rospy/rclpy via a common API, to allow CRTK code to + be written at a higher level, and to work for both ROS 1 and ROS 2. + """ + + @staticmethod + def ros_version(): + return 2 + + @staticmethod + def parse_argv(argv): + # strip ros arguments + rclpy.init(args = argv) + return rclpy.utilities.remove_ros_args(argv) + + def __init__(self, node_name, namespace = '', node = None): + self._node_name = node_name + self._namespace = namespace.strip('/') + + self._children = {} + self._publishers = [] + self._subscribers = [] + self._services = [] + + self._executor_thread = None + + if node is not None: + self._node = node + else: + # initializes rclpy only if it hasn't been already (e.g. via parse_argv()), + # otherwise does nothing (since rclpy will raise a RuntimeError) + try: + rclpy.init() + except: + pass + # ros init node + self._node = rclpy.create_node(self.node_name()) + + def __del__(self): + for pub in self._publishers: + self._node.destroy_publisher(pub) + + for sub in self._subscribers: + self._node.destroy_subscription(sub) + + def node_name(self): + return self._node_name + + def namespace(self): + return self._namespace + + def create_child(self, child_namespace): + if child_namespace in self._children: + err_msg = 'ral object already has child "{}"!'.format(child_namespace) + raise RuntimeError(err_msg) + + full_child_namespace = self._add_namespace_to(child_namespace) + child = ral(self.node_name(), full_child_namespace, self._node) + self._children[child_namespace] = child + return child + + def now(self): + clock = self._node.get_clock() + return clock.now() + + def get_timestamp(self, t): + if hasattr(t, 'header'): + t = t.header + if hasattr(t, 'stamp'): + t = t.stamp + + try: + return rclpy.time.Time.from_msg(t) + except: + return t + + def to_sec(self, t): + t = self.get_timestamp(t) + return float(t.nanoseconds)/1e9 + + def create_duration(self, d): + return rclpy.time.Duration(seconds = d) + + def create_rate(self, rate_hz): + return self._node.create_rate(rate_hz) + + def spin(self): + if self._executor_thread != None: + return + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(self._node) + self._executor_thread = threading.Thread(target = executor.spin, daemon = True) + self._executor_thread.start() + + def shutdown(self): + rclpy.shutdown() + if self._executor_thread != None: + self._executor_thread.join() + self._executor_thread = None + + def spin_and_execute(self, function, *arguments): + self.spin() + try: + function(*arguments) + except KeyboardInterrupt: + pass + self.shutdown() + + def on_shutdown(self, callback): + rclpy.get_default_context().on_shutdown(callback) + + def is_shutdown(self): + return not rclpy.ok() + + def _add_namespace_to(self, name): + name = name.strip('/') + qualified_name = '{}/{}'.format(self.namespace(), name) + return qualified_name + + def publisher(self, topic, msg_type, queue_size = 10, latch = False, *args, **kwargs): + history = rclpy.qos.HistoryPolicy.KEEP_LAST + qos = rclpy.qos.QoSProfile(depth = queue_size, history = history) + if latch: + # publisher will retain queue_size messages for future subscribers + qos.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL + pub = self._node.create_publisher(msg_type, self._add_namespace_to(topic), qos) + self._publishers.append(pub) + return pub + + def subscriber(self, topic, msg_type, callback, queue_size=10, latch = False, *args, **kwargs): + history = rclpy.qos.HistoryPolicy.KEEP_LAST + qos = rclpy.qos.QoSProfile(depth = queue_size, history = history) + if latch: + # subscriber should get past messages + qos.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL + sub = self._node.create_subscription(msg_type, self._add_namespace_to(topic), + callback, qos, *args, **kwargs) + self._subscribers.append(sub) + return sub + + def service_client(self, name, srv_type): + client = self._node.create_client(srv_type, self._add_namespace_to(name)) + if not hasattr(client, '__call__'): + client.__call__ = client.call + self._services.append(client) + return client + + def get_topic_name(self, pubsub): + return pubsub.topic_name + + def _check_connections(self, start_time, timeout_duration, check_children): + def connected(pubsub): + if isinstance(pubsub, rclpy.publisher.Publisher): + # get_subscription_count() available in >= ROS 2 Dashing + return pubsub.get_subscription_count() > 0 + elif isinstance(pubsub, rclpy.subscription.Subscription): + try: + return pubsub.get_publisher_count() > 0 + except AttributeError: + # get_publisher_count() not available until ROS 2 Iron Irwini + return True + else: + raise TypeError("pubsub must be an rclpy Publisher or Subscription") + + check_rate = self.create_rate(100) + + # wait at most timeout_seconds for all connections to establish + while (self.now() - start_time) < timeout_duration: + pubsubs = self._publishers + self._subscribers + unconnected = [ps for ps in pubsubs if not connected(ps)] + if len(unconnected) == 0: + break + + check_rate.sleep() + + if check_children: + # recursively check connections in all child ral objects, + # using same start time so the timeout period doesn't restart + for _, child in self._children.items(): + child._check_connections(start_time, timeout_duration, check_children) + + # last check of connection status, raise error if any remain unconnected + unconnected_pubs = [p for p in self._publishers if not connected(p)] + unconnected_subs = [s for s in self._subscribers if not connected(s)] + if len(unconnected_pubs) == 0 and len(unconnected_subs) == 0: + return + + connected_pub_count = len(self._publishers) - len(unconnected_pubs) + connected_sub_count = len(self._subscribers) - len(unconnected_subs) + unconnected_pub_names = ', '.join([self.get_topic_name(p) for p in unconnected_pubs]) + unconnected_sub_names = ', '.join([self.get_topic_name(s) for s in unconnected_subs]) + + err_msg = \ + ( + 'Timed out waiting for publisher/subscriber connections to establish\n' + ' Publishers: {} connected,' + ' {} not connected\n' + ' not connected: [{}]\n\n' + ' Subscribers: {} connected,' + ' {} not connected\n' + ' not connected: [{}]\n\n' + ) + err_msg = err_msg.format(connected_pub_count, len(unconnected_pubs), unconnected_pub_names, + connected_sub_count, len(unconnected_subs), unconnected_sub_names) + raise TimeoutError(err_msg) + + def check_connections(self, timeout_seconds = 5.0, check_children = True): + """Check that all publishers are connected to at least one subscriber, + and that all subscribers are connected to at least on publisher. + + If timeout_seconds is zero, no checks will be done. + If check_children is True, all children will be checked as well. + """ + + if timeout_seconds == 0.0: + return + + start_time = self.now() + timeout_duration = self.create_duration(timeout_seconds) + + self._check_connections(start_time, timeout_duration, check_children) From 9b6d6c850495770eeaf88183f1b3e61d85235bc6 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Thu, 22 Feb 2024 12:58:11 -0500 Subject: [PATCH 2/4] Minor fixes for ROS2 --- __init__.py | 0 scripts/crtk_teleop_example.py | 1 - src/crtk/utils.py | 10 ++++++++-- src/crtk/wait_move_handle.py | 2 +- 4 files changed, 9 insertions(+), 4 deletions(-) delete mode 100644 __init__.py diff --git a/__init__.py b/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/scripts/crtk_teleop_example.py b/scripts/crtk_teleop_example.py index 792b382..c23f21f 100755 --- a/scripts/crtk_teleop_example.py +++ b/scripts/crtk_teleop_example.py @@ -20,7 +20,6 @@ import crtk import math import PyKDL -import rospy import sys diff --git a/src/crtk/utils.py b/src/crtk/utils.py index f38c18f..6e4484e 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -841,7 +841,10 @@ def add_move_cp(self): # internal methods for forward_kinematics def __forward_kinematics(self, jp, extra = None): # convert to ROS msg and publish - request = crtk_msgs.srv.QueryForwardKinematicsRequest() + if self.__ral.ros_version() == 1: + request = crtk_msgs.srv.QueryForwardKinematicsRequest() + else: + request = crtk_msgs.srv.QueryForwardKinematics.Request() request.jp = jp.tolist() response = self.__forward_kinematics_service.call(request) if not extra: @@ -864,7 +867,10 @@ def add_forward_kinematics(self): # internal methods for inverse_kinematics def __inverse_kinematics(self, jp, cp, extra = None): # convert to ROS msg and publish - request = crtk_msgs.srv.QueryInverseKinematicsRequest() + if self.__ral.ros_version() == 1: + request = crtk_msgs.srv.QueryInverseKinematicsRequest() + else: + request = crtk_msgs.srv.QueryInverseKinematics.Request() request.jp = jp.tolist() request.cp = msg_conv.FrameToPoseMsg(cp) response = self.__inverse_kinematics_service.call(request) diff --git a/src/crtk/wait_move_handle.py b/src/crtk/wait_move_handle.py index 58bb705..b66407f 100644 --- a/src/crtk/wait_move_handle.py +++ b/src/crtk/wait_move_handle.py @@ -1,7 +1,7 @@ # Author(s): Anton Deguet # Created on: 2020-11-23 # -# Copyright (c) 2020-2021 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute +# Copyright (c) 2020-2022 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute # Released under MIT License # Given a class with CRTK operating state, waits for busy/not busy conditions From 7fe12e3658c5e9f0bcb339004fe2dadfb8126ba5 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Thu, 11 Apr 2024 18:13:30 -0400 Subject: [PATCH 3/4] package.xml: added explicit build type for ROS 1 catkin for ROS melodic --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 9628348..86582ef 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ crtk_msgs + catkin ament_cmake From 1dd492c9702859a65694d8c1846e714eb956ac6d Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Sat, 31 Aug 2024 11:15:29 -0400 Subject: [PATCH 4/4] rc-1.3.0 --- CHANGELOG.md | 5 +++++ CMakeLists.txt | 2 +- package.xml | 2 +- src/crtk/__init__.py | 2 ++ 4 files changed, 9 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 49c0b51..ae571a0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,11 @@ Change log ========== +1.3.0 (2024-08-09) +================== + +* Updated code, `package.xml` and `CMakeLists.txt` so the same repository can be used for both ROS1 and ROS2 + 1.2.0 (2023-11-21) ================== diff --git a/CMakeLists.txt b/CMakeLists.txt index 3129802..a84f7d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required (VERSION 3.10) -project (crtk_python_client VERSION 1.2.0) +project (crtk_python_client VERSION 1.3.0) # first test for ROS1 find_package (catkin QUIET diff --git a/package.xml b/package.xml index 86582ef..4ff86c5 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ crtk_python_client - 1.2.0 + 1.3.0 crtk Python client Anton Deguet Adnan Munawar diff --git a/src/crtk/__init__.py b/src/crtk/__init__.py index 92446ef..b6e35bd 100755 --- a/src/crtk/__init__.py +++ b/src/crtk/__init__.py @@ -15,6 +15,8 @@ elif __ros_version_string == '2': __all__.append('ros_ral2') from .ral_ros2 import ral +else: + print('environment variable ROS_VERSION must be either 1 or 2, did you source your setup.bash?') # handle classes from .wait_move_handle import wait_move_handle