Add robot.wait_command()

[hello-binit]

After sending move_to() or move_by() commands to Stretch's joints, there are currently two ways to wait to block program flow until the commands are completed:

1. Call the wait_until_at_setpoint() method belonging to the motor for which the motion was commanded. For example:

   import time
   import stretch_body.robot
   r = stretch_body.robot.Robot()
   r.startup()
   r.end_of_arm.move_to('wrist_roll', 1.57, 0.1)
   time.sleep(0.5)
   r.end_of_arm.get_joint('wrist_roll').wait_until_at_setpoint()

   This requires the user to fetch the motor instance for the joint that was commanded (r.end_of_arm.get_joint('wrist_roll')). This approach also requires the user to sleep for a small amount of time before calling wait_until_at_setpoint() because the Python threads need time to perform the USB communication that fetches the latest status and update the robot's status dictionary.
2. Sleep for some amount of time. This approach is easier as you don't need to query a specific joint. But this approach doesn't know whether the joint finished the commanded motion. It might block program execution longer than is necessary, or not long enough.

This PR introduces wait_command(timeout=15.0) to the robot class. The method blocks program execution until all robot motion is complete. Since users can call robot.wait_command(), they don't need to query specific joint(s) to find out if the motion completed. wait_command() takes in an argument called timeout determining how long the method waits for motion to complete. wait_command() returns a boolean, where True means all motion completed, and False means the motion is ongoing and timeout elapsed.

import stretch_body.robot

r = stretch_body.robot.Robot()
r.startup()
assert(r.is_calibrated())

r.arm.move_to(0.5)
r.lift.move_to(0.5)
r.end_of_arm.move_to('wrist_yaw', 0.0)
r.push_command()
motion_done = r.wait_command()
print(f'All commanded motions completed in 15 seconds: {motion_done}')

r.stop()

Testing

This method hasn't been tested with the waypoint trajectory or the velocity interfaces. This method relies on Python threads to update status dictionaries and Python is known to starve threads to prioritize computation in the main process. Depending on the computation running alongside the Robot class, you may see unexpected behavior from wait_command().

This PR has been tested on a RE2 running Ubuntu 22.04 (Python 3.10), and P3 firmware.

---- Checking Software ----
[Pass] Ubuntu 22.04 is ready
[Pass] Firmware is up-to-date
         hello-pimu = v0.5.1p3
         hello-wacc = v0.5.1p3
         hello-motor-arm = v0.5.1p3
         hello-motor-lift = v0.5.1p3
         hello-motor-left-wheel = v0.5.1p3
         hello-motor-right-wheel = v0.5.1p3
[Pass] Python pkgs are up-to-date
         Stretch Body = 0.6.5 (installed locally at ~/repos/stretch_body/body)
         Stretch Body Tools = 0.6.0 (installed locally at ~/repos/stretch_body/tools)
         Stretch Tool Share = 0.2.8
         Stretch Factory = 0.4.12
         Stretch Diagnostics = 0.0.14