Configurable Asynchronous Planning/Execution

[egordon]

Currently, the moving/planning/executing API is a bit clunky.

• execute() directly asynchronously (without the ability to cancel) calls a controller's FindJointTrajectory Action
• plan(): based on a boolean passed to __init__(), either synchronously calls the MoveGroup Action plan-only or one of the MoveIt2 planning services (/plan_kinematic_path or /compute_cartesian_path)
• move_to_configuration() and move_to_pose() are utility functions that either asynchronously (without the ability to cancel) call the MoveGroup Action directly or concatenate the above two functions.
• set_joint_goal() and set_pose_goal() (which is itself a simple concat of set_position_goal() and set_orientation_goal()) are used as public helper functions to set the goal set that are overridden by the arguments of the above functions

These should be updated with the following principles:

1. All service/action calls should be asynchronous by default, see how to do this with Actions and Services, these can be made synchronous client-side by just waiting on the future.
2. All asynchronous functions should take in a feedback callback parameter and return a single future.
3. Assume all MoveIt2 Services and Actions are available, and use them in favor of non-MoveIt2 interfaces.

Proposed Changes:

• Define execute(self, joint_trajectory: RobotTrajectory, feedback_callback: Optional[Fn], controller_action: Optional[str]): returning a future returning the Action Response. This function will by default call the /execute_trajectory MoveIt2 action server, which should handle controller switching automatically. If the controller action is specified, call the FollowJointTrajectory action directly. Cancellation requests to the future should be forwarded to the action server. Feedback from the action server should be forwarded to the provided
• Define plan(start_joint_state: Optional[Union[JointState, List[float]]] = None, cartesian: bool = False) that returns a future returning the Service response (either GetMotionPlan or GetCartesianPath). Remove all other arguments, as these should be specified by functions like set_joint_goal(), set_pose_goal(), and clear_goal_constraints().
• move_to_configuration() and move_to_pose() should only use the /move_action Action Server asynchronously and the constraints set in other functions. They should take a feedback callback and return a single future returning the Action Response.
• Ensure only a single execution future (move or execute) is active at once. Add cancel_execution() allowing any thread to cancel the currently running execution future.

[egordon]

Solved to sufficiency in egordon/async