From 6521ca96b171e8bb00d9c8e467d2ad7623a92796 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Fri, 7 Jan 2022 12:22:49 -0500 Subject: [PATCH 01/21] Use PoseStamped instead of TransformStamped for CRTK _cp commands (see https://github.com/collaborative-robotics/documentation/issues/1) --- src/crtk/utils.py | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index 7680915..6492cf6 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -1,7 +1,7 @@ # Author(s): Anton Deguet # Created on: 2018-02-15 # -# Copyright (c) 2018-2021 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute +# Copyright (c) 2018-2022 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute # Released under MIT License import threading @@ -13,6 +13,7 @@ import std_msgs.msg import geometry_msgs.msg import sensor_msgs.msg +import tf_conversions.posemath import crtk_msgs.msg import crtk.wait_move_handle @@ -411,9 +412,9 @@ def __setpoint_cp(self, age = None, wait = None, extra = None): self.__setpoint_cp_event, age, wait): if not extra: - return TransformFromMsg(self.__setpoint_cp_data.transform) + return tf_conversions.posemath.fromMsg(self.__setpoint_cp_data.pose) else: - return [TransformFromMsg(self.__setpoint_cp_data.transform), + return [tf_conversions.posemath.fromMsg(self.__setpoint_cp_data.pose), self.__setpoint_cp_data.header.stamp.to_sec()] raise RuntimeWarning('unable to get setpoint_cp') @@ -423,12 +424,12 @@ def add_setpoint_cp(self): if hasattr(self.__class_instance, 'setpoint_cp'): raise RuntimeWarning('setpoint_cp already exists') # data - self.__setpoint_cp_data = geometry_msgs.msg.TransformStamped() + self.__setpoint_cp_data = geometry_msgs.msg.PoseStamped() self.__setpoint_cp_event = threading.Event() self.__setpoint_cp_lock = False # create the subscriber and keep in list self.__setpoint_cp_subscriber = rospy.Subscriber(self.__ros_namespace + '/setpoint_cp', - geometry_msgs.msg.TransformStamped, + geometry_msgs.msg.PoseStamped, self.__setpoint_cp_cb) self.__subscribers.append(self.__setpoint_cp_subscriber) # add attributes to class instance @@ -513,9 +514,9 @@ def __measured_cp(self, age = None, wait = None, extra = None): self.__measured_cp_event, age, wait): if not extra: - return TransformFromMsg(self.__measured_cp_data.transform) + return tf_conversions.posemath.fromMsg(self.__setpoint_cp_data.pose) else: - return [TransformFromMsg(self.__measured_cp_data.transform), + return [tf_conversions.posemath.fromMsg(self.__setpoint_cp_data.pose), self.__measured_cp_data.header.stamp.to_sec()] raise RuntimeWarning('unable to get measured_cp') @@ -525,11 +526,11 @@ def add_measured_cp(self): if hasattr(self.__class_instance, 'measured_cp'): raise RuntimeWarning('measured_cp already exists') # data - self.__measured_cp_data = geometry_msgs.msg.TransformStamped() + self.__measured_cp_data = geometry_msgs.msg.PoseStamped() self.__measured_cp_event = threading.Event() # create the subscriber and keep in list self.__measured_cp_subscriber = rospy.Subscriber(self.__ros_namespace + '/measured_cp', - geometry_msgs.msg.TransformStamped, + geometry_msgs.msg.PoseStamped, self.__measured_cp_cb) self.__subscribers.append(self.__measured_cp_subscriber) # add attributes to class instance @@ -675,7 +676,8 @@ def add_servo_jr(self): # internal methods for servo_cp def __servo_cp(self, setpoint): # convert to ROS msg and publish - msg = TransformToMsg(setpoint) + msg = geometry_msgs.msg.PoseStamped() + msg.pose = tf_conversions.posemath.toMsg(setpoint) self.__servo_cp_publisher.publish(msg) def add_servo_cp(self): @@ -685,7 +687,7 @@ def add_servo_cp(self): raise RuntimeWarning('servo_cp already exists') # create the subscriber and keep in list self.__servo_cp_publisher = rospy.Publisher(self.__ros_namespace + '/servo_cp', - geometry_msgs.msg.TransformStamped, + geometry_msgs.msg.PoseStamped, latch = True, queue_size = 1) self.__publishers.append(self.__servo_cp_publisher) # add attributes to class instance @@ -788,7 +790,8 @@ def add_move_jr(self): # internal methods for move_cp def __move_cp(self, goal): # convert to ROS msg and publish - msg = TransformToMsg(goal) + msg = geometry_msgs.msg.PoseStamped() + msg.pose = tf_conversions.posemath.toMsg(goal) handle = crtk.wait_move_handle(self.__operating_state_instance); self.__move_cp_publisher.publish(msg) return handle @@ -800,7 +803,7 @@ def add_move_cp(self): raise RuntimeWarning('move_cp already exists') # create the subscriber and keep in list self.__move_cp_publisher = rospy.Publisher(self.__ros_namespace + '/move_cp', - geometry_msgs.msg.TransformStamped, + geometry_msgs.msg.PoseStamped, latch = True, queue_size = 1) self.__publishers.append(self.__move_cp_publisher) # add attributes to class instance From ce760a540f7dafd2fb2d15c4746beabfbbb1f89f Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Mon, 21 Mar 2022 15:05:53 -0400 Subject: [PATCH 02/21] * set CMake minimum version to 3.1 * set version number in project() --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c236044..a37d04d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required (VERSION 2.8.3) -project (crtk_python_client) +cmake_minimum_required (VERSION 3.1) +project (crtk_python_client VERSION 1.0.0) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) From 0152a2102921c6a8813e351b69f8bbfbfb0c5f5e Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 7 Jun 2022 17:27:52 -0400 Subject: [PATCH 03/21] utils.py: fixed bug in measured_cp, was returning setpoint_cp --- src/crtk/utils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index 6492cf6..2211eb9 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -514,9 +514,9 @@ def __measured_cp(self, age = None, wait = None, extra = None): self.__measured_cp_event, age, wait): if not extra: - return tf_conversions.posemath.fromMsg(self.__setpoint_cp_data.pose) + return tf_conversions.posemath.fromMsg(self.__measured_cp_data.pose) else: - return [tf_conversions.posemath.fromMsg(self.__setpoint_cp_data.pose), + return [tf_conversions.posemath.fromMsg(self.__measured_cp_data.pose), self.__measured_cp_data.header.stamp.to_sec()] raise RuntimeWarning('unable to get measured_cp') From 1f1782e6bae91f4b81ba683fd3a119a38822e9e3 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 7 Jun 2022 17:54:44 -0400 Subject: [PATCH 04/21] added simple class with measured_cp only --- src/crtk/__init__.py | 5 ++++- src/crtk/measured_cp.py | 45 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 49 insertions(+), 1 deletion(-) create mode 100644 src/crtk/measured_cp.py diff --git a/src/crtk/__init__.py b/src/crtk/__init__.py index 7f9c355..1d4fefd 100755 --- a/src/crtk/__init__.py +++ b/src/crtk/__init__.py @@ -4,10 +4,13 @@ # Copyright (c) 2016-2021 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute # Released under MIT License -__all__ = ["wait_move_handle", "utils"] +__all__ = ["wait_move_handle", "utils", "measured_cp"] # handle classes from .wait_move_handle import wait_move_handle # utilities from .utils import utils + +# example classes +from .measured_cp import measured_cp diff --git a/src/crtk/measured_cp.py b/src/crtk/measured_cp.py new file mode 100644 index 0000000..da96b5f --- /dev/null +++ b/src/crtk/measured_cp.py @@ -0,0 +1,45 @@ +# Author(s): Anton Deguet +# Created on: 2022-06-07 + +# (C) Copyright 2022 Johns Hopkins University (JHU), All Rights Reserved. + +# --- begin cisst license - do not edit --- + +# This software is provided "as is" under an open source license, with +# no warranty. The complete license can be found in license.txt and +# http://www.cisst.org/cisst/license.txt. + +# --- 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): + """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 + + # crtk features + self.__crtk_utils = crtk.utils(self, self.__ros_namespace, 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') From 0bc97422bade5c57454ffde36dce51c8e65e9eff Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 9 May 2023 10:01:45 -0400 Subject: [PATCH 05/21] added ros_12 class to encapsulate differences between ROS 1 and 2 --- src/crtk/__init__.py | 7 +++++-- src/crtk/ros_12.py | 42 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+), 2 deletions(-) create mode 100644 src/crtk/ros_12.py diff --git a/src/crtk/__init__.py b/src/crtk/__init__.py index 1d4fefd..7e330bc 100755 --- a/src/crtk/__init__.py +++ b/src/crtk/__init__.py @@ -1,10 +1,13 @@ # Author(s): Anton Deguet # Created on: 2016-05 # -# Copyright (c) 2016-2021 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute +# Copyright (c) 2016-2023 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute # Released under MIT License -__all__ = ["wait_move_handle", "utils", "measured_cp"] +__all__ = ["ros_12", "wait_move_handle", "utils", "measured_cp"] + +# wrappers +from .ros_12 import ros_12 # handle classes from .wait_move_handle import wait_move_handle diff --git a/src/crtk/ros_12.py b/src/crtk/ros_12.py new file mode 100644 index 0000000..137b021 --- /dev/null +++ b/src/crtk/ros_12.py @@ -0,0 +1,42 @@ +# 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 rospy + +class ros_12: + """Class used to wrap rospy features so we can write code independent + of ROS version. + """ + + def __init__(self, node_name, namespace = ''): + self.__node_name = node_name + self.__namespace = namespace + self.__rate_in_Hz = 0.0 + # ros init node so we can use default ros arguments (e.g. __ns:= for namespace) + rospy.init_node(self.__node_name, anonymous = True) + + @staticmethod + def parse_argv(argv): + # strip ros arguments + return rospy.myargv(argv) + + def set_rate(self, rate_in_Hz): + self.__rate_in_Hz = rate_in_Hz + self.__ros_rate = rospy.Rate(rate_in_Hz) + + def sleep(self): + if self.__rate_in_Hz == 0.0: + raise RuntimeError('set_rate must be called before sleep') + self.__ros_rate.sleep() + + def node_name(self): + return self.__node_name + + def namespace(self): + return self.__namespace + + def spin_and_execute(self, function): + function() From 95ed94c4b88b8eaa733dea1687e7bbceb7efdd74 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 9 May 2023 11:35:11 -0400 Subject: [PATCH 06/21] utils.py: added support for `hold` and made sure all commands `servo/move` are not latched. --- src/crtk/utils.py | 38 +++++++++++++++++++++++++++++--------- 1 file changed, 29 insertions(+), 9 deletions(-) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index 2211eb9..0af436b 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -1,7 +1,7 @@ # Author(s): Anton Deguet # Created on: 2018-02-15 # -# Copyright (c) 2018-2022 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute +# Copyright (c) 2018-2023 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute # Released under MIT License import threading @@ -631,6 +631,26 @@ def add_jacobian(self): + # internal methods for hold + def __hold(self): + # convert to ROS msg and publish + msg = std_msgs.msg.Empty() + self.__hold_publisher.publish(msg) + + def add_hold(self): + # throw a warning if this has alread been added to the class, + # using the callback name to test + if hasattr(self.__class_instance, 'hold'): + raise RuntimeWarning('hold already exists') + # create the subscriber and keep in list + self.__hold_publisher = rospy.Publisher(self.__ros_namespace + '/hold', + std_msgs.msg.Empty, + latch = False, queue_size = 1) + self.__publishers.append(self.__hold_publisher) + # add attributes to class instance + self.__class_instance.hold = self.__hold + + # internal methods for servo_jp def __servo_jp(self, setpoint): # convert to ROS msg and publish @@ -646,7 +666,7 @@ def add_servo_jp(self): # create the subscriber and keep in list self.__servo_jp_publisher = rospy.Publisher(self.__ros_namespace + '/servo_jp', sensor_msgs.msg.JointState, - latch = True, queue_size = 1) + latch = False, queue_size = 1) self.__publishers.append(self.__servo_jp_publisher) # add attributes to class instance self.__class_instance.servo_jp = self.__servo_jp @@ -667,7 +687,7 @@ def add_servo_jr(self): # create the subscriber and keep in list self.__servo_jr_publisher = rospy.Publisher(self.__ros_namespace + '/servo_jr', sensor_msgs.msg.JointState, - latch = True, queue_size = 1) + latch = False, queue_size = 1) self.__publishers.append(self.__servo_jr_publisher) # add attributes to class instance self.__class_instance.servo_jr = self.__servo_jr @@ -688,7 +708,7 @@ def add_servo_cp(self): # create the subscriber and keep in list self.__servo_cp_publisher = rospy.Publisher(self.__ros_namespace + '/servo_cp', geometry_msgs.msg.PoseStamped, - latch = True, queue_size = 1) + latch = False, queue_size = 1) self.__publishers.append(self.__servo_cp_publisher) # add attributes to class instance self.__class_instance.servo_cp = self.__servo_cp @@ -709,7 +729,7 @@ def add_servo_jf(self): # create the subscriber and keep in list self.__servo_jf_publisher = rospy.Publisher(self.__ros_namespace + '/servo_jf', sensor_msgs.msg.JointState, - latch = True, queue_size = 1) + latch = False, queue_size = 1) self.__publishers.append(self.__servo_jf_publisher) # add attributes to class instance self.__class_instance.servo_jf = self.__servo_jf @@ -735,7 +755,7 @@ def add_servo_cf(self): # create the subscriber and keep in list self.__servo_cf_publisher = rospy.Publisher(self.__ros_namespace + '/servo_cf', geometry_msgs.msg.WrenchStamped, - latch = True, queue_size = 1) + latch = False, queue_size = 1) self.__publishers.append(self.__servo_cf_publisher) # add attributes to class instance self.__class_instance.servo_cf = self.__servo_cf @@ -758,7 +778,7 @@ def add_move_jp(self): # create the subscriber and keep in list self.__move_jp_publisher = rospy.Publisher(self.__ros_namespace + '/move_jp', sensor_msgs.msg.JointState, - latch = True, queue_size = 1) + latch = False, queue_size = 1) self.__publishers.append(self.__move_jp_publisher) # add attributes to class instance self.__class_instance.move_jp = self.__move_jp @@ -781,7 +801,7 @@ def add_move_jr(self): # create the subscriber and keep in list self.__move_jr_publisher = rospy.Publisher(self.__ros_namespace + '/move_jr', sensor_msgs.msg.JointState, - latch = True, queue_size = 1) + latch = False, queue_size = 1) self.__publishers.append(self.__move_jr_publisher) # add attributes to class instance self.__class_instance.move_jr = self.__move_jr @@ -804,7 +824,7 @@ def add_move_cp(self): # create the subscriber and keep in list self.__move_cp_publisher = rospy.Publisher(self.__ros_namespace + '/move_cp', geometry_msgs.msg.PoseStamped, - latch = True, queue_size = 1) + latch = False, queue_size = 1) self.__publishers.append(self.__move_cp_publisher) # add attributes to class instance self.__class_instance.move_cp = self.__move_cp From 22c067253f99b2ee78069859d8917b46edda2f23 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 9 May 2023 12:55:04 -0400 Subject: [PATCH 07/21] renamed msg/operating_state to msg/OperatingState to follow standard ROS 1 (and 2) naming convention --- src/crtk/utils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index 0af436b..22ba096 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -289,7 +289,7 @@ def add_operating_state(self, optional_ros_namespace = None): if hasattr(self.__class_instance, 'operating_state'): raise RuntimeWarning('operating_state already exists') # data - self.__operating_state_data = crtk_msgs.msg.operating_state() + self.__operating_state_data = crtk_msgs.msg.OperatingState() self.__operating_state_event = threading.Event() # determine namespace to use @@ -300,7 +300,7 @@ def add_operating_state(self, optional_ros_namespace = None): # create the subscriber/publisher and keep in list self.__operating_state_subscriber = rospy.Subscriber(namespace_to_use + '/operating_state', - crtk_msgs.msg.operating_state, self.__operating_state_cb) + crtk_msgs.msg.OperatingState, self.__operating_state_cb) self.__subscribers.append(self.__operating_state_subscriber) self.__state_command_publisher = rospy.Publisher(namespace_to_use + '/state_command', crtk_msgs.msg.StringStamped, From 261a2d025e1c85821216d0938f8f3128b4a4a86c Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 9 May 2023 17:17:01 -0400 Subject: [PATCH 08/21] utils: added `forward_kinematics` and `inverse_kinematics` --- src/crtk/utils.py | 51 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index 22ba096..72ee8b7 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -15,6 +15,7 @@ import sensor_msgs.msg import tf_conversions.posemath import crtk_msgs.msg +import crtk_msgs.srv import crtk.wait_move_handle def TransformFromMsg(t): @@ -92,6 +93,7 @@ def __init__(self, self.__expected_interval = expected_interval self.__subscribers = [] self.__publishers = [] + self.__service_proxies = [] self.__attributes = [] rospy.on_shutdown(self.__ros_shutdown) @@ -828,3 +830,52 @@ def add_move_cp(self): self.__publishers.append(self.__move_cp_publisher) # add attributes to class instance self.__class_instance.move_cp = self.__move_cp + + + # internal methods for forward_kinematics + def __forward_kinematics(self, jp, extra = None): + # convert to ROS msg and publish + request = crtk_msgs.srv.QueryForwardKinematicsRequest() + request.jp[:] = jp.flat + response = self.__forward_kinematics_service_proxy(request) + if not extra: + return tf_conversions.posemath.fromMsg(response.cp); + else: + return [tf_conversions.posemath.fromMsg(response.cp), response.result, response.message] + + def add_forward_kinematics(self): + # throw a warning if this has alread been added to the class, + # using the callback name to test + if hasattr(self.__class_instance, 'forward_kinematics'): + raise RuntimeWarning('forward_kinematics already exists') + # create the service proxy and keep in list + self.__forward_kinematics_service_proxy = rospy.ServiceProxy(self.__ros_namespace + '/forward_kinematics', + crtk_msgs.srv.QueryForwardKinematics) + self.__service_proxies.append(self.__forward_kinematics_service_proxy) + # add attributes to class instance + self.__class_instance.forward_kinematics = self.__forward_kinematics + + + # internal methods for inverse_kinematics + def __inverse_kinematics(self, jp, cp, extra = None): + # convert to ROS msg and publish + request = crtk_msgs.srv.QueryInverseKinematicsRequest() + request.jp[:] = jp.flat + request.cp = tf_conversions.posemath.toMsg(cp) + response = self.__inverse_kinematics_service_proxy(request) + if not extra: + return numpy.array(response.jp) + else: + return [numpy.array(response.jp), response.result, response.message] + + def add_inverse_kinematics(self): + # throw a warning if this has alread been added to the class, + # using the callback name to test + if hasattr(self.__class_instance, 'inverse_kinematics'): + raise RuntimeWarning('inverse_kinematics already exists') + # create the service proxy and keep in list + self.__inverse_kinematics_service_proxy = rospy.ServiceProxy(self.__ros_namespace + '/inverse_kinematics', + crtk_msgs.srv.QueryInverseKinematics) + self.__service_proxies.append(self.__inverse_kinematics_service_proxy) + # add attributes to class instance + self.__class_instance.inverse_kinematics = self.__inverse_kinematics From 9fd427759c02816d1a02428a6d6eccfec51f628b Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Fri, 12 May 2023 11:49:10 -0400 Subject: [PATCH 09/21] ros_12: added --- src/crtk/ros_12.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/crtk/ros_12.py b/src/crtk/ros_12.py index 137b021..f834c7b 100644 --- a/src/crtk/ros_12.py +++ b/src/crtk/ros_12.py @@ -18,6 +18,10 @@ def __init__(self, node_name, namespace = ''): # ros init node so we can use default ros arguments (e.g. __ns:= for namespace) rospy.init_node(self.__node_name, anonymous = True) + @staticmethod + def ros_version(): + return 1 + @staticmethod def parse_argv(argv): # strip ros arguments From e59ec3ae8c686947728fea6514405099da8208b1 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 16 May 2023 19:14:28 -0400 Subject: [PATCH 10/21] utils.py: use `rospy.Time.now` for all timestamps, rewrote `wait_for_busy` to match ROS2 implementation, added optional velocity for `servo_jp` --- src/crtk/utils.py | 79 +++++++++++++++++++++++++++-------------------- 1 file changed, 45 insertions(+), 34 deletions(-) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index 72ee8b7..3912930 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -106,6 +106,10 @@ def __ros_shutdown(self): self.__operating_state_event.set() + def __now(self): + return rospy.Time.now() + + def remove_all(self): for sub in self.__subscribers: sub.unregister() @@ -119,13 +123,13 @@ def remove_all(self): def __wait_for_valid_data(self, data, event, age, wait): event.clear() - if age == None: + if age is None: age = self.__expected_interval - if wait == None: + if wait is None: wait = self.__expected_interval # check if user accepts cached data if age != 0.0: - data_age = rospy.Time.now() - data.header.stamp + data_age = self.__now() - data.header.stamp if data_age <= rospy.Duration(age): return True if wait != 0.0: @@ -152,7 +156,7 @@ def __operating_state(self, extra = None): def __wait_for_operating_state(self, expected_state, timeout): if timeout < 0.0: return False - start_time = time.time() + start_time = self.__now() in_time = self.__operating_state_event.wait(timeout) if rospy.is_shutdown(): return False; @@ -162,9 +166,9 @@ def __wait_for_operating_state(self, expected_state, timeout): return True else: # wait a bit more - elapsed_time = time.time() - start_time + elapsed_time = self.__now() - start_time self.__operating_state_event.clear() - return self.__wait_for_operating_state(expected_state, timeout - elapsed_time) + return self.__wait_for_operating_state(expected_state, timeout - elapsed_time.to_sec()) # past timeout return False @@ -209,7 +213,7 @@ def __is_homed(self, extra = None): def __wait_for_homed(self, timeout, expected_homed): if timeout < 0.0: return False - start_time = time.time() + start_time = self.__now() self.__operating_state_event.clear() in_time = self.__operating_state_event.wait(timeout) if rospy.is_shutdown(): @@ -220,8 +224,8 @@ def __wait_for_homed(self, timeout, expected_homed): return True else: # wait a bit more - elapsed_time = time.time() - start_time - return self.__wait_for_homed(timeout - elapsed_time, expected_homed) + elapsed_time = self.__now() - start_time + return self.__wait_for_homed(timeout - elapsed_time.to_sec(), expected_homed) # past timeout return False @@ -242,8 +246,11 @@ def __unhome(self, timeout = 0): return self.__wait_for_homed(timeout, False) def __is_busy(self, - start_time = rospy.Time(0.0), + start_time = None, extra = None): + # set start time to now if not specified + if start_time is None: + start_time = self.__now() result = True if (self.__operating_state_data.header.stamp > start_time): result = self.__operating_state_data.is_busy @@ -255,35 +262,37 @@ def __is_busy(self, def __wait_for_busy(self, is_busy = False, - start_time = rospy.Time(0.0), + start_time = None, timeout = 30.0): # if timeout is negative, not waiting if timeout < 0.0: return False - # if start_time 0.0, user provided a start time and we should - # check if an event arrived after start_time - if start_time > rospy.Time(0.0): - if (self.__operating_state_data.header.stamp > start_time - and self.__operating_state_data.is_busy == is_busy): + # set start time to now if not specified + if start_time is None: + start_time = self.__now() + else: + # user provided start_time, check if an event arrived after start_time + last_event_time = self.__operating_state_data.header.stamp + if (last_event_time > start_time and self.__operating_state_data.is_busy == is_busy): return True + # other cases, waiting for an operating_state event - _start_time = time.time() + _start_time = self.__now() self.__operating_state_event.clear() in_time = self.__operating_state_event.wait(timeout) - if rospy.is_shutdown(): + if rospy.is_shutdown() or not in_time: return False; - if in_time: - # within timeout and result we expected - if self.__operating_state_data.is_busy == is_busy: - return True - else: - # wait a bit more - elapsed_time = time.time() - _start_time - return self.__wait_for_busy(is_busy = is_busy, - start_time = start_time, - timeout = (timeout - elapsed_time)) - # past timeout - return False + + # within timeout and result we expected + if self.__operating_state_data.is_busy == is_busy: + return True + else: + # wait a bit more + elapsed_time = self.__now() - _start_time + self.__operating_state_event.clear() + return self.__wait_for_busy(is_busy = is_busy, + start_time = start_time, + timeout = (timeout - elapsed_time.to_sec())) def add_operating_state(self, optional_ros_namespace = None): # throw a warning if this has alread been added to the class, @@ -295,14 +304,15 @@ def add_operating_state(self, optional_ros_namespace = None): self.__operating_state_event = threading.Event() # determine namespace to use - if optional_ros_namespace == None: + if optional_ros_namespace is None: namespace_to_use = self.__ros_namespace else: namespace_to_use = optional_ros_namespace # create the subscriber/publisher and keep in list self.__operating_state_subscriber = rospy.Subscriber(namespace_to_use + '/operating_state', - crtk_msgs.msg.OperatingState, self.__operating_state_cb) + crtk_msgs.msg.OperatingState, self.__operating_state_cb, + queue_size = 10) self.__subscribers.append(self.__operating_state_subscriber) self.__state_command_publisher = rospy.Publisher(namespace_to_use + '/state_command', crtk_msgs.msg.StringStamped, @@ -654,10 +664,11 @@ def add_hold(self): # internal methods for servo_jp - def __servo_jp(self, setpoint): + def __servo_jp(self, setpoint_p, setpoint_v = numpy.array([])): # convert to ROS msg and publish msg = sensor_msgs.msg.JointState() - msg.position[:] = setpoint.flat + msg.position[:] = setpoint_p.flat + msg.velocity[:] = setpoint_v.flat self.__servo_jp_publisher.publish(msg) def add_servo_jp(self): From ffb0a764d245a715c113f1215111de1a2afd5759 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Thu, 1 Jun 2023 17:08:33 -0400 Subject: [PATCH 11/21] added joystick_button to wrap/abstract buttons and foot pedals --- src/crtk/__init__.py | 3 ++- src/crtk/joystick_button.py | 46 +++++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 1 deletion(-) create mode 100644 src/crtk/joystick_button.py diff --git a/src/crtk/__init__.py b/src/crtk/__init__.py index 7e330bc..5058484 100755 --- a/src/crtk/__init__.py +++ b/src/crtk/__init__.py @@ -4,7 +4,7 @@ # Copyright (c) 2016-2023 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute # Released under MIT License -__all__ = ["ros_12", "wait_move_handle", "utils", "measured_cp"] +__all__ = ["ros_12", "wait_move_handle", "utils", "joystick_button", "measured_cp"] # wrappers from .ros_12 import ros_12 @@ -14,6 +14,7 @@ # utilities from .utils import utils +from .joystick_button import joystick_button # example classes from .measured_cp import measured_cp diff --git a/src/crtk/joystick_button.py b/src/crtk/joystick_button.py new file mode 100644 index 0000000..d314d3d --- /dev/null +++ b/src/crtk/joystick_button.py @@ -0,0 +1,46 @@ +# Author(s): Anton Deguet +# Created on: 2023-05-30 +# +# Copyright (c) 2023 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute +# Released under MIT License + +import threading +import rospy +import sensor_msgs.msg + +class joystick_button: + def __init__(self, button_name, index = 0): + self.__button_name = button_name + self.__index = index + self.__value = None + self.__user_callback = None + self.__event = threading.Event() + self.__subscriber = rospy.Subscriber(button_name, + sensor_msgs.msg.Joy, self.__callback, + queue_size = 10) + + def __del__(self): + self.__subscriber.unregister() + + def __callback(self, event): + self.__value = event.buttons[self.__index] + self.__event.set() + if not self.__user_callback is None: + self.__user_callback(self.__value) + + def set_callback(self, callback = None): + self.__user_callback = callback + + def value(self): + return self.__value + + def wait(self, timeout = None, value = None): + ready = False + while not ready: + self.__event.clear() + if not self.__event.wait(timeout): + return False + else: + if value == None or value == self.__value: + ready = True + return False From c117b60c6cdbb3d71d85d917738ded2ad7bc0fb4 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 6 Jun 2023 13:21:45 -0400 Subject: [PATCH 12/21] servo_cv command (#2) (#3) * Add servo_cv command and example. * fixed small typo --------- Co-authored-by: Hisashi Ishida <91571647+hisashiishida@users.noreply.github.com> Co-authored-by: Hisashi Isihda --- scripts/crtk_servo_cv_example.py | 69 ++++++++++++++++++++++++++++++++ src/crtk/utils.py | 26 ++++++++++++ 2 files changed, 95 insertions(+) create mode 100644 scripts/crtk_servo_cv_example.py diff --git a/scripts/crtk_servo_cv_example.py b/scripts/crtk_servo_cv_example.py new file mode 100644 index 0000000..868bb36 --- /dev/null +++ b/scripts/crtk_servo_cv_example.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python + +# Author: Hisashi Ishida, Anton Deguet +# Created on: 2023-04-01 +# +# Copyright (c) 2015-2023 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute +# Released under MIT License + +# Start a single arm using +# > rosrun dvrk_robot dvrk_console_json -j + +# To communicate with the arm using ROS topics, see the python based example dvrk_arm_test.py: +# > rosrun crtk_python_client crtk_arm_test.py + +import crtk +import math +import sys +import rospy +import numpy as np +import PyKDL + + +# example of application using device.py +class crtk_servo_cv_example: + + # configuration + def configure(self, device_namespace): + # ROS initialization + if not rospy.get_node_uri(): + rospy.init_node('crtk_servo_cv_example', anonymous = True, log_level = rospy.WARN) + + print(rospy.get_caller_id() + ' -> configuring crtk_device_test for: ' + device_namespace) + # populate this class with all the ROS topics we need + self.crtk_utils = crtk.utils(self, device_namespace) + self.crtk_utils.add_operating_state() + self.crtk_utils.add_setpoint_cp() + self.crtk_utils.add_servo_cv() + # for all examples + self.duration = 10 # 10 seconds + self.rate = 200 # aiming for 200 Hz + self.samples = self.duration * self.rate + + def run_servo_cv(self): + if not self.enable(60): + print("Unable to enable the device, make sure it is connected.") + return + + # create a new goal swith constant speed + for i in range(self.samples): + vel = np.array([0.05, 0.0, 0.0, 0.0, 0.0, 0.0]) # move 5 cm/sec along x direction + self.servo_cv(vel) + rospy.sleep(1.0 / self.rate) + + # send the zero velcity to stop the robot + vel = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.servo_cv(vel) + +# use the class now, i.e. main program +if __name__ == '__main__': + try: + if (len(sys.argv) != 2): + print(sys.argv[0], ' requires one argument, i.e. crtk device namespace') + else: + example = crtk_servo_cv_example() + example.configure(sys.argv[1]) + example.run_servo_cv() + + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/src/crtk/utils.py b/src/crtk/utils.py index 3912930..a958ec8 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -774,6 +774,32 @@ def add_servo_cf(self): self.__class_instance.servo_cf = self.__servo_cf + # internal methods for servo_cv + def __servo_cv(self, setpoint): + # convert to ROS msg and publish + msg = geometry_msgs.msg.TwistStamped() + msg.Twist.linear.x = setpoint[0] + msg.Twist.linear.y = setpoint[1] + msg.Twist.linear.z = setpoint[2] + msg.Twist.angular.x = setpoint[3] + msg.Twist.angular.y = setpoint[4] + msg.Twist.angular.z = setpoint[5] + self.__servo_cv_publisher.publish(msg) + + def add_servo_cv(self): + # throw a warning if this has alread been added to the class, + # using the callback name to test + if hasattr(self.__class_instance, 'servo_cv'): + raise RuntimeWarning('servo_cv already exists') + # create the subscriber and keep in list + self.__servo_cv_publisher = rospy.Publisher(self.__ros_namespace + '/servo_cv', + geometry_msgs.msg.TwistStamped, + latch = True, queue_size = 1) + self.__publishers.append(self.__servo_cv_publisher) + # add attributes to class instance + self.__class_instance.servo_cv = self.__servo_cv + + # internal methods for move_jp def __move_jp(self, setpoint): # convert to ROS msg and publish From 69755ef15d29c1f0b3cd78f2efa28f46014196df Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 13 Jun 2023 13:49:25 -0400 Subject: [PATCH 13/21] ros_12: added support for user arguments for spin_and_execute --- src/crtk/ros_12.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/crtk/ros_12.py b/src/crtk/ros_12.py index f834c7b..62f9d22 100644 --- a/src/crtk/ros_12.py +++ b/src/crtk/ros_12.py @@ -42,5 +42,5 @@ def node_name(self): def namespace(self): return self.__namespace - def spin_and_execute(self, function): - function() + def spin_and_execute(self, function, *arguments): + function(*arguments) From 073e467908035412e7879a430149d74118f01feb Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Wed, 14 Jun 2023 14:19:18 -0400 Subject: [PATCH 14/21] crtk: added support for `free` --- src/crtk/utils.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index a958ec8..144b71b 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -663,6 +663,26 @@ def add_hold(self): self.__class_instance.hold = self.__hold + # internal methods for free + def __free(self): + # convert to ROS msg and publish + msg = std_msgs.msg.Empty() + self.__free_publisher.publish(msg) + + def add_free(self): + # throw a warning if this has alread been added to the class, + # using the callback name to test + if hasattr(self.__class_instance, 'free'): + raise RuntimeWarning('free already exists') + # create the subscriber and keep in list + self.__free_publisher = rospy.Publisher(self.__ros_namespace + '/free', + std_msgs.msg.Empty, + latch = False, queue_size = 1) + self.__publishers.append(self.__free_publisher) + # add attributes to class instance + self.__class_instance.free = self.__free + + # internal methods for servo_jp def __servo_jp(self, setpoint_p, setpoint_v = numpy.array([])): # convert to ROS msg and publish From 58f979b8cab9653dfb34fc87bb122f49911bb2a3 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Thu, 22 Jun 2023 17:50:05 -0400 Subject: [PATCH 15/21] Add check_connections(timeout) method to ctrk utils (#4) * Add check_connections(timeout) method to ctrk utils check_connections will check that all publishers and subscribers have at least one connection. It waits at most timeout seconds, raising a TimeoutError if some connections have not established within the timeout period. * Increase connection checking frequency --- src/crtk/utils.py | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index 144b71b..d539533 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -137,6 +137,42 @@ def __wait_for_valid_data(self, data, event, age, wait): return True return False + # 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 + def check_connections(self, timeout_seconds): + if timeout_seconds <= 0.0: + return + + start_time = rospy.Time.now() + timeout_duration = rospy.Duration(timeout_seconds) + + connected = lambda pubsub: pubsub.get_num_connections() > 0 + + # wait at most timeout_seconds for all connections to establish + while (rospy.Time.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 + + rospy.sleep(0.01) + + # last check of connection status, raise error if any remain unconnected + unconnected_publishers = [p for p in self.__publishers if not connected(p)] + unconnected_subscribers = [s for s in self.__subscribers if not connected(s)] + if len(unconnected_publishers) == 0 and len(unconnected_subscribers) == 0: + return + + err_msg = \ + ( + f"Timed out while waiting for publisher/subscriber connections to establish\n" + f" Publishers: {len(self.__publishers) - len(unconnected_publishers)} connected, {len(unconnected_publishers)} not connected\n" + f" not connected: [{', '.join([p.name for p in unconnected_publishers])}]\n\n" + f" Subscribers: {len(self.__subscribers) - len(unconnected_subscribers)} connected, {len(unconnected_subscribers)} not connected\n" + f" not connected: [{', '.join([s.name for s in unconnected_subscribers])}]\n\n" + ) + raise TimeoutError(err_msg) # internal methods to manage state def __operating_state_cb(self, msg): From 38044860ce33b11b95f15430dd944cfbcc73f587 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Wed, 5 Jul 2023 13:59:50 -0400 Subject: [PATCH 16/21] ROS abstraction layer (#5) * Overhaul ros_12 to abstract away ros version * Rename ros_12 to ral ral: ROS abstraction layer * Fix ros_12->ral in package __init__.py * check_connections() now also checks all children recursively checks children by default, can be disabled * Add all twist/wrench message conversions * Rewrite CRTK examples to use new ral * Mark crtk_servo_cv_example.py as executable * Add default timeout to ral.check_connections() * Add create_rate() to ral * Use ral.create_rate() instead of ral.rate * Fix missed renames * Clean up RAL interface * Use body/servo_cf in crtk_haptic_example * Add check_connections() to example scripts * Clean up some naming * Use crtk.joystick_button to use ral * Revert * Use argparse, correct haptic example - Use argparse pkg in all CRTK examples - Revert dgain/cube size changes in crtk_haptic_example.py * Fix crtk_teleop_example --- scripts/crtk_haptic_example.py | 109 ++++--- scripts/crtk_move_cp_example.py | 58 ++-- scripts/crtk_servo_cp_example.py | 59 ++-- scripts/crtk_servo_cv_example.py | 73 ++--- scripts/crtk_servo_jp_example.py | 72 ++-- scripts/crtk_teleop_example.py | 95 +++--- src/crtk/__init__.py | 7 +- src/crtk/joystick_button.py | 40 +-- src/crtk/msg_conversions.py | 139 ++++++++ src/crtk/ral.py | 183 +++++++++++ src/crtk/ros_12.py | 46 --- src/crtk/utils.py | 544 +++++++++++++------------------ src/crtk/wait_move_handle.py | 11 +- 13 files changed, 826 insertions(+), 610 deletions(-) mode change 100644 => 100755 scripts/crtk_servo_cv_example.py create mode 100644 src/crtk/msg_conversions.py create mode 100644 src/crtk/ral.py delete mode 100644 src/crtk/ros_12.py diff --git a/scripts/crtk_haptic_example.py b/scripts/crtk_haptic_example.py index c623e06..a261ab8 100755 --- a/scripts/crtk_haptic_example.py +++ b/scripts/crtk_haptic_example.py @@ -13,48 +13,73 @@ # > rosrun sensable_phantom_ros sensable_phantom -j sawSensablePhantomRight.json # To communicate with the device using ROS topics, see the python based example: -# > rosrun crtk_python_client crtk_haptic_example +# > rosrun crtk_python_client crtk_haptic_example.py +import argparse import crtk -import math -import sys -import rospy -import numpy import PyKDL +import std_msgs.msg +import sys + if sys.version_info.major < 3: input = raw_input -# example of application using device.py + class crtk_haptic_example: + class Subframe: + def __init__(self, ral): + self.ral = ral + self.crtk_utils = crtk.utils(self, ral) + self.crtk_utils.add_servo_cf() + + self.set_cf_orientation_absolute_pub = self.ral.publisher( + 'set_cf_orientation_absolute', std_msgs.msg.Bool) + + def set_cf_orientation_absolute(self, absolute = True): + m = std_msgs.msg.Bool() + m.data = absolute + self.set_cf_orientation_absolute_pub.publish(m) - # configuration - def configure(self, device_namespace): - # ROS initialization - if not rospy.get_node_uri(): - rospy.init_node('crtk_haptic_example', anonymous = True, log_level = rospy.WARN) + def __init__(self, ral): + self.ral = ral - print(rospy.get_caller_id() + ' -> configuring crtk_device_test for: ' + device_namespace) # populate this class with all the ROS topics we need - self.crtk_utils = crtk.utils(self, device_namespace) + self.crtk_utils = crtk.utils(self, ral) self.crtk_utils.add_operating_state() self.crtk_utils.add_measured_cp() self.crtk_utils.add_measured_cv() - self.crtk_utils.add_servo_cf() + + self.body = crtk_haptic_example.Subframe(self.ral.create_child('/body')) + # for all examples - self.duration = 10 # 10 seconds - self.rate = 500 # aiming for 500 Hz + self.duration = 20 # seconds + self.rate = 500 # aiming for 500 Hz self.samples = self.duration * self.rate # main loop def run(self): - if not self.enable(60): - print("Unable to enable the device, make sure it is connected.") + self.ral.check_connections() + + if not self.enable(30): + print('Unable to enable the device, make sure it is connected.') + return + + if not self.home(30): + print('Unable to home the device, make sure it is connected.') return + self.body.set_cf_orientation_absolute() + self.running = True while (self.running): - print ('\n- q: quit\n- p: print position, velocity\n- b: virtual box around current position with linear forces (10s)\n- v: viscosity (10s)') + msg = ('\n' + '- q: quit\n' + '- p: print position, velocity\n' + '- b: virtual box around current position with linear forces ({}s)\n' + '- v: viscosity ({}s)' + ) + print(msg.format(self.duration, self.duration)) answer = input('Enter your choice and [enter] to continue\n') if answer == 'q': self.running = False @@ -75,11 +100,14 @@ def run_print(self): # virtual box def run_box(self): - # save current position - dim = 0.01 # 2 cm cube + dim = 0.01 # +/- 1 cm per dimension creates a 2 cm cube p_gain = -500.0 + + # save current position center = PyKDL.Frame() center.p = self.measured_cp().p + + sleep_rate = self.ral.create_rate(self.rate) for i in range(self.samples): wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # foreach d dimension x, y, z @@ -89,33 +117,38 @@ def run_box(self): wrench[d] = p_gain * (distance - dim) elif (distance < -dim): wrench[d] = p_gain * (distance + dim) - self.servo_cf(wrench) - rospy.sleep(1.0 / self.rate) + self.body.servo_cf(wrench) + sleep_rate.sleep() + wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - self.servo_cf(wrench) + self.body.servo_cf(wrench) # viscosity def run_viscosity(self): d_gain = -10.0 + sleep_rate = self.ral.create_rate(self.rate) for i in range(self.samples): wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # foreach d dimension x, y, z for d in range(3): wrench[d] = d_gain * self.measured_cv()[d] - self.servo_cf(wrench) - rospy.sleep(1.0 / self.rate) + self.body.servo_cf(wrench) + sleep_rate.sleep() + wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - self.servo_cf(wrench) + self.body.servo_cf(wrench) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('namespace', type = str, help = 'ROS namespace for CRTK device') + app_args = crtk.ral.parse_argv(sys.argv[1:]) # process and remove ROS args + args = parser.parse_args(app_args) + + example_name = type(crtk_haptic_example).__name__ + ral = crtk.ral(example_name, args.namespace) + example = crtk_haptic_example(ral) + ral.spin_and_execute(example.run) -# use the class now, i.e. main program if __name__ == '__main__': - try: - if (len(sys.argv) != 2): - print(sys.argv[0], ' requires one argument, i.e. crtk device namespace') - else: - example = crtk_haptic_example() - example.configure(sys.argv[1]) - example.run() - - except rospy.ROSInterruptException: - pass + main() diff --git a/scripts/crtk_move_cp_example.py b/scripts/crtk_move_cp_example.py index ac048d6..d219735 100755 --- a/scripts/crtk_move_cp_example.py +++ b/scripts/crtk_move_cp_example.py @@ -10,37 +10,35 @@ # > rosrun dvrk_robot dvrk_console_json -j # To communicate with the arm using ROS topics, see the python based example dvrk_arm_test.py: -# > rosrun crtk_python_client crtk_arm_test.py +# > rosrun crtk_python_client crtk_move_cp_example.py +import argparse import crtk -import math -import sys -import rospy -import numpy import PyKDL +import sys -# example of application using device.py class crtk_move_cp_example: + def __init__(self, ral): + self.ral = ral - # configuration - def configure(self, device_namespace): - # ROS initialization - if not rospy.get_node_uri(): - rospy.init_node('crtk_move_cp_example', anonymous = True, log_level = rospy.WARN) - - print(rospy.get_caller_id() + ' -> configuring crtk_device_test for: ' + device_namespace) # populate this class with all the ROS topics we need - self.crtk_utils = crtk.utils(self, device_namespace) + self.crtk_utils = crtk.utils(self, ral) self.crtk_utils.add_operating_state() self.crtk_utils.add_setpoint_cp() self.crtk_utils.add_move_cp() - def run_move_cp(self): - if not self.enable(60): + def run(self): + self.ral.check_connections() + + if not self.enable(30): print("Unable to enable the device, make sure it is connected.") return + if not self.home(30): + print('Unable to home the device, make sure it is connected.') + return + # create a new goal starting with current position start_cp = PyKDL.Frame() start_cp.p = self.setpoint_cp().p @@ -48,7 +46,7 @@ def run_move_cp(self): goal = PyKDL.Frame() goal.p = self.setpoint_cp().p goal.M = self.setpoint_cp().M - amplitude = 0.01 # 2 centimeters + amplitude = 0.02 # 2 centimeters # first move goal.p[0] = start_cp.p[0] + amplitude @@ -56,25 +54,29 @@ def run_move_cp(self): goal.p[2] = start_cp.p[2] handle = self.move_cp(goal) handle.wait() + # second move goal.p[0] = start_cp.p[0] - amplitude goal.p[1] = start_cp.p[1] - amplitude self.move_cp(goal).wait() + # back to starting point goal.p[0] = start_cp.p[0] goal.p[1] = start_cp.p[1] self.move_cp(goal).wait() -# use the class now, i.e. main program +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('namespace', type = str, help = 'ROS namespace for CRTK device') + app_args = crtk.ral.parse_argv(sys.argv[1:]) # process and remove ROS args + args = parser.parse_args(app_args) + + example_name = type(crtk_move_cp_example).__name__ + ral = crtk.ral(example_name, args.namespace) + example = crtk_move_cp_example(ral) + ral.spin_and_execute(example.run) + + if __name__ == '__main__': - try: - if (len(sys.argv) != 2): - print(sys.argv[0], ' requires one argument, i.e. crtk device namespace') - else: - example = crtk_move_cp_example() - example.configure(sys.argv[1]) - example.run_move_cp() - - except rospy.ROSInterruptException: - pass + main() diff --git a/scripts/crtk_servo_cp_example.py b/scripts/crtk_servo_cp_example.py index f348b08..6edf291 100755 --- a/scripts/crtk_servo_cp_example.py +++ b/scripts/crtk_servo_cp_example.py @@ -10,41 +10,41 @@ # > rosrun dvrk_robot dvrk_console_json -j # To communicate with the arm using ROS topics, see the python based example dvrk_arm_test.py: -# > rosrun crtk_python_client crtk_arm_test.py +# > rosrun crtk_python_client crtk_servo_cp_example.py +import argparse import crtk import math -import sys -import rospy -import numpy import PyKDL +import sys -# example of application using device.py class crtk_servo_cp_example: + def __init__(self, ral): + self.ral = ral - # configuration - def configure(self, device_namespace): - # ROS initialization - if not rospy.get_node_uri(): - rospy.init_node('crtk_servo_cp_example', anonymous = True, log_level = rospy.WARN) - - print(rospy.get_caller_id() + ' -> configuring crtk_device_test for: ' + device_namespace) # populate this class with all the ROS topics we need - self.crtk_utils = crtk.utils(self, device_namespace) + self.crtk_utils = crtk.utils(self, ral) self.crtk_utils.add_operating_state() self.crtk_utils.add_setpoint_cp() self.crtk_utils.add_servo_cp() + # for all examples self.duration = 10 # 10 seconds self.rate = 200 # aiming for 200 Hz self.samples = self.duration * self.rate - def run_servo_cp(self): - if not self.enable(60): + def run(self): + self.ral.check_connections() + + if not self.enable(30): print("Unable to enable the device, make sure it is connected.") return + if not self.home(30): + print('Unable to home the device, make sure it is connected.') + return + # create a new goal starting with current position start= PyKDL.Frame() start.p = self.setpoint_cp().p @@ -53,21 +53,26 @@ def run_servo_cp(self): goal.p = self.setpoint_cp().p goal.M = self.setpoint_cp().M amplitude = 0.01 # 2 centimeters + + sleep_rate = self.ral.create_rate(self.rate) for i in range(self.samples): goal.p[0] = start.p[0] + amplitude * (1.0 - math.cos(i * math.radians(360.0) / self.samples)) goal.p[1] = start.p[1] + amplitude * (1.0 - math.cos(i * math.radians(360.0) / self.samples)) self.servo_cp(goal) - rospy.sleep(1.0 / self.rate) + sleep_rate.sleep() + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('namespace', type = str, help = 'ROS namespace for CRTK device') + app_args = crtk.ral.parse_argv(sys.argv[1:]) # process and remove ROS args + args = parser.parse_args(app_args) + + example_name = type(crtk_servo_cp_example).__name__ + ral = crtk.ral(example_name, args.namespace) + example = crtk_servo_cp_example(ral) + ral.spin_and_execute(example.run) + -# use the class now, i.e. main program if __name__ == '__main__': - try: - if (len(sys.argv) != 2): - print(sys.argv[0], ' requires one argument, i.e. crtk device namespace') - else: - example = crtk_servo_cp_example() - example.configure(sys.argv[1]) - example.run_servo_cp() - - except rospy.ROSInterruptException: - pass + main() diff --git a/scripts/crtk_servo_cv_example.py b/scripts/crtk_servo_cv_example.py old mode 100644 new mode 100755 index 868bb36..9f9b6b7 --- a/scripts/crtk_servo_cv_example.py +++ b/scripts/crtk_servo_cv_example.py @@ -10,60 +10,57 @@ # > rosrun dvrk_robot dvrk_console_json -j # To communicate with the arm using ROS topics, see the python based example dvrk_arm_test.py: -# > rosrun crtk_python_client crtk_arm_test.py +# > rosrun crtk_python_client crtk_servo_cv_example.py +import argparse import crtk -import math -import sys -import rospy import numpy as np -import PyKDL +import sys -# example of application using device.py class crtk_servo_cv_example: + def __init__(self, ral): + self.ral = ral - # configuration - def configure(self, device_namespace): - # ROS initialization - if not rospy.get_node_uri(): - rospy.init_node('crtk_servo_cv_example', anonymous = True, log_level = rospy.WARN) - - print(rospy.get_caller_id() + ' -> configuring crtk_device_test for: ' + device_namespace) # populate this class with all the ROS topics we need - self.crtk_utils = crtk.utils(self, device_namespace) + self.crtk_utils = crtk.utils(self, ral) self.crtk_utils.add_operating_state() self.crtk_utils.add_setpoint_cp() self.crtk_utils.add_servo_cv() - # for all examples - self.duration = 10 # 10 seconds - self.rate = 200 # aiming for 200 Hz - self.samples = self.duration * self.rate - def run_servo_cv(self): - if not self.enable(60): + self.duration = 10 # seconds + + def run(self): + self.ral.check_connections() + + if not self.enable(30): print("Unable to enable the device, make sure it is connected.") return - # create a new goal swith constant speed - for i in range(self.samples): - vel = np.array([0.05, 0.0, 0.0, 0.0, 0.0, 0.0]) # move 5 cm/sec along x direction - self.servo_cv(vel) - rospy.sleep(1.0 / self.rate) + if not self.home(30): + print('Unable to home the device, make sure it is connected.') + return - # send the zero velcity to stop the robot - vel = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + # create a new goal with constant speed + vel = np.array([1e-3, 0.0, 0.0, 0.0, 0.0, 0.0]) # move 1 mm/sec along x direction self.servo_cv(vel) + self.ral.create_rate(1/self.duration).sleep() # sleep once for 'duration' seconds + + # command zero velcity to stop the robot + vel = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.servo_cv(vel) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('namespace', type = str, help = 'ROS namespace for CRTK device') + app_args = crtk.ral.parse_argv(sys.argv[1:]) # process and remove ROS args + args = parser.parse_args(app_args) + + example_name = type(crtk_servo_cv_example).__name__ + ral = crtk.ral(example_name, args.namespace) + example = crtk_servo_cv_example(ral) + ral.spin_and_execute(example.run) -# use the class now, i.e. main program if __name__ == '__main__': - try: - if (len(sys.argv) != 2): - print(sys.argv[0], ' requires one argument, i.e. crtk device namespace') - else: - example = crtk_servo_cv_example() - example.configure(sys.argv[1]) - example.run_servo_cv() - - except rospy.ROSInterruptException: - pass \ No newline at end of file + main() diff --git a/scripts/crtk_servo_jp_example.py b/scripts/crtk_servo_jp_example.py index fc9a485..ae5e7e6 100755 --- a/scripts/crtk_servo_jp_example.py +++ b/scripts/crtk_servo_jp_example.py @@ -10,60 +10,66 @@ # > rosrun dvrk_robot dvrk_console_json -j # To communicate with the arm using ROS topics, see the python based example dvrk_arm_test.py: -# > rosrun crtk_python_client crtk_arm_test.py +# > rosrun crtk_python_client crtk_servo_jp_example.py +import argparse import crtk import math -import sys -import rospy import numpy -import PyKDL +import sys -# example of application using device.py class crtk_servo_jp_example: + def __init__(self, ral): + self.ral = ral - # configuration - def configure(self, device_namespace): - # ROS initialization - if not rospy.get_node_uri(): - rospy.init_node('crtk_servo_jp_example', anonymous = True, log_level = rospy.WARN) - - print(rospy.get_caller_id() + ' -> configuring crtk_device_test for: ' + device_namespace) # populate this class with all the ROS topics we need - self.crtk_utils = crtk.utils(self, device_namespace) + self.crtk_utils = crtk.utils(self, ral) self.crtk_utils.add_operating_state() self.crtk_utils.add_setpoint_js() self.crtk_utils.add_servo_jp() - # for all examples + self.duration = 10 # 10 seconds - self.rate = 500 # aiming for 200 Hz - self.samples = self.duration * self.rate + self.rate = 200 # aiming for 200 Hz + + def run(self): + self.ral.check_connections() - def run_servo_jp(self): - if not self.enable(60): + if not self.enable(30): print("Unable to enable the device, make sure it is connected.") return + if not self.home(30): + print('Unable to home the device, make sure it is connected.') + return + # create a new goal starting with current position start_jp = numpy.copy(self.setpoint_jp()) goal = numpy.copy(self.setpoint_jp()) amplitude = math.radians(10.0) # +/- 10 degrees - for i in range(self.samples): - goal[0] = start_jp[0] + amplitude * (1.0 - math.cos(i * math.radians(360.0) / self.samples)) - goal[1] = start_jp[1] + amplitude * (1.0 - math.cos(i * math.radians(360.0) / self.samples)) + + sleep_rate = self.ral.create_rate(self.rate) + samples = self.duration * self.rate + for i in range(samples): + sine = math.sin(math.radians(360.0) * float(i) / samples) + angle = amplitude * sine + goal[0] = start_jp[0] + angle + goal[1] = start_jp[1] + angle self.servo_jp(goal) - rospy.sleep(1.0 / self.rate) + sleep_rate.sleep() + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('namespace', type = str, help = 'ROS namespace for CRTK device') + app_args = crtk.ral.parse_argv(sys.argv[1:]) # process and remove ROS args + args = parser.parse_args(app_args) + + example_name = type(crtk_servo_jp_example).__name__ + ral = crtk.ral(example_name, args.namespace) + example = crtk_servo_jp_example(ral) + ral.spin_and_execute(example.run) + -# use the class now, i.e. main program if __name__ == '__main__': - try: - if (len(sys.argv) != 2): - print(sys.argv[0], ' requires one argument, i.e. crtk device namespace') - else: - example = crtk_servo_jp_example() - example.configure(sys.argv[1]) - example.run_servo_jp() - - except rospy.ROSInterruptException: - pass + main() diff --git a/scripts/crtk_teleop_example.py b/scripts/crtk_teleop_example.py index 844f145..792b382 100755 --- a/scripts/crtk_teleop_example.py +++ b/scripts/crtk_teleop_example.py @@ -14,65 +14,62 @@ # > rosrun sensable_phantom_ros sensable_phantom -j sawSensablePhantomRight.json # To communicate with the device using ROS topics, see the python based example: -# > rosrun crtk_python_client crtk_teleop_example +# > rosrun crtk_python_client crtk_teleop_example +import argparse import crtk import math -import sys -import rospy -import numpy import PyKDL +import rospy +import sys + if sys.version_info.major < 3: input = raw_input + # example of application using device.py class crtk_teleop_example: - # populate master with the ROS topics we need class Master: - def __init__(self, namespace): - self.crtk = crtk.utils(self, namespace) + def __init__(self, ral): + # populate master with the ROS topics we need + self.crtk = crtk.utils(self, ral) self.crtk.add_operating_state() self.crtk.add_measured_cp() - class Gripper: - def __init__(self, namespace): - self.crtk = crtk.utils(self, namespace) - self.crtk.add_measured_js() - - # populate puppet with the ROS topics we need class Puppet: - def __init__(self, namespace): - self.crtk = crtk.utils(self, namespace) + def __init__(self, ral): + # populate puppet with the ROS topics we need + self.crtk = crtk.utils(self, ral) self.crtk.add_operating_state() self.crtk.add_setpoint_cp() self.crtk.add_servo_cp() + class Gripper: + def __init__(self, ral): + self.crtk = crtk.utils(self, ral) + self.crtk.add_measured_js() + class Jaw: - def __init__(self, namespace): - self.crtk = crtk.utils(self, namespace) + def __init__(self, ral): + self.crtk = crtk.utils(self, ral) self.crtk.add_setpoint_js() self.crtk.add_servo_jp() - # configuration - def configure(self, master_namespace, puppet_namespace, gripper_namespace, jaw_namespace): - # ROS initialization - if not rospy.get_node_uri(): - rospy.init_node('crtk_teleop_example', anonymous = True, log_level = rospy.WARN) - - self.master = self.Master(master_namespace) - self.puppet = self.Puppet(puppet_namespace) - self.has_gripper = False; + def __init__(self, ral, master_namespace, puppet_namespace, gripper_namespace, jaw_namespace): + self.master = self.Master(ral.create_child(master_namespace)) + self.puppet = self.Puppet(ral.create_child(puppet_namespace)) + self.has_gripper = False if ((gripper_namespace != '') and (jaw_namespace != '')): self.has_gripper = True - self.gripper = self.Gripper(gripper_namespace) - self.jaw = self.Jaw(jaw_namespace) + self.gripper = self.Gripper(ral.create_child(gripper_namespace)) + self.jaw = self.Jaw(ral.create_child(jaw_namespace)) - # for all examples - self.duration = 10 # 10 seconds - self.rate = 500 # aiming for 200 Hz + self.duration = 10 # seconds + self.rate = 500 # aiming for 500 Hz + self.sleep_rate = ral.create_rate(self.rate) self.samples = self.duration * self.rate # main loop @@ -146,22 +143,24 @@ def run_teleop(self): else: if (abs(self.gripper.measured_jp()[0] - self.jaw.setpoint_jp()[0]) < math.radians(5.0)): gripper_started = True - rospy.sleep(1.0 / self.rate) -# use the class now, i.e. main program -if __name__ == '__main__': - try: - if (len(sys.argv) == 3): - example = crtk_teleop_example() - example.configure(sys.argv[1], sys.argv[2], '', '') - example.run() - else: - if (len(sys.argv) == 5): - example = crtk_teleop_example() - example.configure(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4]) - example.run() - else: - print(sys.argv[0], ' requires two or four arguments, i.e. master and puppet namespaces [master gripper and pupper jaw namespaces]') + self.sleep_rate.sleep() - except rospy.ROSInterruptException: - pass + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('master', type = str, help = 'ROS namespace for master CRTK device') + parser.add_argument('puppet', type = str, help = 'ROS namespace for puppet CRTK device') + parser.add_argument('-g', '--gripper', type = str, default = '', help = 'absolute ROS namespace for (optional) master gripper') + parser.add_argument('-j', '--jaw', type = str, default = '', help = 'absolute ROS namespace for (optional) puppet jaw') + app_args = crtk.ral.parse_argv(sys.argv[1:]) # process and remove ROS args + args = parser.parse_args(app_args) + + example_name = type(crtk_teleop_example).__name__ + ral = crtk.ral(example_name) + + example = crtk_teleop_example(ral, args.master, args.puppet, args.gripper, args.jaw) + ral.spin_and_execute(example.run) + +if __name__ == '__main__': + main() diff --git a/src/crtk/__init__.py b/src/crtk/__init__.py index 5058484..092f1df 100755 --- a/src/crtk/__init__.py +++ b/src/crtk/__init__.py @@ -4,16 +4,17 @@ # Copyright (c) 2016-2023 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute # Released under MIT License -__all__ = ["ros_12", "wait_move_handle", "utils", "joystick_button", "measured_cp"] +__all__ = ["ral", "wait_move_handle", "utils", "joystick_button", "measured_cp"] -# wrappers -from .ros_12 import ros_12 +# ros abstraction layer +from .ral import ral # handle classes from .wait_move_handle import wait_move_handle # utilities from .utils import utils +from .msg_conversions import * from .joystick_button import joystick_button # example classes diff --git a/src/crtk/joystick_button.py b/src/crtk/joystick_button.py index d314d3d..82ecb49 100644 --- a/src/crtk/joystick_button.py +++ b/src/crtk/joystick_button.py @@ -5,42 +5,36 @@ # Released under MIT License import threading -import rospy import sensor_msgs.msg class joystick_button: - def __init__(self, button_name, index = 0): - self.__button_name = button_name - self.__index = index - self.__value = None - self.__user_callback = None - self.__event = threading.Event() - self.__subscriber = rospy.Subscriber(button_name, - sensor_msgs.msg.Joy, self.__callback, - queue_size = 10) + def __init__(self, ral, button_name, index = 0): + self._index = index + self._value = None + self._user_callback = None + self._event = threading.Event() + self._subscriber = ral.subscriber(button_name, sensor_msgs.msg.Joy, + self._callback, queue_size = 10) - def __del__(self): - self.__subscriber.unregister() - - def __callback(self, event): - self.__value = event.buttons[self.__index] - self.__event.set() - if not self.__user_callback is None: - self.__user_callback(self.__value) + def _callback(self, event): + self._value = event.buttons[self._index] + self._event.set() + if not self._user_callback is None: + self._user_callback(self._value) def set_callback(self, callback = None): - self.__user_callback = callback + self._user_callback = callback def value(self): - return self.__value + return self._value def wait(self, timeout = None, value = None): ready = False while not ready: - self.__event.clear() - if not self.__event.wait(timeout): + self._event.clear() + if not self._event.wait(timeout): return False else: - if value == None or value == self.__value: + if value == None or value == self._value: ready = True return False diff --git a/src/crtk/msg_conversions.py b/src/crtk/msg_conversions.py new file mode 100644 index 0000000..cd92717 --- /dev/null +++ b/src/crtk/msg_conversions.py @@ -0,0 +1,139 @@ +import PyKDL +import numpy +import geometry_msgs.msg + + +def FrameFromTransformMsg(t): + """ + :param t: input transform + :type t: :class:`geometry_msgs.msg.Transform` + :return: New :class:`PyKDL.Frame` object + + Convert a transform represented as a ROS Transform message to a :class:`PyKDL.Frame`. + There must be a standard package to perform this conversion, if you find it, please remove this code. + """ + return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.rotation.x, + t.rotation.y, + t.rotation.z, + t.rotation.w), + PyKDL.Vector(t.translation.x, + t.translation.y, + t.translation.z)) + +def FrameToTransformMsg(f): + """ + :param f: input frame + :type f: :class:`PyKDL.Frame` + + Return a ROS Transform message for the Frame f. + There must be a standard package to perform this conversion, if you find it, please remove this code. + """ + t = geometry_msgs.msg.Transform() + t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w = f.M.GetQuaternion() + t.translation.x = f.p[0] + t.translation.y = f.p[1] + t.translation.z = f.p[2] + return t + + +def FrameFromPoseMsg(p): + """ + :param p: input pose + :type p: :class:`geometry_msgs.msg.Pose` + :return: New :class:`PyKDL.Frame` object + + Convert a pose represented as a ROS Pose message to a :class:`PyKDL.Frame`. + There must be a standard package to perform this conversion, if you find it, please remove this code. + """ + return PyKDL.Frame(PyKDL.Rotation.Quaternion(p.orientation.x, + p.orientation.y, + p.orientation.z, + p.orientation.w), + PyKDL.Vector(p.position.x, + p.position.y, + p.position.z)) + +def FrameToPoseMsg(f): + """ + :param f: input pose + :type f: :class:`PyKDL.Frame` + + Return a ROS Pose message for the Frame f. + There must be a standard package to perform this conversion, if you find it, please remove this code. + """ + p = geometry_msgs.msg.Pose() + p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w = f.M.GetQuaternion() + p.position.x = f.p[0] + p.position.y = f.p[1] + p.position.z = f.p[2] + return p + + +def ArrayFromTwistMsg(t): + """ + :param t: input twist + :type t: :class:`geometry_msgs.msg.Twist` object + + Return a NumPy array for the twist t. + There must be a standard package to perform this conversion, if you find it, please remove this code. + """ + return numpy.array([t.linear.x, + t.linear.y, + t.linear.z, + t.angular.x, + t.angular.y, + t.angular.z]) + + +def ArrayToTwistMsg(a): + """ + :param a: input twist + :type a: list of float + + Return a ROS Twist message for the twist a. + There must be a standard package to perform this conversion, if you find it, please remove this code. + """ + t = geometry_msgs.msg.Twist() + t.linear.x = a[0] + t.linear.y = a[1] + t.linear.z = a[2] + t.angular.x = a[3] + t.angular.y = a[4] + t.angular.z = a[5] + + return t + + +def ArrayFromWrenchMsg(w): + """ + :param w: input wrench + :type w: :class:`geometry_msgs.msg.Wrench` object + + Return a NumPy array for the wrench w. + There must be a standard package to perform this conversion, if you find it, please remove this code. + """ + return numpy.array([w.force.x, + w.force.y, + w.force.z, + w.torque.x, + w.torque.y, + w.torque.z]) + + +def ArrayToWrenchMsg(a): + """ + :param a: input wrench + :type a: list of float + + Return a ROS Wrench message for the wrench w. + There must be a standard package to perform this conversion, if you find it, please remove this code. + """ + w = geometry_msgs.msg.Wrench() + w.force.x = a[0] + w.force.y = a[1] + w.force.z = a[2] + w.torque.x = a[3] + w.torque.y = a[4] + w.torque.z = a[5] + + return w diff --git a/src/crtk/ral.py b/src/crtk/ral.py new file mode 100644 index 0000000..8498752 --- /dev/null +++ b/src/crtk/ral.py @@ -0,0 +1,183 @@ +# 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 rospy + + +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 1 + + @staticmethod + def parse_argv(argv): + # strip ros arguments + return rospy.myargv(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 = [] + + if node is not None: + self._node = node + else: + # ros init node so we can use default ros arguments (e.g. __ns:= for namespace) + rospy.init_node(self.node_name(), anonymous = True) + + def __del__(self): + for pub in self._publishers: + pub.unregister() + + for sub in self._subscribers: + sub.unregister() + + 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) + self._children[child_namespace] = child + return child + + def now(self): + return rospy.Time.now() + + def get_timestamp(self, t): + if hasattr(t, 'header'): + t = t.header + if hasattr(t, 'stamp'): + t = t.stamp + + return t + + def to_sec(self, t): + t = self.get_timestamp(t) + return t.to_sec() + + def create_duration(self, d): + return rospy.Duration(d) + + def create_rate(self, rate_hz): + return rospy.Rate(rate_hz) + + def spin(self): + pass # Not applicable in ROS 1 + + def shutdown(self): + pass # Not applicable in ROS 1 + + def spin_and_execute(self, function, *arguments): + function(*arguments) + + def on_shutdown(self, callback): + rospy.on_shutdown(callback) + + def is_shutdown(self): + return rospy.is_shutdown() + + 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): + pub = rospy.Publisher(self._add_namespace_to(topic), msg_type, + latch = latch, queue_size = queue_size, *args, **kwargs) + self._publishers.append(pub) + return pub + + def subscriber(self, topic, msg_type, callback, queue_size = 10, *args, **kwargs): + sub = rospy.Subscriber(self._add_namespace_to(topic), msg_type, callback=callback, + queue_size = queue_size, *args, **kwargs) + self._subscribers.append(sub) + return sub + + def service_client(self, name, srv_type): + client = rospy.ServiceProxy(self._add_namespace_to(name), srv_type) + self._services.append(client) + return client + + def get_topic_name(self, pubsub): + return pubsub.name + + def _check_connections(self, start_time, timeout_duration, check_children): + check_rate = self.create_rate(100) + connected = lambda pubsub: pubsub.get_num_connections() > 0 + + # 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.format(connected_pub_count)) + + 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) diff --git a/src/crtk/ros_12.py b/src/crtk/ros_12.py deleted file mode 100644 index 62f9d22..0000000 --- a/src/crtk/ros_12.py +++ /dev/null @@ -1,46 +0,0 @@ -# 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 rospy - -class ros_12: - """Class used to wrap rospy features so we can write code independent - of ROS version. - """ - - def __init__(self, node_name, namespace = ''): - self.__node_name = node_name - self.__namespace = namespace - self.__rate_in_Hz = 0.0 - # ros init node so we can use default ros arguments (e.g. __ns:= for namespace) - rospy.init_node(self.__node_name, anonymous = True) - - @staticmethod - def ros_version(): - return 1 - - @staticmethod - def parse_argv(argv): - # strip ros arguments - return rospy.myargv(argv) - - def set_rate(self, rate_in_Hz): - self.__rate_in_Hz = rate_in_Hz - self.__ros_rate = rospy.Rate(rate_in_Hz) - - def sleep(self): - if self.__rate_in_Hz == 0.0: - raise RuntimeError('set_rate must be called before sleep') - self.__ros_rate.sleep() - - def node_name(self): - return self.__node_name - - def namespace(self): - return self.__namespace - - def spin_and_execute(self, function, *arguments): - function(*arguments) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index d539533..138526d 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -5,68 +5,17 @@ # Released under MIT License import threading -import time -import rospy import numpy -import PyKDL import std_msgs.msg import geometry_msgs.msg import sensor_msgs.msg -import tf_conversions.posemath import crtk_msgs.msg import crtk_msgs.srv import crtk.wait_move_handle -def TransformFromMsg(t): - """ - :param p: input pose - :type p: :class:`geometry_msgs.msg.Pose` - :return: New :class:`PyKDL.Frame` object - - Convert a pose represented as a ROS Pose message to a :class:`PyKDL.Frame`. - """ - return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.rotation.x, - t.rotation.y, - t.rotation.z, - t.rotation.w), - PyKDL.Vector(t.translation.x, - t.translation.y, - t.translation.z)) - -def TransformToMsg(f): - """ - :param f: input pose - :type f: :class:`PyKDL.Frame` - - Return a ROS Pose message for the Frame f. +import crtk.msg_conversions as msg_conv - """ - m = geometry_msgs.msg.TransformStamped() - t = m.transform - t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w = f.M.GetQuaternion() - t.translation.x = f.p[0] - t.translation.y = f.p[1] - t.translation.z = f.p[2] - return m - - -def TwistFromMsg(t): - return numpy.array([t.linear.x, - t.linear.y, - t.linear.z, - t.angular.x, - t.angular.y, - t.angular.z]) - - -def WrenchFromMsg(w): - return numpy.array([w.force.x, - w.force.y, - w.force.z, - w.torque.x, - w.torque.y, - w.torque.z]) class utils: """Class containing methods used to populate the interface @@ -79,48 +28,25 @@ class utils: move command). class_instance : object that will be populated - ros_namespace : ROS namespace for the CRTK commands used by the device + ral : ral object for the device namespace expected_interval : expected interval at which the device sends its motion state (measured, setpoint, goal) """ def __init__(self, class_instance, - ros_namespace, + ral, expected_interval = 0.02, operating_state_instance = None): self.__class_instance = class_instance self.__operating_state_instance = operating_state_instance - self.__ros_namespace = ros_namespace + self.__ral = ral self.__expected_interval = expected_interval - self.__subscribers = [] - self.__publishers = [] - self.__service_proxies = [] - self.__attributes = [] - rospy.on_shutdown(self.__ros_shutdown) - - def __del__(self): - self.remove_all() + self.__ral.on_shutdown(self.__ros_shutdown) def __ros_shutdown(self): if hasattr(self, '_utils__operating_state_event'): self.__operating_state_event.set() - - def __now(self): - return rospy.Time.now() - - - def remove_all(self): - for sub in self.__subscribers: - sub.unregister() - for pub in self.__publishers: - pub.unregister() - for attr in self.__attributes: - dir(self.__class_instance) - delattr(self.__class_instance, attr) - dir(self.__class_instance) - - def __wait_for_valid_data(self, data, event, age, wait): event.clear() if age is None: @@ -129,56 +55,18 @@ def __wait_for_valid_data(self, data, event, age, wait): wait = self.__expected_interval # check if user accepts cached data if age != 0.0: - data_age = self.__now() - data.header.stamp - if data_age <= rospy.Duration(age): + data_age = self.__ral.now() - self.__ral.get_timestamp(data) + if data_age <= self.__ral.create_duration(age): return True if wait != 0.0: if event.wait(wait): return True return False - # 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 - def check_connections(self, timeout_seconds): - if timeout_seconds <= 0.0: - return - - start_time = rospy.Time.now() - timeout_duration = rospy.Duration(timeout_seconds) - - connected = lambda pubsub: pubsub.get_num_connections() > 0 - - # wait at most timeout_seconds for all connections to establish - while (rospy.Time.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 - - rospy.sleep(0.01) - - # last check of connection status, raise error if any remain unconnected - unconnected_publishers = [p for p in self.__publishers if not connected(p)] - unconnected_subscribers = [s for s in self.__subscribers if not connected(s)] - if len(unconnected_publishers) == 0 and len(unconnected_subscribers) == 0: - return - - err_msg = \ - ( - f"Timed out while waiting for publisher/subscriber connections to establish\n" - f" Publishers: {len(self.__publishers) - len(unconnected_publishers)} connected, {len(unconnected_publishers)} not connected\n" - f" not connected: [{', '.join([p.name for p in unconnected_publishers])}]\n\n" - f" Subscribers: {len(self.__subscribers) - len(unconnected_subscribers)} connected, {len(unconnected_subscribers)} not connected\n" - f" not connected: [{', '.join([s.name for s in unconnected_subscribers])}]\n\n" - ) - raise TimeoutError(err_msg) - # internal methods to manage state def __operating_state_cb(self, msg): # crtk operating state contains state as well as homed and busy self.__operating_state_data = msg - # then when all data is saved, release "lock" self.__operating_state_event.set() @@ -187,24 +75,26 @@ def __operating_state(self, extra = None): return self.__operating_state_data.state else: return [self.__operating_state_data.state, - self.__operating_state_data.header.stamp.to_sec()] + self.__ral.to_sec(self.__operating_state_data)] def __wait_for_operating_state(self, expected_state, timeout): if timeout < 0.0: return False - start_time = self.__now() + start_time = self.__ral.now() in_time = self.__operating_state_event.wait(timeout) - if rospy.is_shutdown(): - return False; + if self.__ral.is_shutdown(): + return False + if in_time: # within timeout and result we expected if self.__operating_state_data.state == expected_state: return True else: # wait a bit more - elapsed_time = self.__now() - start_time + elapsed_time = self.__ral.to_sec(self.__ral.now() - start_time) self.__operating_state_event.clear() - return self.__wait_for_operating_state(expected_state, timeout - elapsed_time.to_sec()) + return self.__wait_for_operating_state(expected_state = expected_state, + timeout = timeout - elapsed_time) # past timeout return False @@ -244,24 +134,27 @@ def __is_homed(self, extra = None): return self.__operating_state_data.is_homed else: return [self.__operating_state_data.is_homed, - self.__operating_state_data.header.stamp.to_sec()] + self.__ral.to_sec(self.__operating_state_data)] def __wait_for_homed(self, timeout, expected_homed): if timeout < 0.0: return False - start_time = self.__now() + start_time = self.__ral.now() self.__operating_state_event.clear() in_time = self.__operating_state_event.wait(timeout) - if rospy.is_shutdown(): - return False; + if self.__ral.is_shutdown(): + return False + if in_time: # within timeout and result we expected if (self.__operating_state_data.is_homed == expected_homed) and (not self.__operating_state_data.is_busy): return True else: # wait a bit more - elapsed_time = self.__now() - start_time - return self.__wait_for_homed(timeout - elapsed_time.to_sec(), expected_homed) + elapsed_time = self.__ral.to_sec(self.__ral.now() - start_time) + self.__operating_state_event.clear() + return self.__wait_for_homed(expected_homed = expected_homed, + timeout = timeout - elapsed_time) # past timeout return False @@ -286,15 +179,15 @@ def __is_busy(self, extra = None): # set start time to now if not specified if start_time is None: - start_time = self.__now() + start_time = self.__ral.now() result = True - if (self.__operating_state_data.header.stamp > start_time): + if self.__ral.get_timestamp(self.__operating_state_data) > start_time: result = self.__operating_state_data.is_busy if not extra: return result else: return [result, - self.__operating_state_data.header.stamp.to_sec()] + self.__ral.to_sec(self.__operating_state_data)] def __wait_for_busy(self, is_busy = False, @@ -305,32 +198,33 @@ def __wait_for_busy(self, return False # set start time to now if not specified if start_time is None: - start_time = self.__now() + start_time = self.__ral.now() else: # user provided start_time, check if an event arrived after start_time - last_event_time = self.__operating_state_data.header.stamp + last_event_time = self.__ral.get_timestamp(self.__operating_state_data) if (last_event_time > start_time and self.__operating_state_data.is_busy == is_busy): return True # other cases, waiting for an operating_state event - _start_time = self.__now() + _start_time = self.__ral.now() self.__operating_state_event.clear() in_time = self.__operating_state_event.wait(timeout) - if rospy.is_shutdown() or not in_time: - return False; + # past timeout + if self.__ral.is_shutdown() or not in_time: + return False # within timeout and result we expected if self.__operating_state_data.is_busy == is_busy: return True else: # wait a bit more - elapsed_time = self.__now() - _start_time + elapsed_time = self.__ral.to_sec(self.__ral.now() - _start_time) self.__operating_state_event.clear() return self.__wait_for_busy(is_busy = is_busy, start_time = start_time, - timeout = (timeout - elapsed_time.to_sec())) + timeout = timeout - elapsed_time) - def add_operating_state(self, optional_ros_namespace = None): + def add_operating_state(self): # throw a warning if this has alread been added to the class, # using the callback name to test if hasattr(self.__class_instance, 'operating_state'): @@ -339,21 +233,20 @@ def add_operating_state(self, optional_ros_namespace = None): self.__operating_state_data = crtk_msgs.msg.OperatingState() self.__operating_state_event = threading.Event() - # determine namespace to use - if optional_ros_namespace is None: - namespace_to_use = self.__ros_namespace - else: - namespace_to_use = optional_ros_namespace - - # create the subscriber/publisher and keep in list - self.__operating_state_subscriber = rospy.Subscriber(namespace_to_use + '/operating_state', - crtk_msgs.msg.OperatingState, self.__operating_state_cb, - queue_size = 10) - self.__subscribers.append(self.__operating_state_subscriber) - self.__state_command_publisher = rospy.Publisher(namespace_to_use + '/state_command', - crtk_msgs.msg.StringStamped, - latch = True, queue_size = 1) - self.__publishers.append(self.__state_command_publisher) + # create the subscriber/publisher + self.__operating_state_subscriber = self.__ral.subscriber( + 'operating_state', + crtk_msgs.msg.OperatingState, + self.__operating_state_cb, + queue_size = 10 + ) + + self.__state_command_publisher = self.__ral.publisher( + 'state_command', + crtk_msgs.msg.StringStamped, + latch = True, queue_size = 10 + ) + # add attributes to class instance self.__class_instance.operating_state = self.__operating_state self.__class_instance.wait_for_operating_state = self.__wait_for_operating_state @@ -370,7 +263,7 @@ def add_operating_state(self, optional_ros_namespace = None): if not self.__operating_state_instance: self.__operating_state_instance = self.__class_instance else: - raise RuntimeWarning('over writting operating state for ' + self.__ros_namespace) + raise RuntimeWarning('over writting operating state for ' + self.__ral.namespace()) # internal methods for setpoint_js def __setpoint_js_cb(self, msg): @@ -384,8 +277,8 @@ def __setpoint_js(self, age = None, wait = None): return [numpy.array(self.__setpoint_js_data.position), numpy.array(self.__setpoint_js_data.velocity), numpy.array(self.__setpoint_js_data.effort), - self.__setpoint_js_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get setpoint_js') + self.__ral.to_sec(self.__setpoint_js_data)] + raise RuntimeWarning('unable to get setpoint_js ({})'.format(self.__ral.get_topic_name(self.__setpoint_js_subscriber))) def __setpoint_jp(self, age = None, wait = None, extra = None): """Joint Position Setpoint. Default age and wait are set to @@ -402,9 +295,8 @@ def __setpoint_jp(self, age = None, wait = None, extra = None): return numpy.array(self.__setpoint_js_data.position) else: return [numpy.array(self.__setpoint_js_data.position), - self.__setpoint_js_data.header.stamp.to_sec()] - - raise RuntimeWarning('unable to get setpoint_jp in namespace ' + self.__ros_namespace) + self.__ral.to_sec(self.__setpoint_js_data)] + raise RuntimeWarning('unable to get setpoint_jp ({})'.format(self.__ral.get_topic_name(self.__setpoint_js_subscriber))) def __setpoint_jv(self, age = None, wait = None, extra = None): if self.__wait_for_valid_data(self.__setpoint_js_data, @@ -414,8 +306,8 @@ def __setpoint_jv(self, age = None, wait = None, extra = None): return numpy.array(self.__setpoint_js_data.velocity) else: return [numpy.array(self.__setpoint_js_data.velocity), - self.__setpoint_js_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get setpoint_jv') + self.__ral.to_sec(self.__setpoint_js_data)] + raise RuntimeWarning('unable to get setpoint_jv ({})'.format(self.__ral.get_topic_name(self.__setpoint_js_subscriber))) def __setpoint_jf(self, age = None, wait = None, extra = None): if self.__wait_for_valid_data(self.__setpoint_js_data, @@ -425,8 +317,8 @@ def __setpoint_jf(self, age = None, wait = None, extra = None): return numpy.array(self.__setpoint_js_data.effort) else: return [numpy.array(self.__setpoint_js_data.effort), - self.__setpoint_js_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get setpoint_jf') + self.__ral.to_sec(self.__setpoint_js_data)] + raise RuntimeWarning('unable to get setpoint_jf ({})'.format(self.__ral.get_topic_name(self.__setpoint_js_subscriber))) def add_setpoint_js(self): # throw a warning if this has alread been added to the class, @@ -436,11 +328,13 @@ def add_setpoint_js(self): # data self.__setpoint_js_data = sensor_msgs.msg.JointState() self.__setpoint_js_event = threading.Event() - # create the subscriber and keep in list - self.__setpoint_js_subscriber = rospy.Subscriber(self.__ros_namespace + '/setpoint_js', - sensor_msgs.msg.JointState, - self.__setpoint_js_cb) - self.__subscribers.append(self.__setpoint_js_subscriber) + # create the subscriber + self.__setpoint_js_subscriber = self.__ral.subscriber( + 'setpoint_js', + sensor_msgs.msg.JointState, + self.__setpoint_js_cb, + queue_size = 10 + ) # add attributes to class instance self.__class_instance.setpoint_js = self.__setpoint_js self.__class_instance.setpoint_jp = self.__setpoint_jp @@ -460,11 +354,11 @@ def __setpoint_cp(self, age = None, wait = None, extra = None): self.__setpoint_cp_event, age, wait): if not extra: - return tf_conversions.posemath.fromMsg(self.__setpoint_cp_data.pose) + return msg_conv.FrameFromPoseMsg(self.__setpoint_cp_data.pose) else: - return [tf_conversions.posemath.fromMsg(self.__setpoint_cp_data.pose), - self.__setpoint_cp_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get setpoint_cp') + return [msg_conv.FrameFromPoseMsg(self.__setpoint_cp_data.pose), + self.__ral.to_sec(self.__setpoint_cp_data)] + raise RuntimeWarning('unable to get setpoint_cp ({})'.format(self.__ral.get_topic_name(self.__setpoint_cp_subscriber))) def add_setpoint_cp(self): # throw a warning if this has alread been added to the class, @@ -475,11 +369,13 @@ def add_setpoint_cp(self): self.__setpoint_cp_data = geometry_msgs.msg.PoseStamped() self.__setpoint_cp_event = threading.Event() self.__setpoint_cp_lock = False - # create the subscriber and keep in list - self.__setpoint_cp_subscriber = rospy.Subscriber(self.__ros_namespace + '/setpoint_cp', - geometry_msgs.msg.PoseStamped, - self.__setpoint_cp_cb) - self.__subscribers.append(self.__setpoint_cp_subscriber) + # create the subscriber + self.__setpoint_cp_subscriber = self.__ral.subscriber( + 'setpoint_cp', + geometry_msgs.msg.PoseStamped, + self.__setpoint_cp_cb, + queue_size = 10 + ) # add attributes to class instance self.__class_instance.setpoint_cp = self.__setpoint_cp @@ -496,8 +392,8 @@ def __measured_js(self, age = None, wait = None): return [numpy.array(self.__measured_js_data.position), numpy.array(self.__measured_js_data.velocity), numpy.array(self.__measured_js_data.effort), - self.__measured_js_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get measured_js') + self.__ral.to_sec(self.__measured_js_data)] + raise RuntimeWarning('unable to get measured_js ({})'.format(self.__ral.get_topic_name(self.__measured_js_subscriber))) def __measured_jp(self, age = None, wait = None, extra = None): if self.__wait_for_valid_data(self.__measured_js_data, @@ -507,8 +403,8 @@ def __measured_jp(self, age = None, wait = None, extra = None): return numpy.array(self.__measured_js_data.position) else: return [numpy.array(self.__measured_js_data.position), - self.__measured_js_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get measured_jp') + self.__ral.to_sec(self.__measured_js_data)] + raise RuntimeWarning('unable to get measured_jp ({})'.format(self.__ral.get_topic_name(self.__measured_js_subscriber))) def __measured_jv(self, age = None, wait = None, extra = None): if self.__wait_for_valid_data(self.__measured_js_data, @@ -518,8 +414,8 @@ def __measured_jv(self, age = None, wait = None, extra = None): return numpy.array(self.__measured_js_data.velocity) else: return [numpy.array(self.__measured_js_data.velocity), - self.__measured_js_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get measured_jv') + self.__ral.to_sec(self.__measured_js_data)] + raise RuntimeWarning('unable to get measured_jv ({})'.format(self.__ral.get_topic_name(self.__measured_js_subscriber))) def __measured_jf(self, age = None, wait = None, extra = None): if self.__wait_for_valid_data(self.__measured_js_data, @@ -529,8 +425,8 @@ def __measured_jf(self, age = None, wait = None, extra = None): return numpy.array(self.__measured_js_data.effort) else: return [numpy.array(self.__measured_js_data.effort), - self.__measured_js_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get measured_jf') + self.__ral.to_sec(self.__measured_js_data)] + raise RuntimeWarning('unable to get measured_jf ({})'.format(self.__ral.get_topic_name(self.__measured_js_subscriber))) def add_measured_js(self): # throw a warning if this has alread been added to the class, @@ -540,11 +436,12 @@ def add_measured_js(self): # data self.__measured_js_data = sensor_msgs.msg.JointState() self.__measured_js_event = threading.Event() - # create the subscriber and keep in list - self.__measured_js_subscriber = rospy.Subscriber(self.__ros_namespace + '/measured_js', - sensor_msgs.msg.JointState, - self.__measured_js_cb) - self.__subscribers.append(self.__measured_js_subscriber) + # create the subscriber + self.__measured_js_subscriber = self.__ral.subscriber( + 'measured_js', + sensor_msgs.msg.JointState, + self.__measured_js_cb + ) # add attributes to class instance self.__class_instance.measured_js = self.__measured_js self.__class_instance.measured_jp = self.__measured_jp @@ -562,11 +459,11 @@ def __measured_cp(self, age = None, wait = None, extra = None): self.__measured_cp_event, age, wait): if not extra: - return tf_conversions.posemath.fromMsg(self.__measured_cp_data.pose) + return msg_conv.FrameFromPoseMsg(self.__measured_cp_data.pose) else: - return [tf_conversions.posemath.fromMsg(self.__measured_cp_data.pose), - self.__measured_cp_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get measured_cp') + return [msg_conv.FrameFromPoseMsg(self.__measured_cp_data.pose), + self.__ral.to_sec(self.__measured_cp_data)] + raise RuntimeWarning('unable to get measured_cp ({})'.format(self.__ral.get_topic_name(self.__measured_cp_subscriber))) def add_measured_cp(self): # throw a warning if this has alread been added to the class, @@ -576,11 +473,13 @@ def add_measured_cp(self): # data self.__measured_cp_data = geometry_msgs.msg.PoseStamped() self.__measured_cp_event = threading.Event() - # create the subscriber and keep in list - self.__measured_cp_subscriber = rospy.Subscriber(self.__ros_namespace + '/measured_cp', - geometry_msgs.msg.PoseStamped, - self.__measured_cp_cb) - self.__subscribers.append(self.__measured_cp_subscriber) + # create the subscriber + self.__measured_cp_subscriber = self.__ral.subscriber( + 'measured_cp', + geometry_msgs.msg.PoseStamped, + self.__measured_cp_cb, + queue_size = 10 + ) # add attributes to class instance self.__class_instance.measured_cp = self.__measured_cp @@ -595,11 +494,11 @@ def __measured_cv(self, age = None, wait = None, extra = None): self.__measured_cv_event, age, wait): if not extra: - return TwistFromMsg(self.__measured_cv_data.twist) + return msg_conv.ArrayFromTwistMsg(self.__measured_cv_data.twist) else: - return [TwistFromMsg(self.__measured_cv_data.twist), - self.__measured_cv_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get measured_cv') + return [msg_conv.ArrayFromTwistMsg(self.__measured_cv_data.twist), + self.__ral.to_sec(self.__measured_cv_data)] + raise RuntimeWarning('unable to get measured_cv ({})'.format(self.__ral.get_topic_name(self.__measured_cv_subscriber))) def add_measured_cv(self): # throw a warning if this has alread been added to the class, @@ -609,11 +508,13 @@ def add_measured_cv(self): # data self.__measured_cv_data = geometry_msgs.msg.TwistStamped() self.__measured_cv_event = threading.Event() - # create the subscriber and keep in list - self.__measured_cv_subscriber = rospy.Subscriber(self.__ros_namespace + '/measured_cv', - geometry_msgs.msg.TwistStamped, - self.__measured_cv_cb) - self.__subscribers.append(self.__measured_cv_subscriber) + # create the subscriber + self.__measured_cv_subscriber = self.__ral.subscriber( + 'measured_cv', + geometry_msgs.msg.TwistStamped, + self.__measured_cv_cb, + queue_size = 10 + ) # add attributes to class instance self.__class_instance.measured_cv = self.__measured_cv @@ -628,11 +529,11 @@ def __measured_cf(self, age = None, wait = None, extra = None): self.__measured_cf_event, age, wait): if not extra: - return WrenchFromMsg(self.__measured_cf_data.wrench) + return msg_conv.ArrayFromWrenchMsg(self.__measured_cf_data.wrench) else: - return [WrenchFromMsg(self.__measured_cf_data.wrench), - self.__measured_cf_data.header.stamp.to_sec()] - raise RuntimeWarning('unable to get measured_cf') + return [msg_conv.ArrayFromWrenchMsg(self.__measured_cf_data.wrench), + self.__ral.to_sec(self.__measured_cf_data)] + raise RuntimeWarning('unable to get measured_cf ({})'.format(self.__ral.get_topic_name(self.__measured_cf_subscriber))) def add_measured_cf(self): # throw a warning if this has alread been added to the class, @@ -642,11 +543,13 @@ def add_measured_cf(self): # data self.__measured_cf_data = geometry_msgs.msg.WrenchStamped() self.__measured_cf_event = threading.Event() - # create the subscriber and keep in list - self.__measured_cf_subscriber = rospy.Subscriber(self.__ros_namespace + '/measured_cf', - geometry_msgs.msg.WrenchStamped, - self.__measured_cf_cb) - self.__subscribers.append(self.__measured_cf_subscriber) + # create the subscriber + self.__measured_cf_subscriber = self.__ral.subscriber( + 'measured_cf', + geometry_msgs.msg.WrenchStamped, + self.__measured_cf_cb, + queue_size = 10 + ) # add attributes to class instance self.__class_instance.measured_cf = self.__measured_cf @@ -669,16 +572,17 @@ def add_jacobian(self): # data self.__jacobian_data = std_msgs.msg.Float64MultiArray() self.__jacobian_event = threading.Event() - # create the subscriber and keep in list - self.__jacobian_subscriber = rospy.Subscriber(self.__ros_namespace + '/jacobian', - std_msgs.msg.Float64MultiArray, - self.__jacobian_cb) - self.__subscribers.append(self.__jacobian_subscriber) + # create the subscriber + self.__jacobian_subscriber = self.__ral.subscriber( + 'jacobian', + std_msgs.msg.Float64MultiArray, + self.__jacobian_cb, + queue_size = 10 + ) # add attributes to class instance self.__class_instance.jacobian = self.__jacobian - # internal methods for hold def __hold(self): # convert to ROS msg and publish @@ -690,11 +594,12 @@ def add_hold(self): # using the callback name to test if hasattr(self.__class_instance, 'hold'): raise RuntimeWarning('hold already exists') - # create the subscriber and keep in list - self.__hold_publisher = rospy.Publisher(self.__ros_namespace + '/hold', - std_msgs.msg.Empty, - latch = False, queue_size = 1) - self.__publishers.append(self.__hold_publisher) + # create the subscriber + self.__hold_publisher = self.__ral.publisher( + 'hold', + std_msgs.msg.Empty, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.hold = self.__hold @@ -710,11 +615,12 @@ def add_free(self): # using the callback name to test if hasattr(self.__class_instance, 'free'): raise RuntimeWarning('free already exists') - # create the subscriber and keep in list - self.__free_publisher = rospy.Publisher(self.__ros_namespace + '/free', - std_msgs.msg.Empty, - latch = False, queue_size = 1) - self.__publishers.append(self.__free_publisher) + # create the subscriber + self.__free_publisher = self.__ral.publisher( + 'free', + std_msgs.msg.Empty, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.free = self.__free @@ -723,8 +629,8 @@ def add_free(self): def __servo_jp(self, setpoint_p, setpoint_v = numpy.array([])): # convert to ROS msg and publish msg = sensor_msgs.msg.JointState() - msg.position[:] = setpoint_p.flat - msg.velocity[:] = setpoint_v.flat + msg.position = setpoint_p.tolist() + msg.velocity = setpoint_v.tolist() self.__servo_jp_publisher.publish(msg) def add_servo_jp(self): @@ -732,11 +638,12 @@ def add_servo_jp(self): # using the callback name to test if hasattr(self.__class_instance, 'servo_jp'): raise RuntimeWarning('servo_jp already exists') - # create the subscriber and keep in list - self.__servo_jp_publisher = rospy.Publisher(self.__ros_namespace + '/servo_jp', - sensor_msgs.msg.JointState, - latch = False, queue_size = 1) - self.__publishers.append(self.__servo_jp_publisher) + # create the subscriber + self.__servo_jp_publisher = self.__ral.publisher( + 'servo_jp', + sensor_msgs.msg.JointState, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.servo_jp = self.__servo_jp @@ -745,7 +652,7 @@ def add_servo_jp(self): def __servo_jr(self, setpoint): # convert to ROS msg and publish msg = sensor_msgs.msg.JointState() - msg.position[:] = setpoint.flat + msg.position = setpoint.tolist() self.__servo_jr_publisher.publish(msg) def add_servo_jr(self): @@ -753,11 +660,12 @@ def add_servo_jr(self): # using the callback name to test if hasattr(self.__class_instance, 'servo_jr'): raise RuntimeWarning('servo_jr already exists') - # create the subscriber and keep in list - self.__servo_jr_publisher = rospy.Publisher(self.__ros_namespace + '/servo_jr', - sensor_msgs.msg.JointState, - latch = False, queue_size = 1) - self.__publishers.append(self.__servo_jr_publisher) + # create the subscriber + self.__servo_jr_publisher = self.__ral.publisher( + 'servo_jr', + sensor_msgs.msg.JointState, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.servo_jr = self.__servo_jr @@ -766,7 +674,7 @@ def add_servo_jr(self): def __servo_cp(self, setpoint): # convert to ROS msg and publish msg = geometry_msgs.msg.PoseStamped() - msg.pose = tf_conversions.posemath.toMsg(setpoint) + msg.pose = msg_conv.FrameToPoseMsg(setpoint) self.__servo_cp_publisher.publish(msg) def add_servo_cp(self): @@ -774,11 +682,12 @@ def add_servo_cp(self): # using the callback name to test if hasattr(self.__class_instance, 'servo_cp'): raise RuntimeWarning('servo_cp already exists') - # create the subscriber and keep in list - self.__servo_cp_publisher = rospy.Publisher(self.__ros_namespace + '/servo_cp', - geometry_msgs.msg.PoseStamped, - latch = False, queue_size = 1) - self.__publishers.append(self.__servo_cp_publisher) + # create the subscriber + self.__servo_cp_publisher = self.__ral.publisher( + 'servo_cp', + geometry_msgs.msg.PoseStamped, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.servo_cp = self.__servo_cp @@ -787,7 +696,7 @@ def add_servo_cp(self): def __servo_jf(self, setpoint): # convert to ROS msg and publish msg = sensor_msgs.msg.JointState() - msg.effort[:] = setpoint.flat + msg.effort = setpoint.tolist() self.__servo_jf_publisher.publish(msg) def add_servo_jf(self): @@ -795,11 +704,12 @@ def add_servo_jf(self): # using the callback name to test if hasattr(self.__class_instance, 'servo_jf'): raise RuntimeWarning('servo_jf already exists') - # create the subscriber and keep in list - self.__servo_jf_publisher = rospy.Publisher(self.__ros_namespace + '/servo_jf', - sensor_msgs.msg.JointState, - latch = False, queue_size = 1) - self.__publishers.append(self.__servo_jf_publisher) + # create the subscriber + self.__servo_jf_publisher = self.__ral.publisher( + 'servo_jf', + sensor_msgs.msg.JointState, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.servo_jf = self.__servo_jf @@ -808,12 +718,7 @@ def add_servo_jf(self): def __servo_cf(self, setpoint): # convert to ROS msg and publish msg = geometry_msgs.msg.WrenchStamped() - msg.wrench.force.x = setpoint[0] - msg.wrench.force.y = setpoint[1] - msg.wrench.force.z = setpoint[2] - msg.wrench.torque.x = setpoint[3] - msg.wrench.torque.y = setpoint[4] - msg.wrench.torque.z = setpoint[5] + msg.wrench = msg_conv.ArrayToWrenchMsg(setpoint) self.__servo_cf_publisher.publish(msg) def add_servo_cf(self): @@ -821,11 +726,12 @@ def add_servo_cf(self): # using the callback name to test if hasattr(self.__class_instance, 'servo_cf'): raise RuntimeWarning('servo_cf already exists') - # create the subscriber and keep in list - self.__servo_cf_publisher = rospy.Publisher(self.__ros_namespace + '/servo_cf', - geometry_msgs.msg.WrenchStamped, - latch = False, queue_size = 1) - self.__publishers.append(self.__servo_cf_publisher) + # create the subscriber + self.__servo_cf_publisher = self.__ral.publisher( + 'servo_cf', + geometry_msgs.msg.WrenchStamped, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.servo_cf = self.__servo_cf @@ -834,12 +740,7 @@ def add_servo_cf(self): def __servo_cv(self, setpoint): # convert to ROS msg and publish msg = geometry_msgs.msg.TwistStamped() - msg.Twist.linear.x = setpoint[0] - msg.Twist.linear.y = setpoint[1] - msg.Twist.linear.z = setpoint[2] - msg.Twist.angular.x = setpoint[3] - msg.Twist.angular.y = setpoint[4] - msg.Twist.angular.z = setpoint[5] + msg.twist = msg_conv.ArrayToTwistMsg(setpoint) self.__servo_cv_publisher.publish(msg) def add_servo_cv(self): @@ -847,11 +748,12 @@ def add_servo_cv(self): # using the callback name to test if hasattr(self.__class_instance, 'servo_cv'): raise RuntimeWarning('servo_cv already exists') - # create the subscriber and keep in list - self.__servo_cv_publisher = rospy.Publisher(self.__ros_namespace + '/servo_cv', - geometry_msgs.msg.TwistStamped, - latch = True, queue_size = 1) - self.__publishers.append(self.__servo_cv_publisher) + # create the subscriber + self.__servo_cv_publisher = self.__ral.publisher( + 'servo_cv', + geometry_msgs.msg.TwistStamped, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.servo_cv = self.__servo_cv @@ -860,8 +762,8 @@ def add_servo_cv(self): def __move_jp(self, setpoint): # convert to ROS msg and publish msg = sensor_msgs.msg.JointState() - msg.position[:] = setpoint.flat - handle = crtk.wait_move_handle(self.__operating_state_instance) + msg.position = setpoint.tolist() + handle = crtk.wait_move_handle(self.__operating_state_instance, self.__ral) self.__move_jp_publisher.publish(msg) return handle @@ -870,11 +772,12 @@ def add_move_jp(self): # using the callback name to test if hasattr(self.__class_instance, 'move_jp'): raise RuntimeWarning('move_jp already exists') - # create the subscriber and keep in list - self.__move_jp_publisher = rospy.Publisher(self.__ros_namespace + '/move_jp', - sensor_msgs.msg.JointState, - latch = False, queue_size = 1) - self.__publishers.append(self.__move_jp_publisher) + # create the subscriber + self.__move_jp_publisher = self.__ral.publisher( + 'move_jp', + sensor_msgs.msg.JointState, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.move_jp = self.__move_jp @@ -883,8 +786,8 @@ def add_move_jp(self): def __move_jr(self, setpoint): # convert to ROS msg and publish msg = sensor_msgs.msg.JointState() - msg.position[:] = setpoint.flat - handle = crtk.wait_move_handle(self.__operating_state_instance) + msg.position = setpoint.tolist() + handle = crtk.wait_move_handle(self.__operating_state_instance, self.__ral) self.__move_jr_publisher.publish(msg) return handle @@ -893,11 +796,12 @@ def add_move_jr(self): # using the callback name to test if hasattr(self.__class_instance, 'move_jr'): raise RuntimeWarning('move_jr already exists') - # create the subscriber and keep in list - self.__move_jr_publisher = rospy.Publisher(self.__ros_namespace + '/move_jr', - sensor_msgs.msg.JointState, - latch = False, queue_size = 1) - self.__publishers.append(self.__move_jr_publisher) + # create the subscriber + self.__move_jr_publisher = self.__ral.publisher( + 'move_jr', + sensor_msgs.msg.JointState, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.move_jr = self.__move_jr @@ -906,8 +810,8 @@ def add_move_jr(self): def __move_cp(self, goal): # convert to ROS msg and publish msg = geometry_msgs.msg.PoseStamped() - msg.pose = tf_conversions.posemath.toMsg(goal) - handle = crtk.wait_move_handle(self.__operating_state_instance); + msg.pose = msg_conv.FrameToPoseMsg(goal) + handle = crtk.wait_move_handle(self.__operating_state_instance, self.__ral) self.__move_cp_publisher.publish(msg) return handle @@ -916,11 +820,12 @@ def add_move_cp(self): # using the callback name to test if hasattr(self.__class_instance, 'move_cp'): raise RuntimeWarning('move_cp already exists') - # create the subscriber and keep in list - self.__move_cp_publisher = rospy.Publisher(self.__ros_namespace + '/move_cp', - geometry_msgs.msg.PoseStamped, - latch = False, queue_size = 1) - self.__publishers.append(self.__move_cp_publisher) + # create the subscriber + self.__move_cp_publisher = self.__ral.publisher( + 'move_cp', + geometry_msgs.msg.PoseStamped, + latch = False, queue_size = 10 + ) # add attributes to class instance self.__class_instance.move_cp = self.__move_cp @@ -929,22 +834,21 @@ def add_move_cp(self): def __forward_kinematics(self, jp, extra = None): # convert to ROS msg and publish request = crtk_msgs.srv.QueryForwardKinematicsRequest() - request.jp[:] = jp.flat - response = self.__forward_kinematics_service_proxy(request) + request.jp = jp.tolist() + response = self.__forward_kinematics_service.call(request) if not extra: - return tf_conversions.posemath.fromMsg(response.cp); + return msg_conv.FrameFromPoseMsg(response.cp) else: - return [tf_conversions.posemath.fromMsg(response.cp), response.result, response.message] + return [msg_conv.FrameFromPoseMsg(response.cp), response.result, response.message] def add_forward_kinematics(self): # throw a warning if this has alread been added to the class, # using the callback name to test if hasattr(self.__class_instance, 'forward_kinematics'): raise RuntimeWarning('forward_kinematics already exists') - # create the service proxy and keep in list - self.__forward_kinematics_service_proxy = rospy.ServiceProxy(self.__ros_namespace + '/forward_kinematics', - crtk_msgs.srv.QueryForwardKinematics) - self.__service_proxies.append(self.__forward_kinematics_service_proxy) + # create the service + self.__forward_kinematics_service = self.__ral.service_client( + '/forward_kinematics', crtk_msgs.srv.QueryForwardKinematics) # add attributes to class instance self.__class_instance.forward_kinematics = self.__forward_kinematics @@ -953,9 +857,9 @@ def add_forward_kinematics(self): def __inverse_kinematics(self, jp, cp, extra = None): # convert to ROS msg and publish request = crtk_msgs.srv.QueryInverseKinematicsRequest() - request.jp[:] = jp.flat - request.cp = tf_conversions.posemath.toMsg(cp) - response = self.__inverse_kinematics_service_proxy(request) + request.jp = jp.tolist() + request.cp = msg_conv.FrameToPoseMsg(cp) + response = self.__inverse_kinematics_service.call(request) if not extra: return numpy.array(response.jp) else: @@ -966,9 +870,9 @@ def add_inverse_kinematics(self): # using the callback name to test if hasattr(self.__class_instance, 'inverse_kinematics'): raise RuntimeWarning('inverse_kinematics already exists') - # create the service proxy and keep in list - self.__inverse_kinematics_service_proxy = rospy.ServiceProxy(self.__ros_namespace + '/inverse_kinematics', - crtk_msgs.srv.QueryInverseKinematics) - self.__service_proxies.append(self.__inverse_kinematics_service_proxy) + # create the service + self.__inverse_kinematics_service = self.__ral.service_client( + '/inverse_kinematics', + crtk_msgs.srv.QueryInverseKinematics) # add attributes to class instance self.__class_instance.inverse_kinematics = self.__inverse_kinematics diff --git a/src/crtk/wait_move_handle.py b/src/crtk/wait_move_handle.py index 3180950..60c54b8 100644 --- a/src/crtk/wait_move_handle.py +++ b/src/crtk/wait_move_handle.py @@ -4,15 +4,14 @@ # Copyright (c) 2020-2021 Johns Hopkins University, University of Washington, Worcester Polytechnic Institute # Released under MIT License -import rospy - class wait_move_handle: - def __init__(self, class_instance): + def __init__(self, class_instance, ral): self.__class_instance = class_instance - self.__start_time = rospy.Time.now() + self.__ral = ral + self.__start_time = ral.now() def wait(self, is_busy = False, timeout = 30.0): - if rospy.is_shutdown(): + if self.__ral.is_shutdown(): return False if self.__class_instance: return self.__class_instance.wait_for_busy(is_busy = is_busy, @@ -23,7 +22,7 @@ def wait(self, is_busy = False, timeout = 30.0): def is_busy(self, timeout = 30.0): # if we keep asking past timeout, throw an exception - if (rospy.Time.now() - self.__start_time) > rospy.Duration(timeout): + if (self.__ral.now() - self.__start_time) > self.__ral.create_duration(timeout): raise RuntimeWarning('is_busy past timeout') # else, check if we have a new state after start time and return is_busy if self.__class_instance: From 17be6f702ac7fbfa41c2a655b82d9603588605b7 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Wed, 5 Jul 2023 14:22:43 -0400 Subject: [PATCH 17/21] Fix haptic exmpl for both dVRK/sensable_phantom (#7) --- scripts/crtk_haptic_example.py | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/scripts/crtk_haptic_example.py b/scripts/crtk_haptic_example.py index a261ab8..fed8ef5 100755 --- a/scripts/crtk_haptic_example.py +++ b/scripts/crtk_haptic_example.py @@ -13,7 +13,8 @@ # > rosrun sensable_phantom_ros sensable_phantom -j sawSensablePhantomRight.json # To communicate with the device using ROS topics, see the python based example: -# > rosrun crtk_python_client crtk_haptic_example.py +# > rosrun crtk_python_client crtk_haptic_example +# For dVRK, add -b/--body flag when running the crtk_haptic_example import argparse import crtk @@ -41,7 +42,7 @@ def set_cf_orientation_absolute(self, absolute = True): m.data = absolute self.set_cf_orientation_absolute_pub.publish(m) - def __init__(self, ral): + def __init__(self, ral, has_body_orientation): self.ral = ral # populate this class with all the ROS topics we need @@ -50,11 +51,16 @@ def __init__(self, ral): self.crtk_utils.add_measured_cp() self.crtk_utils.add_measured_cv() - self.body = crtk_haptic_example.Subframe(self.ral.create_child('/body')) + if has_body_orientation: + self.body = crtk_haptic_example.Subframe(self.ral.create_child('/body')) + self.servo_cf = lambda sp: self.body.servo_cf(sp) + else: + self.body = None + self.crtk_utils.add_servo_cf() # for all examples self.duration = 20 # seconds - self.rate = 500 # aiming for 500 Hz + self.rate = 200 # aiming for 200 Hz self.samples = self.duration * self.rate # main loop @@ -69,7 +75,8 @@ def run(self): print('Unable to home the device, make sure it is connected.') return - self.body.set_cf_orientation_absolute() + if self.body is not None: + self.body.set_cf_orientation_absolute() self.running = True while (self.running): @@ -117,11 +124,11 @@ def run_box(self): wrench[d] = p_gain * (distance - dim) elif (distance < -dim): wrench[d] = p_gain * (distance + dim) - self.body.servo_cf(wrench) + self.servo_cf(wrench) sleep_rate.sleep() wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - self.body.servo_cf(wrench) + self.servo_cf(wrench) # viscosity def run_viscosity(self): @@ -132,22 +139,23 @@ def run_viscosity(self): # foreach d dimension x, y, z for d in range(3): wrench[d] = d_gain * self.measured_cv()[d] - self.body.servo_cf(wrench) + self.servo_cf(wrench) sleep_rate.sleep() wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - self.body.servo_cf(wrench) + self.servo_cf(wrench) def main(): parser = argparse.ArgumentParser() parser.add_argument('namespace', type = str, help = 'ROS namespace for CRTK device') + parser.add_argument('-b', '--body', action = 'store_true', help = 'Indicates CRTK device body orientation needs to be set to absolute') app_args = crtk.ral.parse_argv(sys.argv[1:]) # process and remove ROS args args = parser.parse_args(app_args) example_name = type(crtk_haptic_example).__name__ ral = crtk.ral(example_name, args.namespace) - example = crtk_haptic_example(ral) + example = crtk_haptic_example(ral, args.body) ral.spin_and_execute(example.run) if __name__ == '__main__': From 96efcb8c5fe004e5d086c0240c5c9e8ccda62fb0 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Sun, 8 Oct 2023 18:33:30 -0400 Subject: [PATCH 18/21] Update wait_move_handle to wait for transition. (#8) * Update wait_move_handle to wait for transition When waiting for robot idle, e.g. waiting for move command to finish, the wait handle will move wait for a transition from busy to idle, rather than just for idle status. Waiting for the robot to become busy (e.g. waiting for a move to start) should work as before. That is, it will just wait for new is_busy=True operating state, *not* an idle-to-busy transition. * Add back dunder members --- src/crtk/utils.py | 60 ++++++++++++++++++++---------------- src/crtk/wait_move_handle.py | 33 ++++++++++++-------- 2 files changed, 54 insertions(+), 39 deletions(-) diff --git a/src/crtk/utils.py b/src/crtk/utils.py index 138526d..f38c18f 100644 --- a/src/crtk/utils.py +++ b/src/crtk/utils.py @@ -65,8 +65,17 @@ def __wait_for_valid_data(self, data, event, age, wait): # internal methods to manage state def __operating_state_cb(self, msg): - # crtk operating state contains state as well as homed and busy - self.__operating_state_data = msg + event_time = self.__ral.get_timestamp(msg) + last_event_time = self.__ral.get_timestamp(self.__operating_state_data) + + # update last_busy_time + if msg.is_busy and event_time > self.__last_busy_time: + self.__last_busy_time = event_time + + if event_time > last_event_time: + # crtk operating state contains state as well as homed and busy + self.__operating_state_data = msg + # then when all data is saved, release "lock" self.__operating_state_event.set() @@ -193,36 +202,34 @@ def __wait_for_busy(self, is_busy = False, start_time = None, timeout = 30.0): - # if timeout is negative, not waiting - if timeout < 0.0: - return False - # set start time to now if not specified - if start_time is None: - start_time = self.__ral.now() + # set start_time if not provided + start_time = start_time or self.__ral.now() + elapsed = self.__ral.to_sec(self.__ral.now() - start_time) + if elapsed > timeout: + return False # past timeout, wait failed + + # check if wait condition has been satisfied + if is_busy: + if self.__last_busy_time > start_time: + return True # waiting for busy and currently busy else: - # user provided start_time, check if an event arrived after start_time - last_event_time = self.__ral.get_timestamp(self.__operating_state_data) - if (last_event_time > start_time and self.__operating_state_data.is_busy == is_busy): - return True + was_busy = self.__last_busy_time > start_time + no_longer_busy = not self.__operating_state_data.is_busy + if was_busy and no_longer_busy: + return True # waiting for idle, and have recent transition to idle - # other cases, waiting for an operating_state event - _start_time = self.__ral.now() + # wait for next operating state event (or timeout) self.__operating_state_event.clear() - in_time = self.__operating_state_event.wait(timeout) - # past timeout + in_time = self.__operating_state_event.wait(timeout - elapsed) + + # check for ROS shutdown or event time-out if self.__ral.is_shutdown() or not in_time: return False - # within timeout and result we expected - if self.__operating_state_data.is_busy == is_busy: - return True - else: - # wait a bit more - elapsed_time = self.__ral.to_sec(self.__ral.now() - _start_time) - self.__operating_state_event.clear() - return self.__wait_for_busy(is_busy = is_busy, - start_time = start_time, - timeout = timeout - elapsed_time) + # recurse + return self.__wait_for_busy(is_busy = is_busy, + start_time = start_time, + timeout = timeout) def add_operating_state(self): # throw a warning if this has alread been added to the class, @@ -232,6 +239,7 @@ def add_operating_state(self): # data self.__operating_state_data = crtk_msgs.msg.OperatingState() self.__operating_state_event = threading.Event() + self.__last_busy_time = self.__ral.now() # create the subscriber/publisher self.__operating_state_subscriber = self.__ral.subscriber( diff --git a/src/crtk/wait_move_handle.py b/src/crtk/wait_move_handle.py index 60c54b8..58bb705 100644 --- a/src/crtk/wait_move_handle.py +++ b/src/crtk/wait_move_handle.py @@ -4,28 +4,35 @@ # Copyright (c) 2020-2021 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 +# +# - wait(is_busy=True) waits for a new is_busy=True operating state to arrive +# - wait(is_busy=False) waits for *transition* to is_busy=False, i.e. waits +# for is_busy=True followed by is_busy=False class wait_move_handle: def __init__(self, class_instance, ral): - self.__class_instance = class_instance + self.__inst = class_instance self.__ral = ral self.__start_time = ral.now() + if not class_instance: + self.wait = self._unsupported + self.is_busy = self._unsupported + + def _unsupported(self, *args, **kwargs): + raise RuntimeWarning("can't wait, class doesn't support CRTK operating state") + def wait(self, is_busy = False, timeout = 30.0): if self.__ral.is_shutdown(): return False - if self.__class_instance: - return self.__class_instance.wait_for_busy(is_busy = is_busy, - start_time = self.__start_time, - timeout = timeout) - else: - raise RuntimeWarning('can\'t wait, the class doesn\'t support CRTK operating state') + + return self.__inst.wait_for_busy(is_busy = is_busy, + start_time = self.__start_time, + timeout = timeout) def is_busy(self, timeout = 30.0): # if we keep asking past timeout, throw an exception if (self.__ral.now() - self.__start_time) > self.__ral.create_duration(timeout): - raise RuntimeWarning('is_busy past timeout') - # else, check if we have a new state after start time and return is_busy - if self.__class_instance: - return self.__class_instance.is_busy(start_time = self.__start_time) - else: - raise RuntimeWarning('can\'t wait, the class doesn\'t support CRTK operating state') + raise RuntimeWarning('is_busy() called after timeout') + + return self.__inst.is_busy(start_time = self.__start_time) From c06d92220cd852d20efc22b902cf06af705ac585 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 31 Oct 2023 21:34:00 -0400 Subject: [PATCH 19/21] ral subscriber, added dumy latch to have same API as ROS2 --- src/crtk/ral.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/crtk/ral.py b/src/crtk/ral.py index 8498752..47cfe28 100644 --- a/src/crtk/ral.py +++ b/src/crtk/ral.py @@ -108,8 +108,9 @@ def publisher(self, topic, msg_type, queue_size = 10, latch = False, *args, **kw self._publishers.append(pub) return pub - def subscriber(self, topic, msg_type, callback, queue_size = 10, *args, **kwargs): - sub = rospy.Subscriber(self._add_namespace_to(topic), msg_type, callback=callback, + # latch is added to be compatible with ROS2 crtk.ral + def subscriber(self, topic, msg_type, callback, queue_size = 10, latch = False, *args, **kwargs): + sub = rospy.Subscriber(self._add_namespace_to(topic), msg_type, callback = callback, queue_size = queue_size, *args, **kwargs) self._subscribers.append(sub) return sub From 41358f79868eccd4e19305343fcf4fa04a0cb3b6 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Mon, 20 Nov 2023 14:53:52 -0500 Subject: [PATCH 20/21] Updated CMake, package.xml and CHANGELOG for 1.2.0 --- CHANGELOG.md | 18 ++++++ CMakeLists.txt | 2 +- README.md | 157 +------------------------------------------------ package.xml | 2 +- 4 files changed, 21 insertions(+), 158 deletions(-) create mode 100644 CHANGELOG.md diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..9ec99b7 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,18 @@ +Change log +========== + +1.2.0 (2023-11-xx) +================== + +* :warning: Use `PoseStamped` instead of `TransformStamped` for CRTK `_cp` commands (see collaborative-robotics/documentation#1) +* Documentation ported to ReadTheDocs: https://crtk-robotics.readthedocs.io +* Added `crtk.ral` (ROS Abstraction Layer) so Python scripts can be written for either ROS 1 or ROS 2 + * `ral.check_connections` allows you to check if there are subscribers/publishers connected to your publishers/subscriber + * `ral.spin` starts the thread for ROS spin (no op on ROS 1) + * `ral.parse_argv` parses and removed ROS specific arguments + * ... +* `utils.py`: + * Fixed bug in `measured_cp`, was returning `setpoint_cp` + * Fixed `wait` on move handle + * Add `hold`, `free`, `forward_kinematics`, `inverse_kinematics`, `servo_cv` + * Added `joystick_button` to wrap foot pedals and other buttons diff --git a/CMakeLists.txt b/CMakeLists.txt index a37d04d..2cc9613 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required (VERSION 3.1) -project (crtk_python_client VERSION 1.0.0) +project (crtk_python_client VERSION 1.2.0) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/README.md b/README.md index d905ff1..2f4f553 100644 --- a/README.md +++ b/README.md @@ -1,158 +1,3 @@ # CRTK Python client library -This Python package provides some tools to facilitate the development of a CRTK compatible client over ROS, i.e. create a "proxy" class to communicate with an existing CRTK compatible ROS device. - -CRTK specifications can be found on the [CRTK github page](https://github.com/collaborative-robotics/documentation/wiki/Robot-API). - -Examples of CRTK devices with a CRTK/ROS interface: -* [Raven II](https://github.com/uw-biorobotics/raven2/tree/crtk) -* [dVRK](https://github.com/jhu-dvrk/sawIntuitiveResearchKit/wiki) - -# Build - -To build this package, we recommend to use the catkin build tools, i.e. `catkin build`. -You will need to clone this repository as well as the repository with CRTK specific ROS messages: -```bash -cd ~/catkin_ws/src # or wherever your catkin workspace is -git clone https://github.com/collaborative-robotics/crtk_msgs -git clone https://github.com/collaborative-robotics/crtk_python_client -catkin build -``` - -Once these packages are built, you should source your `setup.bash`: `source ~/catkin_ws/devel/setup.bash`. -At that point, you should be able to import the crtk python package in Python using `import crtk`. - -# Usage - -The main class in the CRTK Python client library is `crtk.utils`. It can be used to quickly populate an existing class by adding CRTK like methods. -These methods will handle the following for you: -* declare all required ROS publishers and wrap publisher calls in methods to send data to the device. -* declare all required ROS subscribers and provide callbacks to receive the data from the device. -* convert ROS messages to more convenient Python data types, i.e. numpy arrays for joint values and PyKDL types for cartesian data. -* some events to manage asynchronous communication between the device and the "proxy" class. - -The class `crtk.utils` is designed to add CRTK features "a la carte", i.e. it doesn't assume that all CRTK features are available. This allows to: -* match only the features that are available on the CRTK devices one wants to use (server side) -* reduce the number of features to those strictly needed for the application (client side). Reducing the number of ROS topics used helps in terms of performance. - -## Base class and `crtk.utils` - -You can find some examples in the `scripts` directory. Overall, the approach is always the same, i.e. create a class and populate it with `crtk.utils`. You can then use an instance of this class. For example: - -```python -class crtk_move_cp_example: - # constructor - def __init__(self, device_namespace): - # ROS initialization - if not rospy.get_node_uri(): - rospy.init_node('crtk_move_cp_example', anonymous = True, log_level = rospy.WARN) - # populate this class with all the ROS topics we need - self.crtk_utils = crtk.utils(self, device_namespace) - self.crtk_utils.add_measured_cp() - self.crtk_utils.add_move_cp() -``` - -What is happening behind the scene: -* `device_namespace` is the ROS namespace used by the device. E.g. if the namespace is `left`, we assume the device will have its CRTK ROS topics under `/left`. -* `get_node_uri()` and `init_node()` are not strictly needed but helps if the user did not properly initialize the ROS node -* Add an instance of `crtk.utils` in your class. The first parameter indicates which Python object should be "populated", i.e. which object will have the CRTK methods added to its dictionary. -* `add_measured_cp()`: - * Creates a subscriber for the topic, e.g. : `/left/measured_cp` - * Registers a built-in callback for the topic. The callback will store the latest `measured_cp` ROS message in `crtk_utils` - * Provides a method to read the latest `measured_cp` message as a PyKDL frame. - * Adds the method `measured_cp()` to the user class (`crtk_move_cp_example`) -* `add_move_cp()`: - * Creates a publisher for the topic, e.g. : `/left/move_cp` - * Provides a method to send a PyKDL frame (goal), internally converts to a ROS message. - * Adds the method `move_cp()` to the user class (`crtk_move_cp_example`) - -Once the class is defined, the user can use it: -```python -example = crtk_move_cp_example('left') -position = example.measured_cp() -position.p[2] += 0.05 # move by 5 cm -example.move_cp(position) -``` - -## Motion Commands - -`crtk.utils` supports the following CRTK features: -* subscribers: - * `add_setpoint_js`, `add_setpoint_cp` - * `add_measured_js`, `add_measured_cp`, `add_measured_cv`, `add_measured_cf` - * ... -* publishers - * `add_servo_jp`, `add_servo_jf`, `add_servo_cp`, `add_servo_cf` - * `add_move_jp`, `add_move_cp` - * ... - -All methods relying on subscribers to get data have the following two _optional_ parameters: `age` and `wait`: -```python - setpoint_cp(age = None, wait = None) -``` -The parameter `age` specifies how old the data can be to be considered valid and `wait` specifies how long to wait for the next message if the data currently cached is too old. By default, both are based on the expected interval provided when creating an instance of `crtk.utils`. The expected interval should match the publishing rate from the CRTK device. Setting the `age` to zero means that any cached data should be used and the method shouldn't wait for new messages. - -All move commands (`move_jp` and `move_cp`) return a ROS time object. This is the time just before sending (i.e., publishing) the move command to the device. This timestamp can be used to wait for motion completion using: -```python -# wait until robot is not busy, i.e. move has ended -h = example.move_cp(goal) # record time move was sent -h.wait() -# compact syntax -example.move_cp(goal).wait() -# other example, wait until move has started -example.move_cp(goal).wait(is_busy = True) -``` - -The method `wait_for_busy` used by `handle.wait()` depends on the CRTK device operating state and can be added to the example class using `crtk.utils.add_operating_state`. See section below. - -## Operating States - -`crtk.utils.add_operating_state` adds: -* State status `operating_state()` and helper queries: `is_enabled()`,`is_homed()`, `is_busy()` -* State command `operating_state_command()` and helper commands: `enable()`, `disable()`, `home()`, `unhome()` -* Timer/event utilities: - * For subscribers: `wait_for_valid_data` - * For publishers (used by move commands): , `wait_for_busy()` - * For state changes (used by `enable()`, `home()`...): `wait_for_operating_state()` - -# Examples - -## dVRK - -For the dVRK, one can use the classes `dvrk.arm`, `dvrk.psm`, `dvrk.mtm`... that use the `crtk.utils` to provide as many features as possible. This is convenient for general purpose testing, for example in combination with iPython to test snippets of code. In general, it is recommended to use your own class and only add the features you need to reduce the number of ROS messages and callbacks. - -The dVRK arm class implementation can be found in the [dvrk_python](https://github.com/jhu-dvrk/dvrk-ros/blob/devel/dvrk_python/src/dvrk/arm.py) package. - -Example of use: -```python -import dvrk -p = dvrk.arm('PSM1') -p.enable() -p.home() - -# get measured joint state -[position, velocity, effort, time] = p.measured_js() -# get only position -position = p.measured_jp() -# get position and time -[position, time] = p.measured_jp(extra = True) - -# move in joint space -import numpy -p.move_jp(numpy.array([0.0, 0.0, 0.10, 0.0, 0.0, 0.0])) - -# move in cartesian space -import PyKDL -# start position -goal = p.setpoint_cp() -# move 5cm in z direction -goal.p[2] += 0.05 -p.move_cp(goal).wait() - -import math -# start position -goal = p.setpoint_cp() -# rotate tool tip frame by 25 degrees -goal.M.DoRotX(math.pi * 0.25) -p.move_cp(goal).wait() -``` +See https://crtk-robotics.readthedocs.io \ No newline at end of file diff --git a/package.xml b/package.xml index 6441567..3c7ff3b 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ crtk_python_client - 1.0.0 + 1.2.0 crtk Python client Anton Deguet From c1c1f5709179d18c840bee75cfb284b6470358f2 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 21 Nov 2023 11:10:03 -0500 Subject: [PATCH 21/21] Update CHANGELOG.md --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9ec99b7..49c0b51 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,7 +1,7 @@ Change log ========== -1.2.0 (2023-11-xx) +1.2.0 (2023-11-21) ================== * :warning: Use `PoseStamped` instead of `TransformStamped` for CRTK `_cp` commands (see collaborative-robotics/documentation#1)