Publish FSU/PFL speed/power limit activation status

[gavanderhoorn]

ROS applications do not currently have access to any information about the state of FSU power and/or velocity limits, nor can they tell when an FSU enforced limit has altered an executed trajectory (either in space, or in time).

This can lead to unexpected behaviour of the robot, as well as on the ROS side.

(MotoROS2 will do a final check at the last trajectory point to see whether position tolerances specified in the trajectory goal are met, and return an error if that's not the case. While better than nothing, this is insufficient and mostly for humans. It also does not offer any information as to a cause)

We should find a way to make FSU and/or PFL status visible to ROS applications (MoveIt fi), such that appropriate action can be taken by them (ie: replan instead of abort, or reduce maximum velocity and/or acceleration limits).

[gavanderhoorn]

This has come up in the context of MotoROS1 as well, see ros-industrial/motoman#247 and ros-industrial/motoman#503 fi.

Enhancement requests asking to publish PFL status in some way also exist: ros-industrial/motoman#399.

As @EricMarcil explained on the MotoROS1 fora and in issues, a complicating factor is the lack of APIs on the M+ side to interface with the FSU.

An open PR (ros-industrial/motoman#542) attempts to derive whether the FSU is limiting the robot by comparing commanded and queued/submitted increments. This could work, but requires changes to the inner motion control loop of MotoROS2 and is essentially a work-around for missing APIs.

[ted-miller]

Now that #157 has been merged, we have an indicator of when the FSU is limiting speed.

I'm also now envisioning a state where the robot could be completely halted by the FSU. This type of state would make a status signal even more important.

[ted-miller]

@gavanderhoorn. I know other robot manufacturers have something similar to the FSU. Is this a common theme/issue? Does it make sense to modify the industrial_msgs/msg/RobotStatus format?

I can picture two additional indicators, robot is being limited and current trajectory has been limited. Do we need to try to quantify anything?