Getting Started with Standalone ROS Node Generation for Universal Robots

Overview

This example shows how to plan, create and validate a deployable motion planning standalone ROS node for Universal Robot UR5e with URSim, Gazebo simulator or physical UR5e. In this example, you will

In this example, you can select the Gazebo physics simulator, URSim offline simulator or physical UR5e hardware. All the necessary files required to setup URSim are included in the support package. Refer to Install ROS Packages and Dependencies for ROS and Set Up URSim Offline Simulator for more details.

Clear all output in the live script by right-clicking anywhere in the live script and then clicking ‘Clear All Output’ or navigate to VIEW > Clear all Output.

Note: Execute the script section by section as mentioned in this example.

Prerequisites

Required Products

Create and Validate Motion Planning Algorithm with URSim, Gazebo Simulator or Physical UR5e in Connected I/O Mode

In this example, we have considered the following tasks for the robot motion planning algorithm:

Select Interface and Define IP Address of ROS Enabled Device and Robot

The below diagram shows the naming convention of the various IP addresses used in this example.

Note: The above diagram is only applicable for the URSim simulator or UR Series hardware. MATLAB can be present inside the same ROS machine or It can also be on any external machine.

The ROSDeviceAddress is the IP address of your PC/VM where you have installed the ROS and the other required ROS drivers.

The robotAddress is the IP address of your robot hardware or simulated robot(in case of URSim). If you have installed URSim in the same ROS device, then the robot IP will be the same as the local host IP i.e. '127.0.0.1'. Hence robotIPAddress for URSim will be 127.0.0.1.

interface = "URSim"

If you select Gazebo:

If you select URSim:

If you select Hardware:

Specify the below address parameters based on the above given diagram.

% IP of ROS enabled machine
ROSDeviceAddress = '192.168.92.132';

% IP of the robot (in case of URSim, this IP address will be 127.0.0.1)
robotAddress = '192.168.1.10'; 

Generate Automatic Launch Script and Transfer It to ROS Host Computer

Provide parameters of the host machine with ROS.

Note: If you have not yet installed the required ROS drivers in your ROS machine then follow Install ROS Packages and Dependencies for ROS document to setup a catkin workspace.

username = 'user';
password = 'password';
ROSFolder = '/opt/ros/melodic';
WorkSpaceFolder = '~/ur_ws';

Connect to ROS device.

device = rosdevice(ROSDeviceAddress,username,password);
device.ROSFolder = ROSFolder;

Generate launch script and transfer it to ROS host computer.

eampleHelperTransferLaunchScriptURCodegen(device,WorkSpaceFolder,interface,robotAddress);

Launch the required nodes if the ROS core is not running.

if ~isCoreRunning(device)
    w = strsplit(system(device,'who'));
    displayNum = cell2mat(w(2));
    
    system(device,['export SVGA_VGPU10=0; ' ...
                   'export DISPLAY=' displayNum '.0; ' ...
                   './launchURHybrid.sh &']);
end

Optional: Launch Required ROS Driver Manually

Manually launch the required ROS driver in your ROS machine

Follow the below-given instruction on how to launch the required ROS drivers in the ROS enabled machine.

Step:- 1

First, open the terminal in your ROS machine and cd to the catkin workspace you built for the UR ROS drivers (for example, "cd ~/ur_ws").

Step:- 2

Now source the setup file using the command,

source devel/setup.bash

Now, follow the next setup based on your interface selection.

Step:- 3

Export the ROS_IP and ROS_MASTER_URI by running below command inside the same terminal.

Replace the ROSDeviceAddress in below command by its value. (Use direct value here and not the char array i.e. export ROS_IP=192.168.92.132)

export ROS_IP=<ROSDeviceAddress>
export ROS_MASTER_URI=http://$ROS_IP:11311

Step:- 4

For Gazebo:

If you are using Gazebo then use the below command to launch the simulated robot with the required ROS drivers in Gazebo. Note that launching the UR robot in Gazebo does not require the robot's IP address.

roslaunch ur_gazebo ur5e_bringup.launch

For URSim:

This step assumes that you have opened the URSim and also configured the external control program node.

If you are using URSim then use the below-given command to launch the required ROS drivers. (As you have installed URSim in the same ROS machine then robot_ip will be localhost 127.0.0.1).

roslaunch ur_robot_driver ur5e_bringup.launch robot_ip:=127.0.0.1

For UR hardware:

This step assumes that you have configured the external control program node on the teach pendant and that also robot is in remote control mode.

If you are using UR hardware then use below given command to launch the required ROS drivers. Use the defined robotAddress as a value to robot_ip in the below command.

Note: Directly use the robot address as a numeric value and do not use a char array to assign value to the robot_ip.

roslaunch ur_robot_driver ur5e_bringup.launch robot_ip:=<robotAddress>

Load robot RigidBodyTree model and initialize the urROSNode interface

Here we are using Universal Robots UR5e robot model.

ur5e = loadrobot('universalUR5e');

Initialize the communication interface using urROSNode API.

ur = urROSNode(device.DeviceAddress,'RigidBodyTree',ur5e);

Before running the next section, wait for the robot to get ready for accepting the commands if you are using the URSim or the physical hardware. You will be able to see messages like "Robot connected to reverse interface. Ready to receive control commands." in your terminal when the robot is ready for receiving the commands.

Task-1: Move robot to home position (in joint space)

Command robot to perform task-1. For this task we have used sendJointConfigurationAndWait method to send joint command to the robot.

q_home = [0 -90 0 -90 0 0]*pi/180;

[result,state] = sendJointConfigurationAndWait(ur,q_home,'EndTime',5)

Task-2: Move robot to desired joint configuration (in joint space)

Command robot to perform task-2. For this task we have used sendJointConfigurationAndWait method to send joint command to the robot.

if isequal(interface,'Gazebo')
    jointWaypoints = [0 -90 90 -180 -90 0]*pi/180;
elseif isequal(interface,'URSim') || isequal(interface,'Hardware')
    jointWaypoints = [180 -90 90 -180 -90 0]*pi/180;
end

[result,state] = sendJointConfigurationAndWait(ur,jointWaypoints,'EndTime',5)

Task-3: Move robot in circular trajectory path (in task space)

Compute the desired circular trajectory for task-3.

center = [0.4918 0.1334 0.6872]; %[x y z]
radius = 0.2;

theta = (0:30:360)';
points = center + radius*[zeros(size(theta)) cosd(theta) sind(theta)];
points = [center;points];

wayPointTimes = 0:1.2:size(theta,1)*1.2;
taskWayPoints = [ones(size(points)) .*[-pi/2  0  -pi/2], points];

Send command to robot to perform task-3. For this task we have used followWaypoints method to send waypoints command to the robot.

followWaypoints(ur,taskWayPoints,wayPointTimes,'InterpolationMethod','minjerkpolytraj','NumberOfSamples',130);

Wait for the robot to complete the motion.

[result,~] = getMotionStatus(ur);
while ~result
    [result,~] = getMotionStatus(ur);
end
pause(2);

Task-4: Move robot in desired end-effectory Cartesian configuration (in task space)

Command robot to perform task-4. For this task we have used sendCartesianPoseAndWait method to send Cartesian command to the robot.

desPos = [-pi/2  0  -pi/2 0.4918 0.1334 0.6872];
[result,state] = sendCartesianPoseAndWait(ur,desPos,'EndTime',3)

Task-5: Move robot back to the home position (in joint space)

Command robot to perform task-5. For this task we have used sendJointConfigurationAndWait method to send joint command to the robot.

[result,state] = sendJointConfigurationAndWait(ur,q_home,'EndTime',5)

Generate a Standalone C++ ROS Node

Create a Function for Code Generation

To generate a standalone C++ node, we need to create a MATLAB function with the planned motion planning logic. This example uses pre-created MATLAB functions urStandaloneROSNodeGenerationFcnGazebo.m and urStandaloneROSNodeGenerationFcnURSim.m for the Gazebo and URSim simulator (also for UR5e hardware), respectively. Also, take care of the following things while creating a MATLAB function,

Note: It is expected that you get warnings similar to the one mentioned below during the code generation process.

Warning: The "JointNames" field of the "trajectory_msgs/JointTrajectory" message is a cell string which is not supported in code generation. Do not access the "JointNames" field in your MATLAB code.

Suppressing the above mentioned warning for the subsequent code sections.

warning('off','ros:mlroscpp:codegen:UnsupportedMessageField');

Generate and deploy the standalone ROS node

Create a config object.

cfg = coder.config('exe');
cfg.Hardware = coder.hardware('Robot Operating System (ROS)');
cfg.Hardware.BuildAction = 'Build and load';
cfg.Hardware.RemoteDeviceAddress = ROSDeviceAddress;
cfg.Hardware.RemoteDeviceUsername = username;
cfg.Hardware.RemoteDevicePassword = password;
cfg.Hardware.CatkinWorkspace = WorkSpaceFolder;
cfg.Hardware.DeployTo = 'Remote Device';
cfg.RuntimeChecks = true;

Assign a nodeName based on the selected interface.

if isequal(interface,'Gazebo')
    nodeName = 'urStandaloneROSNodeGenerationFcnGazebo';
elseif isequal(interface,'URSim') || isequal(interface,'Hardware')
    nodeName = 'urStandaloneROSNodeGenerationFcnURSim';
end

Generate a node and deploy it to targeted ROS machine. This might take longer time (may be around 5-10 minutes).

codegen(nodeName,'-config',"cfg");

Run the deployed standalone ROS node

Send a system command to ROS machine to run a generated standalone ROS node. If you have selected hardware then ensure that the polyscope is configured in the remote control mode. Also, Ensure that you have verified the ROS node on the URSim before using it with the physical UR5e.

system(device,['cd ' WorkSpaceFolder ';' ' source devel/setup.bash;' ...
                ' rosrun ' lower(nodeName) ' ' nodeName]);

Kill ROS master

clear ur;
system(device,'killall -9 rosmaster');

% turn on the suppressed warning
warning('on','ros:mlroscpp:codegen:UnsupportedMessageField');