driver: assertion in joint_relay_handler when controller has < 4 motion groups configured

[gavanderhoorn]

As reported in #89: the code hits the following assertion quickly after connecting to a controller:

[FATAL] [1463490740.608713916]: ASSERTION FAILED
    file = /path/to/src/motoman/motoman_driver/src/industrial_robot_client/joint_relay_handler.cpp
    line = 322
    cond = all_joint_state.positions.size() == all_joint_names.size()

Observed on a system with an FS100 with two motion groups:

1. SIA20
2. linear track

motoman_driver based on the code in #89.

[jettan]

Alright, so here's what is happening under the hood:
Suppose that we have 2 motion groups. The current driver processes the JointFeedbackMessage from the simple messages in reverse order (meaning group 3 -> 2 -> 1 ->0 if we have 4 groups).
But if we only have 2 groups, simple_message/joint_feedback_ex.cpp:120 will only try to get the first two JointFeedbackMessage, which will result in processing the two invalid groups (since simple messages are of a fixed length).

Therefore, convert_message(..) in JointFeedbackExRelayHandler will get a lot of trash as data to process eventually leading to invalid all_joint_states being created after the convert_messages(..), resulting in the assertion error reported by @gavanderhoorn

[gavanderhoorn]

Therefore, the create_messages [..] will get a lot of trash as data to process eventually leading to invalid all_joint_states being created after the convert_messages

Not really 'trash', an all-zeros JOINT_FEEDBACK message (which is essentially a valid message).

That causes all bits in valid_fields to be zero, leading to getPositions(..) (and similar methods) to return false, which in turn leads to an empty JointState message.

The currently implemented work-around makes JointFeedbackEx deserialise all JointFeedback messages, but only add them to the joint_feedback_messages_ vector if they contain valid data.

[shaun-edwards]

@gavanderhoorn, is the work-around in a PR?

[jettan]

@shaun-edwards: The workaround is not in a PR, but a working version can be found on my fork. I just need to revert the whitespace changes so everybody can see what has been changed :)

[gavanderhoorn]

No there is no associated PR.

As @jettan commented, we have a version that works, but we should probably take a look at cleaning up some of the work-arounds so that we can consider them proper fixes.

Apart from the deserialisation issue, there is also a problem with the code that matches ROS joint names to simple message indices and vice versa. I'll report that in a separate issue.

@jettan and another colleague have fixed that as well, but I thought it might make sense to re-use some of the code in the IRCv2 that @JeremyZoss wrote some time ago. IIRC, that was pretty concise and worked for arbitrary numbers of groups and joints per group (at least in the application(s) that we tested).

[andreaskoepf]

Somebody probably missed that SimpleMessage undload() functions extract from the back of the buffer, e.g. write forward, read backward, as done in the standard messages, for example JointFeedback::load() and JointFeedback::unload() -> accelerations_ is written last but read first.

Arrays therefore also need to be unloaded in reverse order. Since simple messages have a fixed size unused elements at the end of an array have to be ignored before the actual interesting values can be read.

Multiple fixes are available, e.g. A or B.

(Side note: ByteArray since 2015 uses a deque and the whole reverse decoding is no longer important to save memcopy calls - e.g. unloadFront() could be used everywhere if it existed for the standard messages...)

[gavanderhoorn]

Yes, thanks for the pointers. The fix in your A is actually older, and was made by @jettan.

[andreaskoepf]

(@gavanderhoorn I would be fine with the solution by @jettan (as it is already a PR). In my quickpatch I preserved the order of groups (and it is shorter) but both is not important since the the joint-state messages contain getRobotID() which is the groupNo on the MotoPlus side. BTW it would have been a bit clearer if groups_number_ had been called group_count).

[gavanderhoorn]

I completely agree with you (#42).

We could definitely see which patches make the most sense to integrate. I'm only interested in getting this fixed.