Adds support for multi-groups

[thiagodefreitas]

This pull-request results from the implementation that adds support for multi-groups to the MOTOMAN robots.

This was succesfuly tested on a Motoman SDA10F and on a Motoman SIA10F for backwards compatibility purpose.

The modifications intend to be compliant with the REP-I001 :

https://github.com/ros-industrial/rep/blob/master/rep-I0001.rst

[gavanderhoorn]

Quick question: could you elaborate on whether this implements REPI0001? In particular: the DynamicJoint* messages seem to have been implemented as part of the ROS API, while the REP has them listed under Simple Message Structures.

[thiagodefreitas]

@gavanderhoorn : the Simple Messages Structures were implemented on the simple_message, as for example the joint_traj_pt_full_ex. The defined messages that you have mentioned are used for forwarding the information to the thing that is called on this picture as MOTOMAN_driver, but that is not properly named and the fidelity to the architecture is not 100%. It is a sketch ;)

The idea here is to have a /joint_trajectory_action that would be responsible for all the groups and one that would be responsible for each group individually. As per see the MoveIT! controllers.yaml that we have used here to make the tests with the SDA10F.

When I have implemented the multi-groups support for the MOTOMAN driver, the REP was used as a guideline, as I think it brings more if we can come to a common infrastructure for supporting the REP. That is the reason for the changes that also come on the industrial_core as this infrastructure is lacking. So, yes, the idea is to implement the REP.

[gavanderhoorn]

Interesting. Is the diagram you posted from a design document? Anything you can share? It would certainly help trying to understand the rationale behind the current implementation.

[thiagodefreitas]

Unfortunately, I can not share the document:exclamation:I can however try to provide some of the necessary details myself, as the discussion flows and demands.

For example, the simple messages are defined as:

PREFIX	LENGTH
HEADER	MSG_TYPE, COMM_TYPE, REPLY_CODE
BODY	DATA

For the Trajectory point message(streaming) , we created two new MSG_TYPE, JOINT_TRAJ_PT_FULL_EX=16, JOINT_TRAJ_FEEDBACK_EX=17

The JOINT_TRAJ_PT_FULL_EX is now very similar to a vector of JOINT_TRAJ_PT_FULL, actually there are some slight differences as you may see the vector type JointTrajPtData below:

_JOINT_TRAJ_PT_FULL_EX_

num_groups: int
sequence: int
multiJointTrajData: JointTrajPtData[]

_JointTrajPtData_

groupNo: int
validFields: int
time: float
pos: float[ROS_MAX_JOINT]
vel: float[ROS_MAX_JOINT]
acc: float[ROS_MAX_JOINT]

The feedback messages follow a very similar approach.

_JOINT_FEEDBACK_EX_

num_groups: int
multiJointTrajData: JointTrajPtData[]

For the messages that come from higher level packages, I did try not to interfere on how they send the messages nowadays. As for example, we let MoveIT! use its JointTrajectoryActions and write on the specific topic defined for the controller of the groups(single or combined/multi-groups), on the controllers.yaml.

Then, the joint_trajectory_action node can forward the new defined ROS message types to the MOTOMAN layer. The older one is kept there, as this new (REP compatible?) client is not backwards compatible and there would be the necessity for both. This has the advantage that we can include the control groups number and other informations on the message that comes from the ROS higher level layer before forwarding it to the state and motion interfaces.

The state and motion interface are now configured using state_configuration and motion_configuration.

As before, they are responsible for streaming and getting the data from the controller(in our tests the FS100), using and receiving the simple_messages, now extended with the JOINT_TRAJ_PT_FULL_EX=16 and the JOINT_TRAJ_FEEDBACK_EX=17.

A little big, but I hope it was clear.

[shaun-edwards]

Thanks for the contribution. I appreciate your effort. I have several questions/comments, but I'd like to address them a few at a time.

1. Did/would you consider creating a new package, industrial_robot_client2, instead of making the changes in place. My primary concern is that many other vendor packages link to industrial_robot_client. By making a new/copy version, we can isolate changes and lesson the risk.

[thiagodefreitas]

@shaun-edwards At implementation time, I did not consider creating a new package. As I did(at least tried hard) not to change any of the prior functions. The mods are more as additions or overloaded functions. Also, the old industrial_robot_client was kept intact. What I did was to create a new node(cpp file) and this itself contains many differences.

[thiagodefreitas]

@shaun-edwards However, I could consider creating a new package, if that would better suit.

[gavanderhoorn]

@thiagodefreitas: thanks for the additional info. Can you comment a bit on why you opted to not use the Dynamic* messages to communicate with the controller (ie: extend simple_message with them), and instead positioned them on the ROS side?

[thiagodefreitas]

@gavanderhoorn ,If I understood correctly your question...

the *JOINT_TRAJ_PT_FULL_EX" is intended to be a subset of the REP - DynamicJointPoint. As you may see on new MotoPlus. Therefore we have the multi-groups support on the simple-message side also.

A DynamicJointTrajectory replicates the REP definitions for the ROS-side. As it is actually(after expanding all sub-msgs):

Header header
string[] joint_names
int16 num_groups
int16 group_number
int16 num_joints
int16 valid_fields
float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start

This provides the capability from generating a message that directly contains the group information. And for the manufacturer it was easier to implement the extended capabilities with the new MSG_TYPE.

I have done a bigger comment on: ros-industrial/motoman#42 (comment)

[gavanderhoorn]

Thank you for the comments. I've also replied on PR 42.

[thiagodefreitas]

Withdrawing this PR due to reasons in ros-industrial/motoman#42