Point streaming and multiple robot groups

[riv-mjohnson]

Context:

I am taking over from @rr-dave, trying to set up our GP25 and 2-axis positioner with a YRC1000 controller as described in #585.

I have fixed the various MoveIt connections which were causing most of the issues in #585, such that we don't need #259 or #488.

We can now control the GP25 and positioner to move along independent trajectories. (We are only interested in moving one and then the other for now; we will hopefully have migrated to ROS2 by the time we need simultaneous movement).

The problem:

I am now working on getting point streaming working for systems with multiple robot groups. We only need point streaming for the robot, we do not need point streaming for the positioner at this stage.

For consistency, I have updated our other robots (a GP4 and a GP8, both with no Yaskawa external axes) to use a multi-group setup (with only one group on the controller).

The point streaming we are using is #37 (plus some rebasing and other (hopefully unrelated) changes).

I tried adding point streaming to multi-group setups by adding

sub_joint_command = this->node_.subscribe(
            ns_str + "/" + name_str + "/joint_command", 0, &JointTrajectoryInterface::jointCommandCB, this);

this->sub_joint_commands_[robot_id] = sub_joint_command;

to JointTrajectoryInterface::init(..., robot_groups, ...), to mirror that in JointTrajectoryInterface::init(..., joint_names, ...), but point streaming fails immediately with the error

Aborting POINT STREAM operation.  Failed to send point (#47): Invalid message (3) : Invalid robot ID (3004)

My questions:

• Is generalising point streaming from On-the-fly point streaming and Cartesian jog #37 to multi-group systems a sensible thing to try?
• Where should I look to debug the robot ID problem? I assume this is a property of SimpleMessage, but I'm not sure where to set it or what to set it to.
• Is there a better way of approaching the problem of point streaming to a GP25 and also being able to execute trajectories on both the GP25 and the two-axis positioner?

[riv-mjohnson]

Looks like MotomanJointTrajectoryStreamer::create_message uses a setRobotID method, which is presumably getting the wrong id from somewhere. I'll see if I can find out where and how to fix it.

[riv-mjohnson]

Okay, I think I understand what's happening. JointTrajectoryStreamer::jointCommandCB calls MotomanJointTrajectoryStreamer::create_message(..., JointTrajectoryPoint, ...), which uses setRobotId(robot_id_).

What I think I need is MotomanJointTrajectoryStreamer::create_message(..., DynamicJointsGroup, ...), which uses setRobotId(pt.group_number).

So I guess I need to write a JointTrajectoryStreamer::jointCommandCB(DynamicJointTrajectoryConstPtr).

[riv-mjohnson]

That seems to work. My code is here rivelinrobotics@d34b4ad if anyone is interested in cherry-picking it into a PR.

[gavanderhoorn]

Thanks for reporting on your progress + solution.

I noticed rivelinrobotics#1 was closed, not merged.

Did you discover something that was not correct in the end, or was the PR just for running CI?

[gavanderhoorn]

I have fixed the various MoveIt connections which were causing most of the issues in #585, such that we don't need #259 or #488.

late perhaps, but I would really recommend you do in fact incorporate the changes from #488.

The multi-group implementation in HEAD is incomplete, and #488 does fix some of those issues.

[riv-mjohnson]

Thanks for reporting on your progress + solution.

👍

I noticed rivelinrobotics#1 was closed, not merged.
Did you discover something that was not correct in the end, or was the PR just for running CI?

This was just because the branching in our fork is a mess. We decided to make a new master branch, rather than updating our kinetic-devel branch. We are now using the contents of that PR.

If we need to keep using motoman1 for much longer we will need to tidy up our fork, but we're hoping to migrate to motoros2 before that is necessary.

[gavanderhoorn]

we're hoping to migrate to motoros2 before that is necessary.

in this context feedback/input on Yaskawa-Global/motoros2#186 would be highly welcome.

[riv-mjohnson]

I have fixed the various MoveIt connections which were causing most of the issues in #585, such that we don't need #259 or #488.

late perhaps, but I would really recommend you do in fact incorporate the changes from #488.

The multi-group implementation in HEAD is incomplete, and #488 does fix some of those issues.

Our current solution appears to be working, so we're reluctant to make more changes unless something stops working. If we end up sticking with motoros1 for much longer and come across any problems we will look at #488 again.

[riv-mjohnson]

we're hoping to migrate to motoros2 before that is necessary.

in this context feedback/input on Yaskawa-Global/motoros2#186 would be highly welcome.

Sure thing. We are about half way through our migration to ROS2, and we haven't got to the motoman migration yet. No doubt we will have lots of feedback/input when we get to that stage.

[riv-mjohnson]

Actually, you're right, that discussion does look relevant to us at this stage. I'll discuss with the team, and hopefully someone will contribute soon.