Trajectory command sometimes returns success despite not getting to the requested goal

[heuristicus]

I commented about this at https://support.bostondynamics.com/s/question/0D54X00006hjR61SAE/replicating-controller-point-motion-with-sdk-commands. If there is an obstacle in front of the robot, sometimes the trajectory command will return basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL when it is not actually at the goal. Currently the actionserver that we have returns success in this case, although actually we did not get to the goal that was requested.

In my own code I'm currently handling this by comparing the position of the robot to the expected end position, but I think it may be a good idea to have this check in the driver itself. We could do this by storing the position of the robot in the odom frame when the goal is received, then once the trajectory command returns, compare the new odom value against the old odom plus the goal in the body frame. Some kind of goal tolerance would be needed.