Adds support for multi-groups on Motoman Robots

[thiagodefreitas]

This pull-request adds support for multi-groups on the MOTOMAN ROS package.

This needs the latest version from motoman_driver from @ted-miller

This depends on:
ros-industrial/industrial_core#82

• Succesfully tested on the Motoman SDA10F and on the Motoman SIA10F.

[shaun-edwards]

@thiagodefreitas, Can you please point me to the launch file that brings up a dual arm system (I'd like to use rqt_graph to get an understanding of the graph structure).

[thiagodefreitas]

@shaun-edwards:
The launch file would be robot_interface_streaming_sda10f.launch. The only thing I have set is the robot_interface_streaming.launch to use the joint_trajectory_action2. Would you prefer as an argument or to create a new launch file?

[shaun-edwards]

I tried the launch file you recommended, but it fails almost immediately. I don't have a controller connected, but I expected the nodes to just block until a connection was made (this is what happens in the released version). Could you try on you end just to verify I don't have the wrong configuration.

Output

ros@ROS:~/ros/multi_arm_control/src/motoman/motoman_sda10f_support/config$ roslaunch motoman_sia10f_support robot_interface_streaming_sia10f.launch controller:=dx100 robot_ip:=192.168.32.4 --screen
... logging to /home/ros/.ros/log/e451674e-286f-11e4-931d-080027f3243a/roslaunch-ROS-23978.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ROS:41143/

SUMMARY
========

PARAMETERS
 * /controller_joint_names
 * /robot_ip_address
 * /rosdistro
 * /rosversion

NODES
  /
    joint_state (motoman_driver/robot_state)
    joint_trajectory_action (industrial_robot_client/joint_trajectory_action2)
    motion_streaming_interface (motoman_driver/motion_streaming_interface)

auto-starting new master
process[master]: started with pid [23992]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to e451674e-286f-11e4-931d-080027f3243a
process[rosout-1]: started with pid [24005]
started core service [/rosout]
process[joint_state-2]: started with pid [24017]
[ INFO] [1408542177.327995275]: Added message handler for message type: 0
[ INFO] [1408542177.340245174]: Robot state connecting to IP address: '192.168.32.4:50241'
terminate called after throwing an instance of 'XmlRpc::XmlRpcException'
process[motion_streaming_interface-3]: started with pid [24033]
[ INFO] [1408542177.458043681]: Joint Trajectory Interface connecting to IP address: '192.168.32.4:50240'
terminate called after throwing an instance of 'XmlRpc::XmlRpcException'
process[joint_trajectory_action-4]: started with pid [24049]
terminate called after throwing an instance of 'XmlRpc::XmlRpcException'
[joint_state-2] process has died [pid 24017, exit code -6, cmd /home/ros/ros/multi_arm_control/devel/lib/motoman_driver/robot_state __name:=joint_state __log:=/home/ros/.ros/log/e451674e-286f-11e4-931d-080027f3243a/joint_state-2.log].
log file: /home/ros/.ros/log/e451674e-286f-11e4-931d-080027f3243a/joint_state-2*.log
[motion_streaming_interface-3] process has died [pid 24033, exit code -6, cmd /home/ros/ros/multi_arm_control/devel/lib/motoman_driver/motion_streaming_interface __name:=motion_streaming_interface __log:=/home/ros/.ros/log/e451674e-286f-11e4-931d-080027f3243a/motion_streaming_interface-3.log].
log file: /home/ros/.ros/log/e451674e-286f-11e4-931d-080027f3243a/motion_streaming_interface-3*.log
[joint_trajectory_action-4] process has died [pid 24049, exit code -6, cmd /home/ros/ros/multi_arm_control/devel/lib/industrial_robot_client/joint_trajectory_action2 __name:=joint_trajectory_action __log:=/home/ros/.ros/log/e451674e-286f-11e4-931d-080027f3243a/joint_trajectory_action-4.log].
log file: /home/ros/.ros/log/e451674e-286f-11e4-931d-080027f3243a/joint_trajectory_action-4*.log

[thiagodefreitas]

@shaun-edwards
I know it is not optimal, but for the moment...

NEW VERSION FROM THE MOTOMAN DRIVER
roslaunch motoman_sda10f_support robot_interface_streaming_sda10f.launch controller:=dx100 robot_ip:=192.168.32.4 legacy_mode:=false --screen

OLD VERSION FROM THE MOTOMAN DRIVER
roslaunch motoman_sda10f_support robot_interface_streaming_sda10f.launch controller:=dx100 robot_ip:=192.168.32.4 legacy_mode:=true --screen

Could you please test? Works for me.

[gavanderhoorn]

I don't have a controller connected, but I expected the nodes to just block until a connection was made [..]

Wasn't the point of @shaun-edwards post that starting the nodes without any controller immediately results in crashes?

[thiagodefreitas]

@gavanderhoorn: I have just pulled the latest version from ipa320/motoman and ipa320/industrial_core and ran without any controller attached:

NEW VERSION FROM THE MOTOMAN DRIVER
roslaunch motoman_sda10f_support robot_interface_streaming_sda10f.launch controller:=dx100 robot_ip:=192.168.32.4 legacy_mode:=false --screen

OLD VERSION FROM THE MOTOMAN DRIVER
roslaunch motoman_sda10f_support robot_interface_streaming_sda10f.launch controller:=dx100 robot_ip:=192.168.32.4 legacy_mode:=true --screen

This does not crash for me, it hangs for both options. I wanted @shaun-edwards to check if it still happens for him. Then, I would need to take further investigations.

[shaun-edwards]

@thiagodefreitas, both versions worked for me (I think will revisit launch structure later, but good for now). Looking at the graph, I have a few questions:

1. I see the groups are defined in sda10f_state/motion_interface.yaml. These correlate with namespaced topics. However, there are some extra topics that aren't specified in the yaml. In particular, /robot_status, /joint_path_command, /joint_states, and /dynamic_feedback_states. What are these topics for? How are they configured? I assume they capture all the joints, but wouldn't it be possible to just add this to the config file. There are use cases in which controlling all the joints in a robot is not required (say multiple painting robots on a line).

2. Why are the new message types (industrial_msgs/DynamicJointTrajectory required for <ns>/joint_path_command and joint_path_command? Why are the new message types (industrial_msgs/DynamicJointTrajectoryFeedback) required for dynamic_feedback_states(new name?), but not for <ns>/feedback_states. There should be symmetry here, right? Why not utilize the sensor_msgs/JointState and trajectory_msgs/JointTrajectory. This would be more consistent with ROS-I, and it would allow us to utilize existing simulation tools. The namespaces would provide seperation that seems to be captured in the dynamic messages.

3. I see that the there is one joint_trajectory_action node that handles sending commands to the appropriate topics. However, isn't this built into MoveIt via the controllers.yaml file. If we had multiple joint_trajectory_action nodes (i.e. the current versions), MoveIt could decide which one to use. It feels like you have replicates this controller selection capability of MoveIt in the action server, but instead you are selecting topics.

   I'm sure you made these decisions for specific reasons. The ROS-I spec didn't envision creating new ROS messages. I just want to understand what was the rationale.

[thiagodefreitas]

@shaun-edwards ,

1. [...] I assume they capture all the joints, but wouldn't it be possible to just add this to the config file. There are use cases in which controlling all the joints in a robot is not required (say multiple painting robots on a line).

The assumption that they capture everything is correct. e.g. /joint_path_command is for commanding all the joints. And /dynamic_feedback_states is a message that contains the information from all the groups connected to the controller. I could add some sort of configuration from the yaml, that would be no problem. For the use case that we had here, this was necessary, but I agree with you that it would be better to have this configurable somehow.

Why are the new message types (industrial_msgs/DynamicJointTrajectory required for /joint_path_command and joint_path_command? Why are the new message types (industrial_msgs/DynamicJointTrajectoryFeedback) required for dynamic_feedback_states(new name?), but not for /feedback_states. There should be symmetry here, right? Why not utilize the sensor_msgs/JointState and trajectory_msgs/JointTrajectory. This would be more consistent with ROS-I, and it would allow us to utilize existing simulation tools. The namespaces would provide seperation that seems to be captured in the dynamic messages.

As for question 1, dynamic_feedback_states is the feedback state for all the control groups at once, containing the group number and group specific info. Actually, it was provided as a possible feature, but we did not happen to use. /feedback_states as well as /feedback_states uses the existing ROS messages.

I see that the there is one joint_trajectory_action node that handles sending commands to the appropriate topics. However, isn't this built into MoveIt via the controllers.yaml file. If we had multiple joint_trajectory_action nodes (i.e. the current versions), MoveIt could decide which one to use. It feels like you have replicates this controller selection capability of MoveIt in the action server, but instead you are selecting topics.

A JointTrajectory msg ends up being:

Header header
string[] joint_names
float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start

A DynamicJointTrajectory imitates 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

A point to be clear is that 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.
One of the ideas for the new messages support on the ROS-client is that I could have a message from where I could directly command the multiple groups.

I mean, we could have a scenario from where, some higher level package generates a classical JointTrajectoryAction and the client can perform the action for all the groups, making the aforementioned. We could have the same thing for the namespaced joint trajectory actions, what later came to the controllers.yaml from MoveIT!

Or we could even directly command the intended group, when sending directly the message from a ROS nodenew messages and individual groups separately. This would be then a one-to-one mapping between the ROS message and the Simple message generated in compliance with the REP.

[gavanderhoorn]

One of the ideas for the new messages support on the ROS-client is that I could have a message from where I could directly command the multiple groups.

With directly, you mean you can address them individually, using separate messages?

Was this not possible with the existing support for multiple planning groups in moveit and suitable logic combining / coordinating the multiple trajectories in the new trajectory_relay node? I'd always understood rep-I0001 - Node Configuration as suggesting to exploit the moveit planning group support.

A trajectory including all joints would automatically map onto synchronised motion for all involved groups. Trajectories including only the joints for a single planning group would result in motion for that group. Separate trajectories for different groups would result in non-synchronised motion of the relevant groups.

I'm really just asking for the rationale here, which was also why I asked for the design document :).

[thiagodefreitas]

A trajectory including all joints would automatically map onto synchronised motion for all involved groups. Trajectories including only the joints for a single planning group would result in motion for that group. Separate trajectories for different groups would result in non-synchronised motion of the relevant groups.

That's exactly how it works now. Including using the multiple planning groups from MoveIT!.

The rationale is actually simple so that the client receives the "JointTrajectory msg" from MoveIT! or anywhere else , and according to the planning group attaches the controller group info then forwards it directly to the motoman driver where it is converted to a simple message(REP).

The new messages could also be used so that you could from the ROS side program a trajectory that already contains the control groups information, that is mostly the main point for this approach, and one of the requirements.

How this combination/coordination from the multiple trajectories is done, can lead to many variants. This is the approach that we have got after some internal discussions and after that first loop on the mailing-list. I can somehow variate it, but I need to wait after it reaches some sort of consensus on the Theme :)

[gavanderhoorn]

That's exactly how it works now. Including using the multiple planning groups from MoveIT!.

Right, ok.

The new messages could also be used so that you could from the ROS side program a trajectory that already contains the control groups information, that is mostly the main point for this approach, and one of the requirements.

So the Dynamic* messages are just an alternative interface then? Would we all agree that it would be better to use different msg names then? Seeing as intent and content is different from the REP? Just to avoid confusion?

Or we should change the contents of the REP.

This is the approach that we have got after some internal discussions and after that first loop on the mailing-list.

Hm, I remember that we concluded that thread with "we'll report back to the list when we come up with something" ..

[thiagodefreitas]

So the Dynamic* messages are just an alternative interface then?

That's right. Thanks for summarizing that :)

Would we all agree that it would be better to use different msg names then? Seeing as intent and content is different from the REP? Just to avoid confusion?

+1 on the aspect that the names confusion needs to be fixed.

The DynamicJointPoint mentioned on the REP Simple Message section should be considered as MSG_TYPE JOINT_TRAJ_PT_FULL_EX and JOINT_FEEDBACK_EX a subset of DynamicJointState .

Or we should change the contents of the REP.

I have no fixed opinion on that, I mean if the REP should contain also those messages.

As I see, there are 3 different aims on this pull request and on ros-industrial/industrial_core#82. The first to provide multi-groups support to the motoman robots. The second to provide the first version of some base infrastructure for the REP on the industrial_core level. And finally to make this MOTOMAN driver REP compatible. Where the first has definitely the highest priority for us.

Although I have sent the PR, I am going to take the opportunity to make some comments :)

Going on the REP-compatible path, there are some points that I have thought to be analysed:

1. To guarantee the compliance of the new MSG_TYPEs.
2. To incorporate the new-defined ROS messages on the industrial_core package.
3. If there is the need to add the ROS messages to the REP
   4.If the new client can be on industrial_core or if there should be created a new package.
4. And something that really depends on 2 is that if this client could have the defined ROS messages as an interface. As if not, this would need the rationale from the node to be changed, and therefore I would be really keen to avoid that at this point, at least for now.

To conclude, I see some variations that could make the driver with a completely MOTOMAN specific solution:

1. Concerning the new MSG_TYPEs, if needed, we could go back to the solution that those were implemented as new MOTOMAN specific messages.
2. Concerning the new ROS messages, we could bring them all to the motoman package.
3. The same for the interface client.

[shaun-edwards]

This URDF(xacro) is more complex than it has to be. It should really be a macro call for the base, and two calls to an arm macro (with different prefixes). This would also ensure that link/joint naming matches the current conventions as well.

[shaun-edwards]

Ok...this discussion really helped me understand what was going on. I'm going to try and summarize the current state of the discussion and capture the todo items.

I expected this PR to look something like the REP, but after this discussion, I now understand that it does not.

1. The REP was meant to standardize communications between the client on the PC and the server on the controller (here). The approach taken is specific to Motoman and will not work for other controllers. Specifically, fixed message lengths were used. The REP specified dynamic messages in order to limit the communications burden on other, less powerful, platforms. This is OK. The current Motoman client is specific to their controller.
2. The REP also aimed to adopt ROS standards for the API (here). This may have occurred, but I'm not sure. Furthermore, it is obscured by the introduction of new messages/interfaces that were unfortunately named the same as REP simple messages. These new messages were a requirement of the project, so this is also OK.

The goal of ROS-Industrial is to remain vendor/platform agnostic. This can't occur if vendor specific requirements are included in the core. The proper place for this type of code is in the vendor repo. (Note: In order to perform a Hydro release of this capability, this was always going to happen. I do not feel comfortable releasing the proposed core changes in between ROS releases). As a result, this PR will have to be modified to address the following issues:

• Withdraw the PR to industrial core ( Adds support for multi-groups industrial_core#82 ).
• Pull industrial_robot_client changes into the motoman_driver package. If the driver is backwards compatible, the original package can be modified, otherwise a new motoman_driver2 package should be created.
• Create a new package for motoman_msgs. Rename the messages if another name makes sense (the package should isolate them even if they aren't renamed). Remove references to the REP since the REP does not specify the ROS API.
• Any simple_message ID's that need to be added, should be added within the Motoman assigned ranges.
• Fix the URDFs in the motoman package based on this PR's comments.
• Determine the best way to address "legacy" support in the launch files. "Legacy" isn't really descriptive enough. We should either come up with something more intuitive, or provide enough comments in the launch files to explain the difference.
• Document the yaml file format in some way (ideally on a wiki, or at the very least in comments in the existing file).
• Optional: Add a moveit config package. I realize this may not have been part of the original project, but much of the confusion in this review was a lack of an example use case.

I think this capability is very important. I am very thankful for the contribution. Perhaps this approach will prove in the future to be more broadly applicable then it appears to be. If this occurs, then I will have no problem porting this functionality from the motoman packages to the core industrial packages.

For now, I think this compromise allows for this cool capability to be used by others, while minimizing the potential impact to other platforms.

[thiagodefreitas]

• Withdraw the PR to industrial core ( Adds support for multi-groups industrial_core#82 ).
• Pull industrial_robot_client changes into the motoman_driver package. If the driver is backwards compatible, the original package can be modified, otherwise a new motoman_driver2 package should be created.
• Create a new package for motoman_msgs. Rename the messages if another name makes sense (the package should isolate them even if they aren't renamed). Remove references to the REP since the REP does not specify the ROS API.
• Any simple_message ID's that need to be added, should be added within the Motoman assigned ranges.
• Fix the URDFs in the motoman package based on this PR's comments.
• Determine the best way to address "legacy" support in the launch files. "Legacy" isn't really descriptive enough. We should either come up with something more intuitive, or provide enough comments in the launch files to explain the difference.

I would appreciate some additional input on the legacy_mode issue. I thought of calling this "multi_groups", so that multi_groups=true would imply to use the new version, is that ok?

• Document the yaml file format in some way (ideally on a wiki, or at the very least in comments in the existing file).
• Optional: Add a moveit config package. I realize this may not have been part of the original project, but much of the confusion in this review was a lack of an example use case.

I need to do another test on the beginning of next week on the hardware. For that I am going to try to use it our current MoveIT! config and change it, so that I can add that to the pull request.

[shaun-edwards]

I would appreciate some additional input on the legacy_mode issue. I thought of calling this "multi_groups", so that multi_groups=true would imply to use the new version, is that ok?

Is there a way to not use the old ROS driver. Will the new client (PC side) work with either server (Controller side). I'm pretty sure this is not the case, but I thought I would ask. Perhaps we could trigger based on "version_0.x=true" or "version_1.x=true", assuming version 1 is the new version (we should match motoman's versioning. I do not like "multi_groups" since I believe this driver could be used with a single arm. FYI...I suggest that we wrap the "legacy" driver in a launch file with a "deprecated" warning, see here This would warn anybody launching the old driver that it will be removed.

[gavanderhoorn]

I think this needs a dependency on motoman_msgs_.. now as well.

[gavanderhoorn]

missing ${prefix}

[gavanderhoorn]

Some more general comments:

• the indentation issues may not all be yours (the original industrial_robot_client and simple_message classes have them as well), but we can at least fix them in files that are added by this PR I think.
• there are quite some member variables and methods missing (doxygen) comments, mostly in the classes that were extended to support multiple groups and / or legacy mode.
• the robot id or group number variable is used quite extensively throughout the code (obviously), but there are very few places where it is actually checked to be a valid value. As the []operator does not do any bounds checking, this may lead to weird errors. Perhaps a more defensive programming style should be used.
• the code assigns default values when parameters are not set by the user, but doesn't communicate this. I think certainly for things like group numbers this is important.
• there are some TODOs left in the code. When this PR is accepted, those should be converted into issues (and perhaps updated to refer to the issues).

Some suggestions:

• run roslint on the code (this may not be really fair, as the current industrial_robot_client and simple_message will probably also fail this)
• run catkin_lint on the newly created support packages

[thiagodefreitas]

@shaun-edwards Going to proceed like this. Using the version number.

@gavanderhoorn Thank you very much for all the comments and for the detailed inputs. Really appreciate that. I am going to try to address all of them carefully.

[shaun-edwards]

ping @thiagodefreitas, where are we on this?

I'd like to try these changes out on our SIA20D with the old driver installed to ensure backwards compatibility. I'll be doing this in the next couple of days using the existing PR (assuming none of the requested changes will alter how this is done).

[thiagodefreitas]

@shaun-edwards Tested on the sda10f.

1. Not all, but many roslint fixes done. Also, overall fixes between tabs and spaces, and catkin_lint.
2. From your previous list, missing "Document the yaml file format in some way (ideally on a wiki, or at the very least in comments in the existing file)."
3. Although motoman joints are usually named as joint_letter, I have an issue when running both drivers(even the current state of the older one from ros-i hydro-devel), where I can not work properly with MoveIt!. I would suggest that an issue is created to investigate this problem with the alphabetical order. For the moment, I have kept it on the URDF as joint_number_letter to avoid this problem.

[gavanderhoorn]

A number of review comments have not been addressed, but also have not been moved to issues (assuming defaults, magic nrs, ambiguous logging statements). Are these planned to be addressed in the future?

[shaun-edwards]

@thiagodefreitas, can you add any remaining items as issues (or comment inline why they aren't addressed.

@gavanderhoorn, the PR was accepted in order to get ready for ROSCon. It also helped to get others to use the software and shake out the bugs (which some have been found). The release plan #47, does not officially release a hydro version (i.e. only available on the hydro-devel branch). We will address these issues and new ones before an indigo release.

[gavanderhoorn]

@shaun-edwards: I appreciate the pressure because of ROSCon, but testing the functionality could have just as easily been done using a branch from this PR.

I agree that there are no releases planned for Hydro, but imho that doesn't mean we can just treat hydro-devel as a free-for-all. This is not an _experimental repository. Silently assuming defaults fi seems like a dangerous thing to me, which imo should have been fixed before accepting this PR.

[thiagodefreitas]

@shaun-edwards, @gavanderhoorn

I have started commenting in-line,but most of the comments are not valid anymore for the current state.
Including the mentioned valid_fields "magic number".

As the version0 aspect is going to be fixed by the release plan #47

I would therefore list the following issues:

• xacro identation
• joint names convention when using with moveit, and we also had that problem with the version0 driver.
• Update documentation

FYI: We are no treating the hydro-devel as a free-for-all or experimental rep. This driver has run stably for months for our use cases here at IPA. And has also been tested by the manufacturer itself during on-site visit. And also, the older version is isolated as it was, to avoid causing problems for the current users of the source version. As Shaun mentioned, there is not going to be any released version for this new version on Hydro.

It also helped to get others to use the software and shake out the bugs (which some have been found)

+1 on that. I do not see the issues as so critical, as that they would avoid the merge or avoid people start using the driver. As we can not address all the user cases here, I think the driver itself benefits much more if it has a wider community adoption and off course if bugs are found asap.

Unfortunately, due to legal aspects we could not push the code "in pieces" during the project. I myself apologize for that, as I know that there are too many changes to be checked, what may have implied much of the confusion during the PR. But do not think that it made it easier for our side.

[gavanderhoorn]

@thiagodefreitas: from a software-quality perspective, the fact that it has been running "for [your] use cases" is obviously not very relevant. Code reviews are important exactly for cases that you did not consider, or did not come across. The fact that there are no unit-tests only increases the importance of reviews, imo.

re: merging to get people to test it: how is testing from hydro-devel (a branch) different from testing a specific branch from a PR?

re: list of outstanding issues: I don't really care about the indentation. I don't have time to go through everything, but some items that should be appended to your list:

• ambiguous logging messages: afaict none of the logging statements in parts of the new code that deal with multiple groups tell the user for which group they are doing things. Example: JointFeedbackRelayHandler::convert_message(.., int robot_id).
• silently assuming defaults: the various subclassed industrial_robot_client nodes now look for a parameter robot_id, and use a default 0 if the user has not provided one. Using defaults is fine, but at least warn (or inform) the user. Afaict none of the newly added or changes nodes do so. Example: JointFeedbackRelayHandler::init(..).
• repeated code / definitions of (the same) constant / magic nr. Example: MAX_NUM_GROUPS is defined in both joint_feedback_ex.h and joint_traj_pt_full_ex.h. The same constant (but now as magic nr) is used in joint_trajectory_interface.cpp (admittedly, with a TODO attached).

Please do not take this personal, I was merely trying to push back a bit against the "o we need this for roscon, so let's merge and we'll do things later".

[thiagodefreitas]

Created #51 , #52, #53, #54 .