Io support rebased

[pbeeson]

These are the changes we made to get to to work on the MH180. We needed the service init and we needed to make sure that joint states could properly handle multiple /joint_state publishers even if the incoming messages didn't agree with the 6 joints controlled by this driver.

[gavanderhoorn]

Thanks for the PR 🍻

I'll take a look at the included changes some time next week.

Can you explain e369ddc a bit more?

[pbeeson]

e369ddc was needed because we kept getting messages that the trajectories we requested didn't match the joints stored. Because we have a separate driver for the gripper that publishes /joint_states, this driver occasionally gets those messages and overwrites the motoman joint info with that info then says the motoman trajectories we send aren't consistent with the current joint info. The fix simply logs which joints this driver cares about so that it can ignore incoming /joint_states messages that deal with other joints.

[gavanderhoorn]

Interesting. Have you seen ros-industrial#111? Could it be that you ran into that?

[gavanderhoorn]

As to motoman_driver nodes receiving JointStates they don't know/care about: to avoid these things I typically run the driver in its own namespace, then use a joint_state_publisher instance with a properly configured source_list parameter to publish the aggregate state.

That keeps things nicely isolated and I believe avoids the need for changes.

But it might be that the current implementation is slightly naive, and should indeed filter incoming joint states.

[pbeeson]

No. This was a different error about a "non-valid key", when it went to look up joint states in the trajectory in a map somewhere, because our two gripper joints overwrote the last joint_States message for the 6 motoman joints. You don't see this in the groups code, it's only for code that uses the joints read from the parameter at start time. We do see that issue 111, but I have logic in our code that tries to add waypoints if we are out of the 5e-5 hardcoded threshold they set. Despite this, we do very rarely still see that issue with new new waypoint, but haven't had time to track it down.

[gavanderhoorn]

Ok. Thanks for the info.

[pbeeson]

While I agree this is a solution, I think any ROS node that deals with
joint states should handle asynchronous, multiple publishers.

[gavanderhoorn]

Well, that would be a possibility, but without a re-usable library a la TF to take care of merging the potentially many fragmented JointStates I don't think I'd be in favour of that.

[pbeeson]

I disagree. Every row nose I make that handles joint states, I force to
deal with multiple fragmented joint state messages. You never know who and
how people are going to use your nodes.

[pbeeson]

Ros node....Spell check on phone

[gavanderhoorn]

You never know who and how people are going to use your nodes.

Exactly. So exploit node isolation and the data flow configuration infrastructure (remapping) and run them in their own namespace.

Why make things more complicated?

[pbeeson]

Because I can't control how people set up their launch files, so I never rely on the fact that someone will copy my launch file and use namespaces appropriately. So, I always try to handle non-agreegated /jointe_states appropriately (Rviz does, my control system does, so I think the low-level driver should too).

Furthermore, we were using the industrial robot simulator, which WAS taking in the gripper joints, so there was some scrambling once the real robot had a different node to handle that. Namespaces would have been a good solution, but I was trying to get around changing EVERY different launch file configuration we have for running our software.

IF you want to take out that commit from the PR, feel free.

[pbeeson]

On a related note, it's strange to me that the driver listens to joint states to begin with given that it's the same driver that publishes them. This data should be cached and used without having to add the overhead of//latency of listening to what it just published. But I couldn't figure out the flow given the limited time I had to look at this.

[gavanderhoorn]

I think we're actually running into the channel vs bus difference here. In a bus system, all nodes are responsible for filtering for the msgs they care about (content or type). In a channel system, nodes are only connected to data flows that they have already expressed an interest in, and no filtering is needed any more.

TF is essentially a bus on top of channels (ie: get all msgs, need to filter / build-up a coherent picture of the state yourself). What you are describing is essentially that: JointStates are published to a single topic, making it essentially a bus, then interested nodes have to filter to get the info they are interested in.

I'm not saying one is necessarily better than the other, just that my preference has been to use remapping and isolation to avoid having to do that.

[pbeeson]

Good analogy. I've seen multiple large systems (some at NASA, some at other
large research institutions) that indeed treat joint-states like a bus.
Rviz assumes this, and eventually, after many issues in the past, I've
started writing all my nodes to assume this is the use case. The issue is
that you then take that for granted you start using it that way out of ease
of configuration and it can cause problems when other nodes don't. The nice
thing about treating joint-states like a bus in your nodes is that if
people use it via channels (namespaces) it still works.

[pbeeson]

I'd like to bump this PR as we are about to go into a production system using our fork of your io_support_rebased branch.

[gavanderhoorn]

Well I can accept your PR into my private fork, but the IO services should probably not be merged as they are implemented now. At the very least they should be in a separate file.

Do you just want to have this code accepted somewhere else than your fork, or would you rather it'd be merged upstream?

[pbeeson]

I assume that if/when IO services get refactored in order to make it into the main motoman repo, this will largely come from your repo. So, I guess for now, just putting this PR into your private repo is good enough. I just want to make sure that changes we've needed to make for IO on our end don't get lost when this eventually does get ported to the main repo.

[pbeeson]

The latest changes move a lock that was around a 250ms sleep when in idle that was affecting the performance of I/O speeds.

[gavanderhoorn]

Yes, that is an unfortunate result of the fact that the IO msgs are being multiplexed into the trajectory datastream. All of that is not by design, but by necessity at the moment.

The READ_SINGLE_IO and WRITE_SINGLE_IO as they are right now in motoman_driver were really not meant for use in production contexts, but added as a convenience to toggle an IO element now and then. That was also one of the reasons why this hasn't been merged upstream: the design is not OK.

We'll have to revisit how to integrate this more cleanly, or remove it and promote something else.