Support executing multiple plans in IiwaPlanManager

[pangtao22]

The new lcm type lcmt_our_robot_plan (#15) allows sending multiple plans to the robot in one message, which is indeed a valid use case. For example, in the labs of 6.881, students need to run multiple JointSpaceTrajectoryPlans generated by a sequence of IK solves. In one of my own projects, I usually prepend a more interesting Plan with a JointSpaceTrajectoryPlan to make sure that the more interesting Plan always starts at the same initial configuration.

The previous solution to executing consecutive plans is to queue them up in the client, send one to the server (IiwaPlanManager), wait for the server to reply a success token, and then send the next plan. This introduces an indeterministic time interval between the end of one plan and the beginning of the next. The indeterminacy is partly due to network communication, and partly due to the inaccurate std::this_thread::sleep_for() function currently used to enforce the update frequency of the plan-handling thread.

Among its many other side effects, the indeterminacy makes it more cumbersome to compute the mean and variance of executing the same plan for 50 times, as each Plan starts at a slightly different time.

On the other hand, queuing the Plans up in IiwaPlanManager eliminates this indeterminacy, as plan switching is timed by IIWA_STATUS messages, which we believe is a more accurate clock signal.

The proposed state machine behavior for executing multiple plans, which is consistent with the current plan-switching logic, is as follows:

• No transition to state IDLE is made between Plans. In other words, the state machine is in state RUNNING from receiving the list of Plans to the end of the last Plan in the list or when an error occurs, whichever happens earlier.
• If all Plans are successfully executed, a successful token is sent to the client after all Plans are completed.
• If an error occurs during execution, i.e. the state machine enters state ERROR, an unsuccessful is sent to the client, together with the index of the Plan where error occurred.
• Communicating plan execution status back to the client can be done with another custom lcm type such as lcmt_our_plan_status.

We can discuss this in the weekly meeting on Thursday.

[pangtao22]

@weigao95 mentioned that the pointer to Plan retrieved here can be invalidated when the Queue that holds the Plan reaches its capacity and re-allocates memory, which can happen if a new plan is queued while the current plan is executed. Although I don't think this can happen under the current finite state machine logic, it is important to keep this failure mode in mind when we change the logic, which will probably happen as we resolve #21 .