Update position based on the new frame

Waits for function evaluation to be true. Exits cleanly from ros. Introduces sleep delay, not recommended for time critical operations

* Implementation of method described in (http://stackoverflow.com/a/27410865) * See paper at (http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf)

【Body_to_ENU】 机体系移动。

【Body_to_ENU】 机体系移动。

Calculate the cross product of two vectors

* @brief DistanceToLine * Math taken from http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html * x = x0 * point_on_line =

////////////////////////////////////////////

! * Calculate the coefficients that define a trajectory. This solves, in closed form * the optimal trajectory for some given set of goal constraints

Return the execution time

Get the parameters defining the trajectory

Get the parameters defining the trajectory

Two simple helper function to make testing easier

Two simple helper function to make testing easier

! * Calculate the maximum jerk squared along the trajectory. This is made easy * because the jerk is quadratic, so we need to evaluate at most three

! * Calculate the extrema of the acceleration trajectory. This is made relatively * easy by the fact that the acceleration is a cubic polynomial, so