driver: add support for MotoROS SelectTool

[gavanderhoorn]

As per subject.

This is just the copy-paste boilerplate search-replace result.

Hasn't been tested on HW yet.

[riv-robot]

@gavanderhoorn Do you want help testing on hardware?

[gavanderhoorn]

@gavanderhoorn Do you want help testing on hardware?

That would be very much appreciated, yes.

Similar to what I wrote in #435 and #439, merge and build should be enough on the ROS side.

If you don't actually need the mpSetToolNo(..) part of this (see discussion in #440), any MotoROS version >= 1.9.0 should work.

[gavanderhoorn]

I've left some in-line comments @EricMarcil and @ted-miller.

If you could take a look that would be appreciated.

@marip8 @steviedale: your thoughts would also be appreciated.

Note though that I really just did a copy-paste and search-replace of the ReadSingleIO files, so if you come across something which you feel should be done in a nicer way (or is duplicated unnecessarily), that would be expected. We might consider deduplicating some of the result code -> error message functionality, but this PR focuses on the functionality first.

[steviedale]

@marip8 @steviedale: your thoughts would also be appreciated.

Hey @gavanderhoorn, just wanted to give you a heads up that I'm no longer working with Southwest Research Institute so I won't be contributing to ROS-I anymore. But its been a pleasure working with you and the rest of the community and I wish you all the best!

[gavanderhoorn]

@rr-robert: just for planning purposes: could you give me an indication when you'd have time to test this on your HW?

[gavanderhoorn]

Oh and please note that you should actually use the updated MotoROS .out from #440.

[gavanderhoorn]

Just realised: the select_tool service would be one we should/could expose per-group, instead of only a single instance per controller (or: driver instance).

The implementation in this PR only exposes a global one (which works as SelectTool requires both a group_number and a tool_number).

[gavanderhoorn]

I've rebased this on kinetic-devel, so it now includes #389 which was just merged.

17f9482 has been updated to make use of the new smpl_msg_conx_mutex_ instead of the incorrect mutex_ (which is in industrial_robot_client::JointTrajectoryStreamer, see also #389).

[gavanderhoorn]

@rr-robert: friendly ping?

[akashjinandra]

Hello @gavanderhoorn, just tested this on the YRC1000 controller after switching to the 199 .out file , and was able to switch tools and validated that it switched on the pendant.

[gavanderhoorn]

Thanks @akashjinandra.

[gavanderhoorn]

I've added 1ce68a4 which clarifies changing the tool used for jogging is not supported on FS100 controllers.

[gavanderhoorn]

@akashjinandra: have you also tested trying to switch to a tool file outside the defined range (ie: > 63)? And for a non-existent group?

[gavanderhoorn]

@marip8: it's not the nicest code (as it's copy-paste search-replace), but it is apparently functional, so the ROS-side could now also be reviewed.

[gavanderhoorn]

@marip8 wrote:

would it make sense to push the tool change service into a separate node like the current I/O interface? That would eliminate the need for the connection mutex, but it does seem like the current tool is loosely related to motion control

selecting the tool file is actually intimately tied to "motion" (even if we only execute joint space trajectories at the moment, it still notifies the Yaskawa controller of important information about what is actually mounted on the flange), so from that perspective putting it in MotomanMotionCtrl made sense to me.

As to using a separate connection: that would require changes to MotoROS, and quite substantial ones. It would essentially either require a new socket added to the existing MotionServer and refactoring the network handling code, or a completely new task, with a new server implementation and refactoring of the MotionServer to transplant the affected code paths.

All-in-all, I don't believe it'd be worth it.

[marip8]

As to using a separate connection: that would require changes to MotoROS, and quite substantial ones.

I didn't realize there wasn't an abundance of ports available on the controller. It seemed like we just incremented the port number for the IO relay, so I thought we might be able to do the same here if it made sense.

Since the tool is tied closely with execution, I'm on board with keeping this functionality in the motion control class

[gavanderhoorn]

I didn't realize there wasn't an abundance of ports available on the controller. It seemed like we just incremented the port number for the IO relay, so I thought we might be able to do the same here if it made sense.

no, sockets are available. That's not the problem.

Changing the tool frame on the controller however has two sides, and while one of those actions can easily be delegated to a separate task, the other needs to mutate state that would be part of the MotionController. So we'd avoid locks on the motoman_driver side, only to have to introduce them on the MotoROS side (if we want to do it right).

[gavanderhoorn]

I've tested this on our YRC1000 as well.

No regressions detected.

Requests for changing to tool files outside the valid range are refused as they should.

[riv-robot]

@gavanderhoorn I'm sorry for the silence - commercial activities exploded just after I sent that message offering to test it! This is awesome that you guys decided to get on and implement the enhancement. Massive thanks.

What (if any) testing do you still need on this?

[gavanderhoorn]

I believe we've sufficiently tested this and it seems to work.

If you want, you could build the driver with this PR included and try to invoke the service.

[gavanderhoorn]

@marip8: I believe I've applied all your suggestions.

Could you take a final look?

CI is very slow today, but if you're OK with these changes and CI is green, we'll merge.

[gavanderhoorn]

thanks for the review @marip8 :+1: