5. Client Libraries

The client libraries provide 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. We currently support two client libraries, one for Python and one for Matlab.

The main class in the 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 data types. The Python client use numpy arrays for joint values and PyKDL types for cartesian data. The Matlab client uses vectors and matrices.
Some events to manage asynchronous communication between the device and the “proxy” class (e.g. end of move command, change of state…)

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.

In version 1.2, crtk also includes a ROS Abstraction Layer. The ral goal is to hide which version of ROS is used so the end-user script can run with either ROS 1 or ROS 2. The ral doesn’t cover all the ROS features (either in Python or Matlab) so not all scripts can be totally portable.

Note

As of December 2023, the Python client libraries have been used extensively and are well tested. The Matlab version is not as mature. More specifically the RAL implementation is buggy on ROS 2 and missing in the ROS 1 implementation.

6. Client Libraries: Python

6.1. Installation

Note

If you’ve installed a package relying on CRTK using a tool such as vcs, it is quite possible all the CRTK dependencies have already been pulled from GitHub. Before pulling another copy, check the content of your ROS workspace src directory.

6.1.1. ROS 1

To build the ROS 1 CRTK Client library, 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:

cd ~/catkin_ws/src   # or wherever your catkin workspace is
git clone https://github.com/collaborative-robotics/crtk_msgs crtk/crtk_msgs
git clone https://github.com/collaborative-robotics/crtk_python_client crtk/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.

6.1.2. ROS 2

You will first need to clone this repository as well as the repository with CRTK specific ROS messages:

cd ~/ros2_ws/src   # or wherever your ROS 2 workspace is
git clone https://github.com/collaborative-robotics/ros2_crtk_msgs crtk/crtk_msgs
git clone https://github.com/collaborative-robotics/ros2_crtk_python_client crtk/crtk_python_client
cd ~/ros2_ws;
colcon build

Once these packages are built, you should source your setup.bash: source ~/ros_ws/install/setup.bash. At that point, you should be able to import the crtk python package in Python using import crtk.

6.2. Setting up a client

6.2.1. Overview

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:

class crtk_move_cp_example:
    # constructor
    def __init__(self, ral):
        self.ral = ral # keep a copy for future use
        # populate this class with all the ROS topics we need
        self.crtk_utils = crtk.utils(self, ral)
        self.crtk_utils.add_measured_cp()
        self.crtk_utils.add_move_cp()

What is happening behind the scene:

ral is the ROS Abstraction Layer used by the device. It contains information about the ROS node as well as the namespace. For example, if the namespace is left, we assume the device will have its CRTK ROS topics under /left
Add an instance of crtk.utils to 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. The second parameter is the ROS Abstraction Layer. The ral will keep track of all the publishers and subscribers used.

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:

ral = crtk.ral('node_name', '/left') # the node name is mandatory, default namespace is empty
example = crtk_move_cp_example(ral)

# make sure all the subscribers (resp. publishers) are connected to one or more publisher (resp. subscriber)
ral.check_connections()
# make sure ROS spin is happening (no-op on ROS 1, needed on ROS 2)
ral.spin()

position = example.measured_cp()
position.p[2] += 0.05 # move by 5 cm
example.move_cp(position)

# stop
ral.shutdown()

6.2.2. Operating state

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()

6.2.3. Robot motion

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:

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:

# 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 previous section.

6.2.4. RAL

The ROS Abstraction Layer (RAL) allows programmers to write Python scripts that can be easily ported between ROS 1 (rospy) and ROS 2 (rclpy).

6.2.4.1. Spinning

One of the main differences between the ROS 1 and ROS 2 Python APIs is that rospy handles the spinning thread behind the scene. So one can create a subscriber with a callback but it’s impossible to know when the spin happens, i.e. when the incoming ROS messages are being processed. In rclpy, one has to create their own thread and make sure this thread calls spin. When the application ends, the programmer must also ensure that the “spinning” thread is stop cleanly. RAL provides a few methods to manage this thread. These methods are not needed for ROS 1 scripts but if the goal is to use your script with ROS 2 later on, it’s a good habit to use them:

spin(): starts an executor and makes sure the spin method is called

spin_and_execute(f, *args): calls ral.spin and then the function f with the arguments *args

shutdown(): calls ROS shutdown and stops the “spinning” thread

on_shutdown(cb): register a callback for shutdown

is_shutdown(): check if the RAL is shut down

6.2.4.2. Time wrappers

now():

get_timestamp(t): get best timestamp for object t, representation is ROS version dependent

to_sec(t): get best timestamp for object t, result is a floating point

create_duration(d_seconds): create a duration object, the type is ROS version dependent but the APIs are similar

create_rate(r_hz): create a rate object, the type is ROS version dependent but the APIs are similar

6.2.4.3. Create publishers, subscribers and service clients

publisher(self, topic, msg_type, queue_size = 10, latch = False, *args, **kwargs)

subscriber(self, topic, msg_type, callback, queue_size=10, latch = False, *args, **kwargs)

service_client(self, name, srv_type)

6.2.4.4. Checking connections

Besides abstracting ROS 1 rospy and ROS 2 rclpy, the RAL is also used to keep track of all the publishers and subscribers created for the Python client. This allows to check if there are existing nodes subscribing to the client publishers and nodes publishing to the client subscribers. This is useful for a few reasons:

Detecting mismatches between ROS namespaces. One can easily have a typo or simply use the wrong namespace. If this is the case, ral.check_connection() will timeout and throw an exception

Detecting a missing feature on the server side. The client might expect a CRTK feature from the server (device) but that feature doesn’t exist. In this case, ral.check_connection() will throw an exception. The list of missing CRTK features will be provided in the exception message.

Avoiding race conditions when starting a node. With ROS, one can create a publisher and immediately publish. The issue is that even if there is already a subscriber for the same topic, it is possible to publish before the subscriber is connected. The connection process is fast but it is still possible to use publish before the subscriber is connected. To avoid this, ral.check_connection() performs a busy wait, checking all the node’s publishers and subscribers and returns when they are all connected. The default time-out is 5 seconds.

6.3. Using a client

For this section we will use the dVRK since it comes with a set of ready-to-use classes based on the CRTK Python Client. One can use the classes dvrk.arm, dvrk.psm, dvrk.mtm… that rely on crtk.utils to provide as many CRTK features as possible.

Note

The client’s developer has to make some choices regarding which features to provide. For an interactive session (say iPython) or quick prototyping, it’s useful to have as many features as possible. The issue is that each subscriber has a cost. For one thing, the publisher will have to publish. Then the Python client will receive the message and the callback will be called. This extra cost will incur whether the feature is used of not. Therefore, if performances are an issue, it is recommended to create a custom client with the just required features and nothing else.

The dVRK arm class implementation can be found in the “dvrk_python” package.

Example of use:

import crtk, dvrk
ral = crtk.ral('PSM1')
p = dvrk.arm(ral)
ral.check_connections()
ral.spin()
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()

ral.shutdown()

7. Client Libraries: Matlab

7.1. Installation

To note, rosgenmsh (ROS 1) and ros2genmsg (ROS 2) have one rather annoying limitation, they expect a directory that contains directories of ROS packages. So if you clone crtk_msgs in my_ws/src you will need to use rosgenmsg (or ros2genmsg) on all packages in my_ws/src. To avoid this, you need to create a sub-directory in my_ws/src and then clone crtk_msgs in said sub-directory (for example ~/my_ws/src/crtk/crtk_msgs).

Matlab uses pretty similar names for the ROS 1 and ROS 2 commands so the following instructions are valid for both ROS 1 and ROS 2. Instructions below are for ROS 2. For ROS 1, replace ros2_ws by catkin_ws, ros2genmsg by rosgenmsg and colcon build by catkin_build.

Note

If you’ve installed a package relying on CRTK using a tool such as vcs, it is quite possible all the CRTK dependencies have already been pulled from GitHub. Before pulling another copy, check the content of your ROS workspace src directory.

If you’re cloning crtk_msgs by hand, you can use the following instructions.

7.1.1. ROS 1

cd ~/catkin_ws/src
# clone in sub-directory crtk and rename to crtk_msgs
git clone https://github.com/collaborative-robotics/crtk_msgs crtk/crtk_msgs
git clone https://github.com/collaborative-robotics/crtk_matlab_client crtk/crtk_matlab_client
cd ~/catkin_ws
catkin build

7.1.2. ROS 2

cd ~/ros2_ws/src
# clone in sub-directory crtk and rename to crtk_msgs
git clone https://github.com/collaborative-robotics/ros2_crtk_msgs crtk/crtk_msgs
git clone https://github.com/collaborative-robotics/ros2_crtk_matlab_client crtk/crtk_matlab_client
cd ~/ros2_ws
colcon build

7.2. Custom message generation

At that point, you can finally generate the code in Matlab:

ROS 1:

rosgenmsg('~/catkin_ws/src/crtk')

ROS 2:

ros2genmsg('~/ros2_ws/src/crtk')

You could follow the instructions displayed at the end of the message generation to use savepath but this would apply to all users on the workstation. So you should probably skip that step and read the following section so the paths would be defined per user.

Instead of setting the path for all users, you can use your startup script. To edit the user’s startup.m in Matlab, use:

edit(fullfile(userpath,'startup.m'))

If the file doesn’t exist yet, click yes to create it. In your startup.m, you can add the addpath commands that you want executed everytime your start Matlab:

Note

In the following, the path to find the generated code varies based on the ROS and Matlab versions, make sure you use the path provided by Matlab at the end of the rosgenmsg call.

ROS 1:

% to locate crtk_msgs
% some Matlab versions use the following
addpath('~/catkin_ws_ws/src/crtk/matlab_msg_gen_XXXXXXXXXXX')
% to locate crtk client
addpath('~/catkin_ws/src/crtk/crtk_matlab_client')
% to locate dvrk code - only for dVRK users
addpath('~/catkin_ws/src/dvrk-ros/dvrk_matlab')

ROS 2:

% to locate crtk_msgs
% some Matlab versions use the following
addpath('~/ros2_ws/src/crtk/matlab_msg_gen')
% to locate crtk client
addpath('~/ros2_ws/src/crtk/crtk_matlab_client')
% to locate dvrk code - only for dVRK users
addpath('~/ros2_ws/src/dvrk/dvrk_matlab')

Then quit Matlab, restart it and test using:

clear classes
rehash toolboxcache
which startup
% create a message (ROS 1)
m = rosmessage('crtk_msgs/OperatingState')
% create a message (ROS 2)
m = ros2message('crtk_msgs/OperatingState')

7.3. Setting up a client

The first step is to create a Matlab class with dynamic properties. For example, let’s assume we want to create a simple force sensor client:

classdef force_sensor < dynamicprops

The class should own an instance of crtk_utils:

properties (Access = protected)
  crtk_utils;
end

Then in the constructor, create an instance of crtk_utils and add the CRTK features you need. For example, if the device supports measured_cf, use the method add_measured_cf().

methods
  function self = force_sensor(ros_namespace)
    self.crtk_utils = crtk.utils(self, ros_namespace);
    self.crtk_utils.add_measured_cf();
  end
end

The method add_measured_cf will create the necessary ROS subscriber and add a function handle (measured_cf) to the force sensor class. Once this is done, you can create an instance of the force sensor and call the method measured_cf:

>> fs = force_sensor('optoforce/');
>> cf = fs.measured_cf()
cf =
   -0.0025   -0.0125    0.0775         0         0         0

If there are no messages on the CRTK topic subscribed to, you will get a warning similar to:

>> cf = fs.measured_cf()
Warning: measured_cf has not received messages yet (topic /optoforce/measured_cf)

This can be used to make sure you’re using the right ROS topic name and namespace.

7.4. Using a client

This example is based on the dVRK Matlab package.

ral = crtk.ral('test_arm_move');
r = dvrk.arm(arm_name, ral);
disp('---- Enabling (waiting up to 30s)');
if ~r.enable(30.0)
    error('Unable to enable arm');
end
disp('---- Homing (waiting up to 30s)');
if ~r.home(30.0)
    error('Unable to home arm');
end

% general settings
rate = 200; % aiming for 200 Hz
ros_rate = ral.rate(rate);

% move_jp
disp('---- Joint move');
% move to 0 position
joints_home = r.setpoint_js();
joints_home(:) = 0.0;
if (strcmp(arm_name, 'ECM') || strncmp(arm_name, 'PSM', 3))
    joints_home(3) = 0.12;
end
r.move_jp(joints_home).wait();
% wiggle first two joints, matlab index starts at 1
amplitude = deg2rad(10.0);
% first move
start = r.setpoint_js();
goal = start;
goal(1:2) = amplitude;
r.move_jp(goal).wait();
% second move
goal = start;
goal(1:2) = -amplitude;
r.move_jp(goal).wait();

disp('---- Joint servo');
% move to 0 position
r.move_jp(joints_home).wait();
% wiggle first two joints, matlab index starts at 1
amplitude = deg2rad(10.0);
duration = 10.0; % seconds
samples = duration * rate;
% create a new goal starting with current position
start = r.setpoint_js();
goal = start;
reset(ros_rate);
for i = 0:samples
    goal(1) = start(1) + amplitude * (1.0 - cos(i * deg2rad(360.0) / samples));
    goal(2) = start(2) + amplitude * (1.0 - cos(i * deg2rad(360.0) / samples));
    r.servo_jp(goal);
    waitfor(ros_rate);
end