5.2. Client Libraries: Matlab

5.2.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.

5.2.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

5.2.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

5.2.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')

5.2.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.

5.2.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