Merge pull request #9 from collaborative-robotics/rc-1.2

1.2.0

Author: adeguet1

CHANGELOG.md

@@ -0,0 +1,18 @@
+Change log
+==========
+
+1.2.0 (2023-11-21)
+==================
+
+* :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

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.2.0)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

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

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>crtk_python_client</name>
-  <version>1.0.0</version>
+  <version>1.2.0</version>
   <description>crtk Python client</description>
 
   <maintainer email="[email protected]">Anton Deguet</maintainer>

scripts/crtk_haptic_example.py

@@ -14,47 +14,79 @@
 
 # To communicate with the device using ROS topics, see the python based example:
 # > rosrun crtk_python_client crtk_haptic_example <device-namespace>
+# For dVRK, add -b/--body flag when running the crtk_haptic_example
 
+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, has_body_orientation):
+        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()
+
+        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 = 10 # 10 seconds
-        self.rate = 500    # aiming for 500 Hz
+        self.duration = 20 # seconds
+        self.rate = 200 # aiming for 200 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
 
+        if self.body is not None:
+            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 +107,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
@@ -90,32 +125,38 @@ def run_box(self):
                 elif  (distance < -dim):
                     wrench[d] = p_gain * (distance + dim)
             self.servo_cf(wrench)
-            rospy.sleep(1.0 / self.rate)
+            sleep_rate.sleep()
+
         wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         self.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)
+            sleep_rate.sleep()
+
         wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         self.servo_cf(wrench)
 
-# 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
+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, args.body)
+    ral.spin_and_execute(example.run)
+
+if __name__ == '__main__':
+    main()