The industrial_robot_client package contains several class/interface implementations of basic ROS-Industrial functionality using the simple_message protocol for communication with industrial robot controllers. These classes can be expanded through typical C++ derived-class functionality to implement robot-specific functionality that differs from this reference implementation (see here).

This page gives an overview of the industrial_robot_client library design, to help guide integrators in selecting which functionality to override when implementing new robot interfaces.

Overview

The industrial_robot_client library provides functionality for two main nodes that should be present in every robot interface:

  1. robot_state node -- publishes various topics related to the robot's current position, status, etc.

  2. joint_trajectory node -- subscribes to the command topic, to issue joint-position commands to the robot controller.

The functionality for these nodes is encapsulated into two primary classes in the industrial_robot_client namespace: RobotStateInterface and JointTrajectoryInterface. The node executable programs contain minimal code: just an instantiation of the class object and a few method-calls.

Integrators can derive new robot-specific classes from these base classes and override individual method calls as needed. The following sections describe these base classes and the main method calls in more detail.

State Publishing

The RobotStateInterface class contains functionality for publishing various robot position and status data at regular intervals, as received from the robot controller connection.

This class is primarily a wrapper around the MessageManager class, which listens to the simple_message robot connection and processes each incoming message according to its message type. In the default RobotStateInterface implementation, two MessageHandlers are registered with the manager:

  1. JointRelayHandler -- publish current joint positions in the joint_states topic.

  2. RobotStatusRelayHandler -- publish current robot status info in the robot_status topic.

Messages are published at a rate determined by the robot controller code. When new messages are received over the simple_message connection, they are immediately published on the appropriate topics. There is no buffering or polling, so messages are not guaranteed to be published at regular intervals.

Key Methods

  1. init() -- Establishes a connection to the robot controller.

    • By default, this method establishes a simple_message TCP-socket connection to the address specified in the robot_ip_address parameter using a default port.

    • Alternate forms use a robot connection specified by the calling code.
    • This method also attaches the default message-handlers.
  2. run() -- Begin listening to messages from the robot controller.

  3. add_handler(...) -- add (or replace) a new message handler

In general, integrators will probably not need to override any of the RobotStateInterface methods. Instead, robot-specific functionality may require altering the behavior of one of the default message-handlers. To change message-handler behavior, derive a new class from one of the existing handlers that implements the new behavior. Then, call the add_handler method to replace the default handler of that message type with the new robot-specific handler.

JointRelayHandler

The JointRelayHandler publishes the current robot joint positions. When a new message is received, it is processed as follows:

1) Retrieve all joint positions from the robot message
2) Call the create_message() method to generate ROS messages for publishing
  2.1) Call the transform() method to alter the reported positions, if needed
        - by default, this method does nothing
  2.2) Call the select() method to select a subset of joints for publishing
        - by default, this method implements the joint_name mapping functionality
3) Publish messages on the appropriate topics
4) Reply to the robot controller, if handshake requested

Integrators can override the transform() or select() methods to implement robot-specific behavior, or can override the create_messages() method to provide a completely custom message-parsing sequence.

RobotStatusRelayHandler

TBD.

Position Command

The JointTrajectoryInterface class encapsulates most functionality for commanding robot joint positions based on incoming ROS messages.

Since the ROS-Industrial specification allows multiple methods of transferring position commands to the robot controller, we separate the message-transfer methods from the message-creation methods. The JointTrajectoryInterface class is an abstract base-class, providing methods for translating ROS messages into simple_message messages. However, it only provides an interface specification for transferring the messages to the robot controller. The JointTrajectoryDownloader and JointTrajectoryStreamer classes provide the required methods to transfer positions to the robot controller, as described below.

Sequence of Operation

The JointTrajectoryInterface class subscribes to the command topic, and listens to the incoming message stream. Each incoming joint-position message is processed using the following steps:

  1. Call trajectory_to_msgs() method to convert ROS message to simple_messages
      - Loop over all trajectory points, executing the following steps
      1.1 Call select() method to select/reorder joints for robot comms.
      1.2 Call transform() method to alter the reported positions, if needed
      1.3 Call calc_speed() method to calculate a scalar velocity representation
      1.4 Call create_message() method to create the simple_message
  2. Call send_to_robot() method to send trajectory to the robot controller

Key Methods

  1. init() -- Establishes a connection to the robot controller.

    • By default, this method establishes a simple_message TCP-socket connection to the address specified in the robot_ip_address parameter using a default port.

    • Alternate forms use a robot connection specified by the calling code.
    • This method also subscribes to the command topic

  2. run() -- Begin listening to messages on the ROS topic.

  3. trajectory_to_msgs(...) -- Convert ROS trajectory to a list of simple_messages

  4. select(...) -- select/reorder points

    • transforms from ROS ordering (arbitrary) to order expected by the robot server
    • by default, this method implements the joint_name mapping functionality

  5. transform(...) -- alter the commanded joint position values

    • by default, this method does nothing
    • this can be overridden to implement robot-specific transforms (e.g. joint-coupling)
  6. calc_speed(...) -- calculate a scalar velocity & duration

    • the simple_message protocol expects a scalar velocity/duration, but the ROS topic provides a velocity for each joint
  7. calc_velocity(...) -- calculate a scalar velocity from joint trajectory

    • by default, velocity = the lowest joint_vel_ratio (joint_vel / joint_max_vel)
  8. calc_duration(...) -- calculate a scalar duration from joint trajectory

    • by default, duration = the expected time to complete the motion
      • NOTE: this uses the trajectory timestamps, not pos/velocity.

  9. send_to_robot(...) -- interface definition for a method to transfer command messages to the robot controller

    • the JointTrajectoryInterface class does not provide an implementation of this method

    • reference derived-class implementations are provided for both download and streaming methods, as described below.

In general, integrators should derive from one of the download/streaming classes listed below and override the select(), transform(), or calc_velocity() methods to implement robot-specific behavior. If a non-standard robot connection is used, you could still derive from the JointTrajectoryInterface class and provide a new robot-specific send_to_robot() method, if appropriate.

Download/Streaming Classes

The JointTrajectoryDownloader class is derived from the JointTrajectoryInterface base class, and provides an implementation of the send_to_robot() method. This implementation sends the entire trajectory to the robot as a sequence of messages, with "magic" markers to signal the beginning and end of the trajectory. The robot should begin executing the trajectory as soon as the last trajectory point is received.

The JointTrajectoryStreamer class is also derived from the JointTrajectoryInterface base class, but provides a send_to_robot() implementation using a position-streaming approach. In this method, a separate thread is used to stream individual position commands to the robot controller. A new position is not sent until the robot has executed the previous motion. The robot-side implementation may use a small buffer of positions to achieve smooth motion between successive commands.

Generic Robot-Interface Nodes

The industrial_robot_client package also provides generic nodes that run the basic reference implementation classes described above. These nodes can be used for initial testing of new robots, or for robots that do not require robot-specific processing. The following nodes are provided:

File Summary

Filename

Description

generic_joint_downloader_node.cpp

Main src-file for generic motion_download_interface node

generic_joint_streamer_node.cpp

Main src-file for generic motion_streaming_interface node

generic_robot_state_node.cpp

Main src-file for generic robot_state node

joint_relay_handler.*

message-handler for publishing joint positions

joint_trajectory_action.*

actionlib interface

joint_trajectory_downloader.*

position-command class using download method

joint_trajectory_interface.*

position-command abstract base class

joint_trajectory_streamer.*

position-command class using streaming method

robot_state_interface.*

state-publishing base-class

robot_status_relay_handler.*

message-handler for publishing robot status

Wiki: industrial_robot_client/design (last edited 2013-01-07 21:00:46 by JeremyZoss)