Fixes to work on real hardware

[brianhou]

Issues:

• RosJointStateClient::jointStateCallback was incorrectly handling empty buffers. Messages were being ignored because the timestamp was being read from the last record in an empty buffer.

• For some reason, RosJointStateClient::spin doesn't seem to be waiting long enough for callbacks to be placed on its queue. I've temporarily added a duration of 1 second here.

• The future returned by RosTrajectoryExecutor::execute never seems to be fulfilled. Calling future::wait waits forever (even after the trajectory seems to be successfully executed). I have a temporary hack in pantry_loading that sleeps for the duration of the trajectory instead of calling future::wait, but that needs to be fixed.

  It seems that RosTrajectoryExecutor::transitionCallback is sometimes not being called at all. On other trajectories, it is being called and isSuccessful is true (the resulting message is "Execution failed", which seems like a typo?) and the promise's value seems to be set. In both cases, the future still doesn't seem to be fulfilled. I thought that calling future::wait would return once a value is set by promise::set_value, but I could be misunderstanding.

  When RosTrajectoryExecutor::transitionCallback is being called, it also seems that it is called with handle.getCommState() == DONE well before the trajectory actually terminates (basically immediately). Is this expected behavior?

• Calling future::wait on the future returned by RosPositionCommandExecutor results in the error message: Exception thrown by callback: Promise already satisfied.

  This was because I forgot to start right_hand_controller 😓 Now the communication state is stuck at ACTIVE forever...

[mkoval]

For some reason, RosJointStateClient::spin doesn't seem to be waiting long enough for callbacks to be placed on its queue. I've temporarily added a duration of 1 second here.

You are intended to call spin() regularly, e.g. at a fixed frequency in a background thread. There is no reason to think that calling spin() once after executing a trajectory will work correctly. Adding a timeout to callAvailable() is entirely the wrong solution. 😉

The future returned by RosTrajectoryExecutor::execute never seems to be fulfilled. Calling future::wait waits forever (even after the trajectory seems to be successfully executed).
[...]
It seems that RosTrajectoryExecutor::transitionCallback is sometimes not being called at all.

The root issue here is the same as above: there is no background thread periodically calling spin(). Or, if there is a thread, the thread hung/crashed and is no longer running.

On other trajectories, it is being called and isSuccessful is true (the resulting message is "Execution failed", which seems like a typo?) and the promise's value seems to be set.

The logic in transitionCallback is actually correct. An FollowJointTrajectory can be flagged as a failure in two different ways:

1. at the actionlib level because the goal was recalled, rejected, preempted, aborted, or lost (indicated by GoalHandle::getTerminalState())
2. at the FollowJointTrajectoryResult level because of a logical error, e.g. an unknown joint was specified or the robot deviated too far from the path (indicated by FollowJointTrajectoryResult::error_code.

We only want to report success if getTerminalState() is SUCCEEDED and error_code is SUCCESSFUL. All of the rest of the code in transitionCallback exists to munge all of the error flags (getTerminalState(), error_code, and error_string) into one human-readable message.

I thought that calling future::wait would return once a value is set by promise::set_value, but I could be misunderstanding.

That is correct. We are using std::future, which is part of the standard library, so I would be surprised if the error resides here. Are you sure set_result or set_exception is being called on promise associated with the future that you expect to return?

It is very possible there are bugs in RosTrajectoryExecutor, but I would be surprised if they are this fundamental. I successfully used the class that RosTrajectoryExecutor was based on successfully on Valkyrie without any of these issues.

[brianhou]

Yep, that was definitely it. 😅 Fixed in https://github.com/personalrobotics/pantry-loading/pull/12/commits/3738173c75af9dca135264b7765b7a697e84e870.

Also, it seems that the hand issues stem from a lower level -- trying to just use libbarrett_ros without aikido doesn't successfully move the hand to a preshape either. I'm currently thinking there might be an issue in https://github.com/personalrobotics/pr_ros_controllers/tree/master/position_command_controller or something? Still need to look into this more.

The logic in transitionCallback is actually correct. An FollowJointTrajectory can be flagged as a failure in two different ways...

I think I mostly understand this method, although

aikido/src/control/ros/RosTrajectoryExecutor.cpp

Line 178 in 427f7be

message << "Execution failed.";

still confuses me a bit. It seems that the message is set to Execution failed if terminalState == TerminalState::SUCCEEDED without considering the FollowJointTrajectoryResult. Would it make more sense to remove lines 176-179 and add the << "Execution failed." to line 186? Never mind!

[brianhou]

I think these changes are uncontroversial, so I'm merging this now.