Restructuring error reporting in start_traj_mode and start_point_queue_mode

[jimmy-mcelwain]

Changes start_traj_mode and start_point_queue_mode services so that they properly report the reasons for failure in case motion cannot be started. This addresses many of the concerns brought up in the discussion of PR #265. I closed that PR and opened this to simplify bringing it up to date with main and to make the history less confusing.

[jimmy-mcelwain]

Also in reference to #37 and #192

[gavanderhoorn]

@jimmy-mcelwain: could you perhaps describe your approach at a high-level?

You write:

Changes start_traj_mode and start_point_queue_mode services so that they properly report the reasons for failure in case motion cannot be started.

what did they not do correctly before these changes, and which changes (to control flow perhaps) address that specifically?

[jimmy-mcelwain]

What I wrote there doesn't describe what I changed since the last PR that I closed. I rebased on main and that comment sums up the purpose of this PR as a whole, not just the differences from the last PR.

This approach changes Ros_Controller_GetNotReadySubcode() to work better with start_traj_mode and start_point_queue_mode such that much less code is duplicated. Within Ros_Controller_GetNotReadySubcode() I check for intractable problems that require the user to take some action, e.g. like switching to remote mode or calling /reset_error. If I am attempting to start some motion mode, I will ignore tractable errors from Ros_Controller_GetNotReadySubcode() that are potentially addressed by starting the motion, e.g. turning on servos, starting the job.

I also significantly simplified the control flow of the program. Previously, there were several calls to goto updatestatus which are no longer needed. The jumps to updatestatus always followed some intractable issue. These goto calls didn't make sense to me, since they are checking if motion is ready almost immediately after they already determined that motion was not ready. I should have gotten rid of them in the previous PR. To me it makes a lot more sense to simply return with a MotionNotReadyCode if it is determined that the motion cannot start and nothing can be done about it.

[ted-miller]

testing worked well. Conflicts resolved using web editor