[Navigation][merge after #554 #555] trajectory generation based on optimization methods (e.g. minimum snap trajectory)

[tongtybj]

What is this

Enable to generate and follow a smooth trajectory (polynomial) based on the optimization methods.

Details

• optmization methods are either minimum snap or minimum jerk trajectories. Minimum snap one is proposed by https://ieeexplore.ieee.org/document/5980409
• the code is based on https://github.com/uzh-rpg/dodgedrone_simulation/tree/main/dodgelib/src/reference/trajectory_reference.
• The refine codes can be found in https://github.com/tongtybj/aerial_robot/tree/develop/navigation/trajectory_generation/aerial_robot_control/src/trajectory
• minimum jerk trajectory has better performance than minimum snap trajectory for our robots. https://github.com/tongtybj/aerial_robot/blob/develop/navigation/trajectory_generation/aerial_robot_control/src/flight_navigation.cpp#L1010
• Interpolation points are implemented, but are not supported in ROS system yet.

Usage

• Use "2D Nav Goal" plugin in Rviz

https://github.com/jsk-ros-pkg/jsk_aerial_robot/assets/3666095/dbc359eb-ae0e-4289-b5b5-d9e3835a0c49

• Send to /<robot_name>/target_pose topic with geometry_msgs/PoseStamped. If x, y, z, w in orientation are all zero, yaw is static

https://github.com/jsk-ros-pkg/jsk_aerial_robot/assets/3666095/d98ac536-a4e7-4dea-871b-e6a159c5597b

[tongtybj]

TODO

• [ ] use actionlib
• [ ] support trajectory generation for orientation/quaternion
• [ ] support interpolation points