recovery

src/飞控/command_to_mavros.h

@@ -0,0 +1,624 @@
+/***************************************************************************************************************************
+* command_to_mavros.h
+*
+* Author: Qyp
+*
+* Update Time: 2020.08.12
+*
+* Introduction:  Drone control command send class using Mavros package
+*         1. Ref to the Mavros plugins (setpoint_raw, loca_position, imu and etc..)
+*         2. Ref to the Offboard Flight task in PX4 code: https://github.com/PX4/Firmware/blob/master/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp
+*         3. Ref to the Mavlink module in PX4 code: https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp
+*         4. Ref to the position control module in PX4: https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control
+*         5. Ref to the attitude control module in PX4: https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control
+*         6. 还需要考虑复合形式的输出情况
+* 主要功能：
+*    本库函数主要用于连接prometheus_control与mavros两个功能包。简单来讲，本代码提供飞控的状态量，用于控制或者监控，本代码接受控制指令，并将其发送至飞控。
+* 1、发布prometheus_control功能包生成的控制量至mavros功能包，可发送期望位置、速度、角度、角速度、底层控制等。
+* 2、订阅mavros功能包发布的飞控状态量（包括PX4中的期望位置、速度、角度、角速度、底层控制），用于检查飞控是否正确接收机载电脑的指令
+* 3、解锁上锁、修改模式两个服务。
+
+***************************************************************************************************************************/
+#ifndef COMMAND_TO_MAVROS_H
+#define COMMAND_TO_MAVROS_H
+
+#include <ros/ros.h>
+#include <math_utils.h>
+#include <mavros_msgs/CommandBool.h>
+#include <mavros_msgs/SetMode.h>
+#include <mavros_msgs/State.h>
+#include <mavros_msgs/AttitudeTarget.h>
+#include <mavros_msgs/PositionTarget.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <mavros_msgs/ActuatorControl.h>
+#include <mavros_msgs/MountControl.h>
+#include <sensor_msgs/Imu.h>
+#include <prometheus_msgs/DroneState.h>
+#include <bitset>
+#include <prometheus_msgs/AttitudeReference.h>
+#include <prometheus_msgs/DroneState.h>
+using namespace std;
+
+class command_to_mavros
+{
+    public:
+    //constructed function 1
+    command_to_mavros(void):
+        command_nh("~")
+    {
+        command_nh.param<string>("uav_name", uav_name, "/uav0");
+
+        if (uav_name == "/uav0")
+        {
+            uav_name = "";
+        }
+        
+        pos_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
+        vel_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
+        accel_drone_fcu_target  = Eigen::Vector3d(0.0,0.0,0.0);
+        q_fcu_target            = Eigen::Quaterniond(0.0,0.0,0.0,0.0);
+        euler_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
+        rates_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
+        Thrust_target           = 0.0;
+
+        // 【订阅】无人机期望位置/速度/加速度 坐标系:ENU系
+        //  本话题来自飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp读取), 对应Mavlink消息为POSITION_TARGET_LOCAL_NED, 对应的飞控中的uORB消息为vehicle_local_position_setpoint.msg
+        position_target_sub = command_nh.subscribe<mavros_msgs::PositionTarget>(uav_name + "/mavros/setpoint_raw/target_local", 10, &command_to_mavros::pos_target_cb,this);
+
+        // 【订阅】无人机期望角度/角速度 坐标系:ENU系
+        //  本话题来自飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp读取), 对应Mavlink消息为ATTITUDE_TARGET (#83), 对应的飞控中的uORB消息为vehicle_attitude_setpoint.msg
+        attitude_target_sub = command_nh.subscribe<mavros_msgs::AttitudeTarget>(uav_name + "/mavros/setpoint_raw/target_attitude", 10, &command_to_mavros::att_target_cb,this);
+
+        // 【订阅】无人机底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量 及 油门推力
+        //  本话题来自飞控(通过Mavros功能包 /plugins/actuator_control.cpp读取), 对应Mavlink消息为ACTUATOR_CONTROL_TARGET, 对应的飞控中的uORB消息为actuator_controls.msg
+        actuator_target_sub = command_nh.subscribe<mavros_msgs::ActuatorControl>(uav_name + "/mavros/target_actuator_control", 10, &command_to_mavros::actuator_target_cb,this);
+
+        // 【发布】位置/速度/加速度期望值 坐标系 ENU系
+        //  本话题要发送至飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp发送), 对应Mavlink消息为SET_POSITION_TARGET_LOCAL_NED (#84), 对应的飞控中的uORB消息为position_setpoint_triplet.msg
+        setpoint_raw_local_pub = command_nh.advertise<mavros_msgs::PositionTarget>(uav_name + "/mavros/setpoint_raw/local", 10);
+
+        // 【发布】角度/角速度期望值 坐标系 ENU系
+        //  本话题要发送至飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp发送), 对应Mavlink消息为SET_ATTITUDE_TARGET (#82), 对应的飞控中的uORB消息为vehicle_attitude_setpoint.msg（角度） 或vehicle_rates_setpoint.msg（角速度）
+        setpoint_raw_attitude_pub = command_nh.advertise<mavros_msgs::AttitudeTarget>(uav_name + "/mavros/setpoint_raw/attitude", 10);
+
+        // 【发布】底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量 及 油门推力 注意 这里是NED系的！！
+        //  本话题要发送至飞控(通过Mavros功能包 /plugins/actuator_control.cpp发送), 对应Mavlink消息为SET_ACTUATOR_CONTROL_TARGET, 对应的飞控中的uORB消息为actuator_controls.msg
+        actuator_setpoint_pub = command_nh.advertise<mavros_msgs::ActuatorControl>(uav_name + "/mavros/actuator_control", 10);
+
+        //　本话题要发送至飞控(通过Mavros_extra功能包 /plugins/mount_control.cpp发送)
+        mount_control_pub = command_nh.advertise<mavros_msgs::MountControl>(uav_name + "/mavros/mount_control/command", 1);
+
+        // 【服务】解锁/上锁
+        //  本服务通过Mavros功能包 /plugins/command.cpp 实现
+        arming_client = command_nh.serviceClient<mavros_msgs::CommandBool>(uav_name + "/mavros/cmd/arming");
+
+        // 【服务】修改系统模式
+        //  本服务通过Mavros功能包 /plugins/command.cpp 实现
+        set_mode_client = command_nh.serviceClient<mavros_msgs::SetMode>(uav_name + "/mavros/set_mode");
+    }
+
+		//constructed function 2
+    command_to_mavros(string Uav_name):
+        command_nh("")
+    {
+        uav_name = Uav_name;
+        
+        pos_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
+        vel_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
+        accel_drone_fcu_target  = Eigen::Vector3d(0.0,0.0,0.0);
+        q_fcu_target            = Eigen::Quaterniond(0.0,0.0,0.0,0.0);
+        euler_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
+        rates_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
+        Thrust_target           = 0.0;
+
+        // 【订阅】无人机期望位置/速度/加速度 坐标系:ENU系
+        //  本话题来自飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp读取), 对应Mavlink消息为POSITION_TARGET_LOCAL_NED, 对应的飞控中的uORB消息为vehicle_local_position_setpoint.msg
+        position_target_sub = command_nh.subscribe<mavros_msgs::PositionTarget>(uav_name + "/mavros/setpoint_raw/target_local", 10, &command_to_mavros::pos_target_cb,this);
+
+        // 【订阅】无人机期望角度/角速度 坐标系:ENU系
+        //  本话题来自飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp读取), 对应Mavlink消息为ATTITUDE_TARGET (#83), 对应的飞控中的uORB消息为vehicle_attitude_setpoint.msg
+        attitude_target_sub = command_nh.subscribe<mavros_msgs::AttitudeTarget>(uav_name + "/mavros/setpoint_raw/target_attitude", 10, &command_to_mavros::att_target_cb,this);
+
+        // 【订阅】无人机底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量 及 油门推力
+        //  本话题来自飞控(通过Mavros功能包 /plugins/actuator_control.cpp读取), 对应Mavlink消息为ACTUATOR_CONTROL_TARGET, 对应的飞控中的uORB消息为actuator_controls.msg
+        actuator_target_sub = command_nh.subscribe<mavros_msgs::ActuatorControl>(uav_name + "/mavros/target_actuator_control", 10, &command_to_mavros::actuator_target_cb,this);
+
+        // 【发布】位置/速度/加速度期望值 坐标系 ENU系
+        //  本话题要发送至飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp发送), 对应Mavlink消息为SET_POSITION_TARGET_LOCAL_NED (#84), 对应的飞控中的uORB消息为position_setpoint_triplet.msg
+        setpoint_raw_local_pub = command_nh.advertise<mavros_msgs::PositionTarget>(uav_name + "/mavros/setpoint_raw/local", 10);
+
+        // 【发布】角度/角速度期望值 坐标系 ENU系
+        //  本话题要发送至飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp发送), 对应Mavlink消息为SET_ATTITUDE_TARGET (#82), 对应的飞控中的uORB消息为vehicle_attitude_setpoint.msg（角度） 或vehicle_rates_setpoint.msg（角速度）
+        setpoint_raw_attitude_pub = command_nh.advertise<mavros_msgs::AttitudeTarget>(uav_name + "/mavros/setpoint_raw/attitude", 10);
+
+        // 【发布】底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量 及 油门推力 注意 这里是NED系的！！
+        //  本话题要发送至飞控(通过Mavros功能包 /plugins/actuator_control.cpp发送), 对应Mavlink消息为SET_ACTUATOR_CONTROL_TARGET, 对应的飞控中的uORB消息为actuator_controls.msg
+        actuator_setpoint_pub = command_nh.advertise<mavros_msgs::ActuatorControl>(uav_name + "/mavros/actuator_control", 10);
+
+        //　本话题要发送至飞控(通过Mavros_extra功能包 /plugins/mount_control.cpp发送)
+        mount_control_pub = command_nh.advertise<mavros_msgs::MountControl>(uav_name + "/mavros/mount_control/command", 1);
+
+        // 【服务】解锁/上锁
+        //  本服务通过Mavros功能包 /plugins/command.cpp 实现
+        arming_client = command_nh.serviceClient<mavros_msgs::CommandBool>(uav_name + "/mavros/cmd/arming");
+
+        // 【服务】修改系统模式
+        //  本服务通过Mavros功能包 /plugins/command.cpp 实现
+        set_mode_client = command_nh.serviceClient<mavros_msgs::SetMode>(uav_name + "/mavros/set_mode");
+    }
+
+    string uav_name;
+    //Target pos of the drone [from fcu]
+    Eigen::Vector3d pos_drone_fcu_target;
+    //Target vel of the drone [from fcu]
+    Eigen::Vector3d vel_drone_fcu_target;
+    //Target accel of the drone [from fcu]
+    Eigen::Vector3d accel_drone_fcu_target;
+    //Target att of the drone [from fcu]
+    Eigen::Quaterniond q_fcu_target;
+    Eigen::Vector3d euler_fcu_target;
+    Eigen::Vector3d rates_fcu_target;
+    //Target thrust of the drone [from fcu]
+    float Thrust_target;
+    mavros_msgs::ActuatorControl actuator_target;
+
+    //变量声明 - 服务
+    mavros_msgs::SetMode mode_cmd;
+
+    mavros_msgs::CommandBool arm_cmd;
+
+    ros::ServiceClient arming_client;
+
+    ros::ServiceClient set_mode_client;
+
+    //Idle. Do nothing.
+    void idle();
+
+    //px4 takeoff
+    void takeoff();
+
+    //px4 loiter
+    void loiter();
+
+    // px4 land
+    void land();
+
+    //发送位置期望值至飞控（输入：期望xyz,期望yaw）
+    void send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp);
+
+    //发送速度期望值至飞控（输入：期望vxvyvz,期望yaw）
+    void send_vel_setpoint(const Eigen::Vector3d& vel_sp, float yaw_sp);
+
+    //发送速度期望值至飞控（输入：期望vxvy z,期望yaw）
+    void send_vel_xy_pos_z_setpoint(const Eigen::Vector3d& state_sp, float yaw_sp);
+    void send_vel_xy_pos_z_setpoint_yawrate(const Eigen::Vector3d& state_sp, float yaw_rate_sp);
+
+    //发送速度期望值至飞控（机体系）（输入：期望vxvyvz,期望yaw）
+    void send_vel_setpoint_body(const Eigen::Vector3d& vel_sp, float yaw_sp);
+
+    void send_vel_setpoint_yaw_rate(const Eigen::Vector3d& vel_sp, float yaw_rate_sp);
+
+    // 发送位置+速度期望值至飞控（机体系）（输入：期望xyz + vxvyvz,期望yaw）
+    void send_pos_vel_xyz_setpoint(const Eigen::Vector3d& pos_sp, const Eigen::Vector3d& vel_sp, float yaw_sp);
+
+    //发送加速度期望值至飞控（输入：期望axayaz,期望yaw）
+    void send_acc_xyz_setpoint(const Eigen::Vector3d& accel_sp, float yaw_sp);
+
+    //发送角度期望值至飞控（输入：期望角度-四元数,期望推力）
+    //这是px4_pos_controller.cpp中目前使用的控制方式
+    void send_attitude_setpoint(const prometheus_msgs::AttitudeReference& _AttitudeReference);
+
+    //发送角度期望值至飞控（输入：期望角速度,期望推力）
+    void send_attitude_rate_setpoint(const Eigen::Vector3d& attitude_rate_sp, float thrust_sp);
+
+    void send_attitude_setpoint_yawrate(const prometheus_msgs::AttitudeReference& _AttitudeReference, float yaw_rate_sp);
+
+    //发送底层至飞控（输入：MxMyMz,期望推力）[Not recommanded. Because the high delay between the onboard computer and Pixhawk]
+    void send_actuator_setpoint(const Eigen::Vector4d& actuator_sp);
+
+    //发送云台控制指令
+    void send_mount_control_command(const Eigen::Vector3d& gimbal_att_sp);
+    
+    private:
+
+        ros::NodeHandle command_nh;
+
+        ros::Subscriber position_target_sub;
+        ros::Subscriber attitude_target_sub;
+        ros::Subscriber actuator_target_sub;
+
+        ros::Publisher setpoint_raw_local_pub;
+        ros::Publisher setpoint_raw_attitude_pub;
+        ros::Publisher actuator_setpoint_pub;
+        ros::Publisher mount_control_pub;
+
+        void pos_target_cb(const mavros_msgs::PositionTarget::ConstPtr& msg)
+        {
+            pos_drone_fcu_target = Eigen::Vector3d(msg->position.x, msg->position.y, msg->position.z);
+
+            vel_drone_fcu_target = Eigen::Vector3d(msg->velocity.x, msg->velocity.y, msg->velocity.z);
+
+            accel_drone_fcu_target = Eigen::Vector3d(msg->acceleration_or_force.x, msg->acceleration_or_force.y, msg->acceleration_or_force.z);
+        }
+
+        void att_target_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg)
+        {
+            q_fcu_target = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
+
+            //Transform the Quaternion to euler Angles
+            euler_fcu_target = quaternion_to_euler(q_fcu_target);
+
+            rates_fcu_target = Eigen::Vector3d(msg->body_rate.x, msg->body_rate.y, msg->body_rate.z);
+
+            Thrust_target = msg->thrust;
+        }
+
+        void actuator_target_cb(const mavros_msgs::ActuatorControl::ConstPtr& msg)
+        {
+            actuator_target = *msg;
+        }
+
+
+};
+
+void command_to_mavros::send_mount_control_command(const Eigen::Vector3d& gimbal_att_sp)
+{
+  mavros_msgs::MountControl mount_setpoint;
+  //
+  mount_setpoint.mode = 2;
+  mount_setpoint.pitch = gimbal_att_sp[0]; // Gimbal Pitch
+  mount_setpoint.roll = gimbal_att_sp[1]; // Gimbal  Yaw
+  mount_setpoint.yaw = gimbal_att_sp[2]; // Gimbal  Yaw
+
+  mount_control_pub.publish(mount_setpoint);
+
+}
+
+void command_to_mavros::takeoff()
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    //飞控如何接收该信号请见mavlink_receiver.cpp
+    //飞控如何执行该指令请见FlightTaskOffboard.cpp
+    //需要在QGC设置起飞高度
+    pos_setpoint.type_mask = 0x1000;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+}
+
+void command_to_mavros::land()
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    //飞控如何接收该信号请见mavlink_receiver.cpp
+    //飞控如何执行该指令请见FlightTaskOffboard.cpp
+    //需要在QGC设置降落速度
+    pos_setpoint.type_mask = 0x2000;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+}
+
+void command_to_mavros::loiter()
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    //飞控如何接收该信号请见mavlink_receiver.cpp
+    //飞控如何执行该指令请见FlightTaskOffboard.cpp
+    pos_setpoint.type_mask = 0x3000;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+}
+
+void command_to_mavros::idle()
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    //飞控如何接收该信号请见mavlink_receiver.cpp
+    //飞控如何执行该指令请见FlightTaskOffboard.cpp
+    pos_setpoint.type_mask = 0x4000;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+}
+
+//发送位置期望值至飞控（输入：期望xyz,期望yaw）
+void command_to_mavros::send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp)
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+    //Bitmask toindicate which dimensions should be ignored (1 means ignore,0 means not ignore; Bit 10 must set to 0)
+    //Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
+    //Bit 10 should set to 0, means is not force sp
+    pos_setpoint.type_mask = 0b100111111000;  // 100 111 111 000  xyz + yaw
+
+    pos_setpoint.coordinate_frame = 1;
+
+    pos_setpoint.position.x = pos_sp[0];
+    pos_setpoint.position.y = pos_sp[1];
+    pos_setpoint.position.z = pos_sp[2];
+
+    pos_setpoint.yaw = yaw_sp;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+
+    // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Pos_target [X Y Z] : " << pos_drone_fcu_target[0] << " [ m ] "<< pos_drone_fcu_target[1]<<" [ m ] "<<pos_drone_fcu_target[2]<<" [ m ] "<<endl;
+    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
+}
+
+//发送速度期望值至飞控（输入：期望vxvyvz,期望yaw）
+void command_to_mavros::send_vel_setpoint(const Eigen::Vector3d& vel_sp, float yaw_sp)
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    pos_setpoint.type_mask = 0b100111000111;
+
+    pos_setpoint.coordinate_frame = 1;
+
+    pos_setpoint.velocity.x = vel_sp[0];
+    pos_setpoint.velocity.y = vel_sp[1];
+    pos_setpoint.velocity.z = vel_sp[2];
+
+    pos_setpoint.yaw = yaw_sp;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+    
+    // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Vel_target [X Y Z] : " << vel_drone_fcu_target[0] << " [m/s] "<< vel_drone_fcu_target[1]<<" [m/s] "<<vel_drone_fcu_target[2]<<" [m/s] "<<endl;
+    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
+}
+
+//发送速度期望值至飞控（输入：期望vxvyvz,期望yaw）
+void command_to_mavros::send_vel_setpoint_yaw_rate(const Eigen::Vector3d& vel_sp, float yaw_rate_sp)
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+    //Bitmask toindicate which dimensions should be ignored (1 means ignore,0 means not ignore; Bit 10 must set to 0)
+    //Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
+    //Bit 10 should set to 0, means is not force sp
+    pos_setpoint.type_mask = 0b010111000111;
+
+    pos_setpoint.coordinate_frame = 1;
+
+    pos_setpoint.velocity.x = vel_sp[0];
+    pos_setpoint.velocity.y = vel_sp[1];
+    pos_setpoint.velocity.z = vel_sp[2];
+
+    pos_setpoint.yaw_rate = yaw_rate_sp;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+    
+    // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Vel_target [X Y Z] : " << vel_drone_fcu_target[0] << " [m/s] "<< vel_drone_fcu_target[1]<<" [m/s] "<<vel_drone_fcu_target[2]<<" [m/s] "<<endl;
+    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
+}
+
+//发送速度期望值至飞控（机体系）（输入：期望vxvyvz,期望yaw）
+void command_to_mavros::send_vel_setpoint_body(const Eigen::Vector3d& vel_sp, float yaw_sp)
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    pos_setpoint.type_mask = 0b100111000111;
+
+    //uint8 FRAME_LOCAL_NED = 1
+    //uint8 FRAME_BODY_NED = 8
+    pos_setpoint.coordinate_frame = 8;
+
+    pos_setpoint.position.x = vel_sp[0];
+    pos_setpoint.position.y = vel_sp[1];
+    pos_setpoint.position.z = vel_sp[2];
+
+    pos_setpoint.yaw = yaw_sp;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+
+    // // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Vel_target [X Y Z] : " << vel_drone_fcu_target[0] << " [m/s] "<< vel_drone_fcu_target[1]<<" [m/s] "<<vel_drone_fcu_target[2]<<" [m/s] "<<endl;
+    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
+}
+
+void command_to_mavros::send_vel_xy_pos_z_setpoint(const Eigen::Vector3d& state_sp, float yaw_sp)
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    // 此处由于飞控暂不支持位置－速度追踪的复合模式，因此type_mask设定如下
+    pos_setpoint.type_mask = 0b100111000011;   // 100 111 000 011  vx vy vz z + yaw
+
+    pos_setpoint.coordinate_frame = 1;
+
+    pos_setpoint.velocity.x = state_sp[0];
+    pos_setpoint.velocity.y = state_sp[1];
+    pos_setpoint.velocity.z = 0.0;
+    pos_setpoint.position.z = state_sp[2];
+
+    pos_setpoint.yaw = yaw_sp;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+    
+    // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Vel_target [X Y Z] : " << vel_drone_fcu_target[0] << " [m/s] "<< vel_drone_fcu_target[1]<<" [m/s] "<<vel_drone_fcu_target[2]<<" [m/s] "<<endl;
+    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
+}
+void command_to_mavros::send_vel_xy_pos_z_setpoint_yawrate(const Eigen::Vector3d& state_sp, float yaw_rate_sp)
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    // 此处由于飞控暂不支持位置－速度追踪的复合模式，因此type_mask设定如下
+    pos_setpoint.type_mask = 0b010111000011;   // 100 111 000 011  vx vy vz z + yawrate
+
+    pos_setpoint.coordinate_frame = 1;
+
+    pos_setpoint.velocity.x = state_sp[0];
+    pos_setpoint.velocity.y = state_sp[1];
+    pos_setpoint.velocity.z = 0.0;
+    pos_setpoint.position.z = state_sp[2];
+
+    pos_setpoint.yaw_rate = yaw_rate_sp;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+    
+    // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Vel_target [X Y Z] : " << vel_drone_fcu_target[0] << " [m/s] "<< vel_drone_fcu_target[1]<<" [m/s] "<<vel_drone_fcu_target[2]<<" [m/s] "<<endl;
+    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
+}
+
+void command_to_mavros::send_pos_vel_xyz_setpoint(const Eigen::Vector3d& pos_sp, const Eigen::Vector3d& vel_sp, float yaw_sp)
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    // 速度作为前馈项， 参见FlightTaskOffboard.cpp
+    // 2. position setpoint + velocity setpoint (velocity used as feedforward)
+    // 控制方法请见 PositionControl.cpp
+    pos_setpoint.type_mask = 0b100111000000;   // 100 111 000 000  vx vy　vz x y z+ yaw
+
+    pos_setpoint.coordinate_frame = 1;
+
+    pos_setpoint.position.x = pos_sp[0];
+    pos_setpoint.position.y = pos_sp[1];
+    pos_setpoint.position.z = pos_sp[2];
+    pos_setpoint.velocity.x = vel_sp[0];
+    pos_setpoint.velocity.y = vel_sp[1];
+    pos_setpoint.velocity.z = vel_sp[2];
+
+    pos_setpoint.yaw = yaw_sp;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+}
+
+//发送加速度期望值至飞控（输入：期望axayaz,期望yaw）
+void command_to_mavros::send_acc_xyz_setpoint(const Eigen::Vector3d& accel_sp, float yaw_sp)
+{
+    mavros_msgs::PositionTarget pos_setpoint;
+
+    pos_setpoint.type_mask = 0b100000111111;
+
+    pos_setpoint.coordinate_frame = 1;
+
+    pos_setpoint.acceleration_or_force.x = accel_sp[0];
+    pos_setpoint.acceleration_or_force.y = accel_sp[1];
+    pos_setpoint.acceleration_or_force.z = accel_sp[2];
+
+    pos_setpoint.yaw = yaw_sp;
+
+    setpoint_raw_local_pub.publish(pos_setpoint);
+
+    // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Acc_target [X Y Z] : " << accel_drone_fcu_target[0] << " [m/s^2] "<< accel_drone_fcu_target[1]<<" [m/s^2] "<<accel_drone_fcu_target[2]<<" [m/s^2] "<<endl;
+    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
+
+}
+
+//发送角度期望值至飞控（输入：期望角度-四元数,期望推力）
+void command_to_mavros::send_attitude_setpoint(const prometheus_msgs::AttitudeReference& _AttitudeReference)
+{
+    mavros_msgs::AttitudeTarget att_setpoint;
+
+    //Mappings: If any of these bits are set, the corresponding input should be ignored:
+    //bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude
+
+    att_setpoint.type_mask = 0b00000111;
+
+    att_setpoint.orientation.x = _AttitudeReference.desired_att_q.x;
+    att_setpoint.orientation.y = _AttitudeReference.desired_att_q.y;
+    att_setpoint.orientation.z = _AttitudeReference.desired_att_q.z;
+    att_setpoint.orientation.w = _AttitudeReference.desired_att_q.w;
+
+    att_setpoint.thrust = _AttitudeReference.desired_throttle;
+
+    setpoint_raw_attitude_pub.publish(att_setpoint);
+
+    // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Att_target [R P Y] : " << euler_fcu_target[0] * 180/M_PI <<" [deg] "<<euler_fcu_target[1] * 180/M_PI << " [deg] "<< euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
+    // cout << "Thr_target [0 - 1] : " << Thrust_target <<endl;
+}
+
+
+//发送角度期望值至飞控（输入：期望角度-四元数,期望推力）
+void command_to_mavros::send_attitude_setpoint_yawrate(const prometheus_msgs::AttitudeReference& _AttitudeReference, float yaw_rate_sp)
+{
+    mavros_msgs::AttitudeTarget att_setpoint;
+
+    //Mappings: If any of these bits are set, the corresponding input should be ignored:
+    //bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude
+
+    att_setpoint.type_mask = 0b00000011;
+
+    att_setpoint.orientation.x = _AttitudeReference.desired_att_q.x;
+    att_setpoint.orientation.y = _AttitudeReference.desired_att_q.y;
+    att_setpoint.orientation.z = _AttitudeReference.desired_att_q.z;
+    att_setpoint.orientation.w = _AttitudeReference.desired_att_q.w;
+
+    att_setpoint.thrust = _AttitudeReference.desired_throttle;
+
+    att_setpoint.body_rate.x = 0.0;
+    att_setpoint.body_rate.y = 0.0;
+    att_setpoint.body_rate.z = yaw_rate_sp;
+
+    setpoint_raw_attitude_pub.publish(att_setpoint);
+
+    // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Att_target [R P Y] : " << euler_fcu_target[0] * 180/M_PI <<" [deg] "<<euler_fcu_target[1] * 180/M_PI << " [deg] "<< euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
+    // cout << "Thr_target [0 - 1] : " << Thrust_target <<endl;
+}
+
+//发送角度期望值至飞控（输入：期望角速度,期望推力）
+void command_to_mavros::send_attitude_rate_setpoint(const Eigen::Vector3d& attitude_rate_sp, float thrust_sp)
+{
+    mavros_msgs::AttitudeTarget att_setpoint;
+
+    //Mappings: If any of these bits are set, the corresponding input should be ignored:
+    //bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude
+    // msg.type_mask = 128;  // Ignore orientation messages
+    att_setpoint.type_mask = 0b10000000;
+
+    att_setpoint.body_rate.x = attitude_rate_sp[0];
+    att_setpoint.body_rate.y = attitude_rate_sp[1];
+    att_setpoint.body_rate.z = attitude_rate_sp[2];
+
+    att_setpoint.thrust = thrust_sp;
+
+    setpoint_raw_attitude_pub.publish(att_setpoint);
+
+    // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // cout << "Att_rate_target [R P Y] : " << rates_fcu_target[0] * 180/M_PI <<" [deg/s] "<<rates_fcu_target[1] * 180/M_PI << " [deg/s] "<< rates_fcu_target[2] * 180/M_PI<<" [deg/s] "<<endl;
+    // cout << "Thr_target [0 - 1] : " << Thrust_target <<endl;
+}
+
+//发送底层至飞控（输入：MxMyMz,期望推力）
+void command_to_mavros::send_actuator_setpoint(const Eigen::Vector4d& actuator_sp)
+{
+    mavros_msgs::ActuatorControl actuator_setpoint;
+
+    actuator_setpoint.group_mix = 0;
+    actuator_setpoint.controls[0] = actuator_sp(0);
+    actuator_setpoint.controls[1] = actuator_sp(1);
+    actuator_setpoint.controls[2] = actuator_sp(2);
+    actuator_setpoint.controls[3] = actuator_sp(3);
+    actuator_setpoint.controls[4] = 0.0;
+    actuator_setpoint.controls[5] = 0.0;
+    actuator_setpoint.controls[6] = 0.0;
+    actuator_setpoint.controls[7] = 0.0;
+
+    actuator_setpoint_pub.publish(actuator_setpoint);
+
+    // // 检查飞控是否收到控制量
+    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    // //ned to enu
+    // cout << "actuator_target [0 1 2 3] : " << actuator_target.controls[0] << " [ ] "<< -actuator_target.controls[1] <<" [ ] "<<-actuator_target.controls[2]<<" [ ] "<<actuator_target.controls[3] <<" [ ] "<<endl;
+
+    // cout << "actuator_target [4 5 6 7] : " << actuator_target.controls[4] << " [ ] "<< actuator_target.controls[5] <<" [ ] "<<actuator_target.controls[6]<<" [ ] "<<actuator_target.controls[7] <<" [ ] "<<endl;
+
+}
+
+
+#endif
+
+

src/飞控/controller_test.h

@@ -0,0 +1,290 @@
+/***************************************************************************************************************************
+* controller_test.h
+*
+* Author: Qyp
+*
+* Update Time: 2020.1.10
+*               Controller_Test类用于控制器测试，提供圆形、8字、阶跃响应三种参考输入
+*                   1、圆形需设置圆心、半径、线速度、旋转方向等参数
+*                   2、8字需设置圆心、半径、角速度等参数
+*                   3、阶跃响应需设置响应大小及响应间隔
+***************************************************************************************************************************/
+#ifndef CONTROLLER_TEST_H
+#define CONTROLLER_TEST_H
+
+#include <Eigen/Eigen>
+#include <math.h>
+#include <math_utils.h>
+#include <prometheus_control_utils.h>
+#include <prometheus_msgs/PositionReference.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Path.h>
+
+
+using namespace std;
+
+class Controller_Test
+{
+    public:
+        //构造函数
+        Controller_Test(void):
+            Controller_Test_nh("~")
+        {
+            Controller_Test_nh.param<float>("Controller_Test/Circle/Center_x", circle_center[0], 0.0);
+            Controller_Test_nh.param<float>("Controller_Test/Circle/Center_y", circle_center[1], 0.0);
+            Controller_Test_nh.param<float>("Controller_Test/Circle/Center_z", circle_center[2], 1.0);
+            Controller_Test_nh.param<float>("Controller_Test/Circle/circle_radius", circle_radius, 2.0);
+            Controller_Test_nh.param<float>("Controller_Test/Circle/linear_vel", linear_vel, 0.5);
+            Controller_Test_nh.param<float>("Controller_Test/Circle/direction", direction, 1.0);
+
+            Controller_Test_nh.param<float>("Controller_Test/Eight/Center_x", eight_origin_[0], 0.0);
+            Controller_Test_nh.param<float>("Controller_Test/Eight/Center_y", eight_origin_[1], 0.0);
+            Controller_Test_nh.param<float>("Controller_Test/Eight/Center_z", eight_origin_[2], 1.0);
+            Controller_Test_nh.param<float>("Controller_Test/Eight/omega", eight_omega_, 0.5);
+            Controller_Test_nh.param<float>("Controller_Test/Eight/radial", radial, 2.0);
+
+            Controller_Test_nh.param<float>("Controller_Test/Step/step_length", step_length, 0.0);
+            Controller_Test_nh.param<float>("Controller_Test/Step/step_interval", step_interval, 0.0);
+
+        }
+
+        //Printf the Controller_Test parameter
+        void printf_param();
+
+        //Controller_Test Calculation [Input: time_from_start; Output: Circle_trajectory;]
+        prometheus_msgs::PositionReference Circle_trajectory_generation(float time_from_start);
+
+        prometheus_msgs::PositionReference Eight_trajectory_generation(float time_from_start);
+
+        prometheus_msgs::PositionReference Step_trajectory_generation(float time_from_start);
+
+        prometheus_msgs::PositionReference Line_trajectory_generation(float time_from_start);
+
+    private:
+
+        ros::NodeHandle Controller_Test_nh;
+
+        // Circle Parameter
+        Eigen::Vector3f circle_center;
+        float circle_radius;
+        float linear_vel;
+        float direction;         //direction = 1 for CCW 逆时针, direction = -1 for CW 顺时针
+
+        // Eight Shape Parameter
+        Eigen::Vector3f eight_origin_;
+        float radial;
+        float eight_omega_;
+
+        // Step
+        float step_length;
+        float step_interval;
+        
+};
+
+
+prometheus_msgs::PositionReference Controller_Test::Circle_trajectory_generation(float time_from_start)
+{
+    prometheus_msgs::PositionReference Circle_trajectory;
+    float omega;
+    if( circle_radius != 0)
+    {
+        omega = direction * fabs(linear_vel / circle_radius);
+    }else
+    {
+        omega = 0.0;
+    }
+    const float angle = time_from_start * omega;
+    const float cos_angle = cos(angle);
+    const float sin_angle = sin(angle);
+
+    // cout << "omega : " << omega  * 180/M_PI <<" [deg/s] " <<endl;
+    // cout << "angle : " << angle  * 180/M_PI <<" [deg] " <<endl;
+
+    Circle_trajectory.header.stamp = ros::Time::now();
+
+    Circle_trajectory.time_from_start = time_from_start;
+
+    Circle_trajectory.Move_mode = prometheus_msgs::PositionReference::TRAJECTORY;
+
+    Circle_trajectory.position_ref[0] = circle_radius * cos_angle + circle_center[0];
+    Circle_trajectory.position_ref[1] = circle_radius * sin_angle + circle_center[1];
+    Circle_trajectory.position_ref[2] = circle_center[2];
+
+    Circle_trajectory.velocity_ref[0] = - circle_radius * omega * sin_angle;
+    Circle_trajectory.velocity_ref[1] = circle_radius * omega * cos_angle;
+    Circle_trajectory.velocity_ref[2] = 0;
+
+    Circle_trajectory.acceleration_ref[0] = - circle_radius * pow(omega, 2.0) * cos_angle;
+    Circle_trajectory.acceleration_ref[1] = - circle_radius * pow(omega, 2.0) * sin_angle;
+    Circle_trajectory.acceleration_ref[2] = 0;
+
+    // Circle_trajectory.jerk_ref[0] = circle_radius * pow(omega, 3.0) * sin_angle;
+    // Circle_trajectory.jerk_ref[1] = - circle_radius * pow(omega, 3.0) * cos_angle;
+    // Circle_trajectory.jerk_ref[2] = 0;
+
+    // Circle_trajectory.snap_ref[0] = circle_radius * pow(omega, 4.0) * cos_angle;
+    // Circle_trajectory.snap_ref[1] = circle_radius * pow(omega, 4.0) * sin_angle;
+    // Circle_trajectory.snap_ref[2] = 0;
+
+    Circle_trajectory.yaw_ref = 0;
+    // Circle_trajectory.yaw_rate_ref = 0;
+    // Circle_trajectory.yaw_acceleration_ref = 0;
+
+    return Circle_trajectory;
+}
+
+prometheus_msgs::PositionReference Controller_Test::Line_trajectory_generation(float time_from_start)
+{
+    prometheus_msgs::PositionReference Line_trajectory;
+    float omega;
+    if( circle_radius != 0)
+    {
+        omega = direction * fabs(linear_vel / circle_radius);
+    }else
+    {
+        omega = 0.0;
+    }
+    const float angle = time_from_start * omega;
+    const float cos_angle = cos(angle);
+    const float sin_angle = sin(angle);
+
+
+    Line_trajectory.header.stamp = ros::Time::now();
+
+    Line_trajectory.time_from_start = time_from_start;
+
+    Line_trajectory.Move_mode = prometheus_msgs::PositionReference::TRAJECTORY;
+
+    Line_trajectory.position_ref[0] = 0.0;
+    Line_trajectory.position_ref[1] = circle_radius * sin_angle + circle_center[1];
+    Line_trajectory.position_ref[2] = circle_center[2];
+
+    Line_trajectory.velocity_ref[0] = 0.0;
+    Line_trajectory.velocity_ref[1] = circle_radius * omega * cos_angle;
+    Line_trajectory.velocity_ref[2] = 0;
+
+    Line_trajectory.acceleration_ref[0] = 0.0;
+    Line_trajectory.acceleration_ref[1] = - circle_radius * pow(omega, 2.0) * sin_angle;
+    Line_trajectory.acceleration_ref[2] = 0;
+
+    // Line_trajectory.jerk_ref[0] = circle_radius * pow(omega, 3.0) * sin_angle;
+    // Line_trajectory.jerk_ref[1] = - circle_radius * pow(omega, 3.0) * cos_angle;
+    // Line_trajectory.jerk_ref[2] = 0;
+
+    // Line_trajectory.snap_ref[0] = circle_radius * pow(omega, 4.0) * cos_angle;
+    // Line_trajectory.snap_ref[1] = circle_radius * pow(omega, 4.0) * sin_angle;
+    // Line_trajectory.snap_ref[2] = 0;
+
+    Line_trajectory.yaw_ref = 0;
+    // Line_trajectory.yaw_rate_ref = 0;
+    // Line_trajectory.yaw_acceleration_ref = 0;
+
+    return Line_trajectory;
+}
+
+
+prometheus_msgs::PositionReference Controller_Test::Eight_trajectory_generation(float time_from_start)
+{
+    Eigen::Vector3f position;
+    Eigen::Vector3f velocity;
+    Eigen::Vector3f acceleration;
+    
+    float angle = eight_omega_* time_from_start;
+    const float cos_angle = cos(angle);
+    const float sin_angle = sin(angle);
+    
+    Eigen::Vector3f eight_radial_ ;
+    Eigen::Vector3f eight_axis_ ;
+    eight_radial_ << radial, 0.0, 0.0;
+    eight_axis_ << 0.0, 0.0, 2.0;
+
+    position = cos_angle * eight_radial_ + sin_angle * cos_angle * eight_axis_.cross(eight_radial_)
+                 + (1 - cos_angle) * eight_axis_.dot(eight_radial_) * eight_axis_ + eight_origin_;
+
+    velocity = eight_omega_ * (-sin_angle * eight_radial_ + (pow(cos_angle, 2) - pow(sin_angle, 2)) * eight_axis_.cross(eight_radial_)
+                 + (sin_angle) * eight_axis_.dot(eight_radial_) * eight_axis_);
+
+    acceleration << 0.0, 0.0, 0.0;
+
+    prometheus_msgs::PositionReference Eight_trajectory;
+
+    Eight_trajectory.header.stamp = ros::Time::now();
+
+    Eight_trajectory.time_from_start = time_from_start;
+
+    Eight_trajectory.Move_mode = prometheus_msgs::PositionReference::TRAJECTORY;
+
+    Eight_trajectory.position_ref[0] = position[0];
+    Eight_trajectory.position_ref[1] = position[1];
+    Eight_trajectory.position_ref[2] = position[2];
+
+    Eight_trajectory.velocity_ref[0] = velocity[0];
+    Eight_trajectory.velocity_ref[1] = velocity[1];
+    Eight_trajectory.velocity_ref[2] = velocity[2];
+
+    Eight_trajectory.acceleration_ref[0] = 0;
+    Eight_trajectory.acceleration_ref[1] = 0;
+    Eight_trajectory.acceleration_ref[2] = 0;
+
+    Eight_trajectory.yaw_ref = 0;
+
+    // to be continued...
+
+    return Eight_trajectory;
+}
+
+
+prometheus_msgs::PositionReference Controller_Test::Step_trajectory_generation(float time_from_start)
+{
+    prometheus_msgs::PositionReference Step_trajectory;
+
+    Step_trajectory.header.stamp = ros::Time::now();
+
+    Step_trajectory.time_from_start = time_from_start;
+
+    Step_trajectory.Move_mode = prometheus_msgs::PositionReference::TRAJECTORY;
+
+    int i = time_from_start / step_interval;
+
+    if( i%2 == 0)
+    {
+        Step_trajectory.position_ref[0] = step_length;
+    }else 
+    {
+        Step_trajectory.position_ref[0] = - step_length;
+    }
+
+    Step_trajectory.position_ref[1] = 0;
+    Step_trajectory.position_ref[2] = 1.0;
+
+    Step_trajectory.velocity_ref[0] = 0;
+    Step_trajectory.velocity_ref[1] = 0;
+    Step_trajectory.velocity_ref[2] = 0;
+
+    Step_trajectory.acceleration_ref[0] = 0;
+    Step_trajectory.acceleration_ref[1] = 0;
+    Step_trajectory.acceleration_ref[2] = 0;
+
+    Step_trajectory.yaw_ref = 0;
+
+    return Step_trajectory;
+}
+
+// 【打印参数函数】
+void Controller_Test::printf_param()
+{
+    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>Controller_Test Parameter <<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    cout <<"Circle Shape:  " <<endl;
+    cout <<"circle_center :  " << circle_center[0] <<" [m] "<< circle_center[1] <<" [m] "<< circle_center[2] <<" [m] "<<endl;
+    cout <<"circle_radius :  "<< circle_radius <<" [m] " <<"linear_vel : "<< linear_vel <<" [m/s] "<<"direction : "<< direction << endl;
+
+    cout <<"Eight Shape:  " <<endl;
+    cout <<"eight_origin_ :  "<< eight_origin_[0] <<" [m] "<< eight_origin_[1] <<" [m] "<< eight_origin_[2] <<" [m] "<<endl;
+    cout <<"eight_omega_ :  "<< eight_omega_  <<" [rad/s] " << "radial : "<< radial << endl;
+
+    cout <<"Step:  " <<endl;
+    cout <<"step_length :  "<< step_length << " [m] step_interval : "<< step_interval << " [s] "<<endl;
+}
+
+
+#endif

src/飞控/px4_sender.cpp

@@ -0,0 +1,701 @@
+/***************************************************************************************************************************
+* px4_sender.cpp
+*
+* Author: Qyp
+*
+* Update Time: 2020.10.18
+*
+* Introduction:  PX4 command sender using px4 default command
+*         1. Subscribe command.msg from upper nodes
+*         2. Subscribe drone_state msg from px4_pos_estimator.cpp
+*         2. Send command using command_to_mavros.h
+*         3. Command includes:  (1)idle
+*                               (2)takeoff
+*                               (3)land
+*                               (4)hold
+*                               (5)disarm
+*                               (6)move:
+*                                      1. xyz_pos + yaw in ENU/Body frame
+*                                      2. xyz_vel + yaw in ENU/Body frame
+*                                      3. xy_vel_z_pos + yaw in ENU/Body frame
+*                                      4. xyz_acc + yaw in ENU/Body frame
+*                                      5. trajectory 使用的是位置点控制
+***************************************************************************************************************************/
+
+#include <ros/ros.h>
+
+#include "state_from_mavros.h"
+#include "command_to_mavros.h"
+#include "prometheus_control_utils.h"
+#include "message_utils.h"
+#include "control_common.h"
+#include "printf_utils.h"
+#include "prometheus_station_utils.h"
+
+#define NODE_NAME "px4_sender"
+
+using namespace std;
+ //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+float cur_time;                                             //程序运行时间
+float Takeoff_height;                                       //默认起飞高度
+float Disarm_height;                                        //自动上锁高度
+float Land_speed;                                           //降落速度
+int Land_mode;                                              //降落策略选择
+bool quick_land = false;
+//Geigraphical fence 地理围栏
+Eigen::Vector2f geo_fence_x;
+Eigen::Vector2f geo_fence_y;
+Eigen::Vector2f geo_fence_z;
+
+Eigen::Vector3d Takeoff_position;                              // 起飞位置
+prometheus_msgs::DroneState _DroneState;                         //无人机状态量
+
+prometheus_msgs::ControlCommand Command_Now;                      //无人机当前执行命令
+prometheus_msgs::ControlCommand Command_Last;                     //无人机上一条执行命令
+
+Eigen::Vector3d state_sp(0,0,0);
+Eigen::Vector3d state_sp_extra(0,0,0);
+double yaw_sp;
+double yaw_rate_sp;
+
+prometheus_msgs::Message message;
+prometheus_msgs::LogMessageControl LogMessage;
+
+//RVIZ显示：期望位置
+geometry_msgs::PoseStamped ref_pose_rviz;
+float dt = 0;
+
+ros::Publisher rivz_ref_pose_pub;
+ros::Publisher message_pub;
+ros::Publisher log_message_pub;
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+void printf_param();
+int check_failsafe();
+geometry_msgs::PoseStamped get_rviz_ref_posistion(const prometheus_msgs::ControlCommand& cmd);
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+void Command_cb(const prometheus_msgs::ControlCommand::ConstPtr& msg)
+{
+    // CommandID必须递增才会被记录
+    if( msg->Command_ID  >  Command_Now.Command_ID )
+    {
+        Command_Now = *msg;
+    }else
+    {
+        pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Wrong Command ID.");
+    }
+    
+    // 无人机一旦接受到Disarm指令，则会屏蔽其他指令
+    if(Command_Last.Mode == prometheus_msgs::ControlCommand::Disarm)
+    {
+        Command_Now = Command_Last;
+    }
+}
+void station_command_cb(const prometheus_msgs::ControlCommand::ConstPtr& msg)
+{
+    Command_Now = *msg;
+    pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Get a command from Prometheus Station.");
+    
+    // 无人机一旦接受到Disarm指令，则会屏蔽其他指令
+    if(Command_Last.Mode == prometheus_msgs::ControlCommand::Disarm)
+    {
+        Command_Now = Command_Last;
+    }
+}
+void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
+{
+    _DroneState = *msg;
+
+    _DroneState.time_from_start = cur_time;
+}
+void timerCallback(const ros::TimerEvent& e)
+{
+    cout << GREEN <<">>>>>>>>>>>>>>>>>>>>>>>> PX4 SENDER  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << TAIL <<endl;
+    //固定的浮点显示
+    cout.setf(ios::fixed);
+    //setprecision(n) 设显示小数精度为n位
+    cout<<setprecision(2);
+    //左对齐
+    cout.setf(ios::left);
+    // 强制显示小数点
+    cout.setf(ios::showpoint);
+    // 强制显示符号
+    cout.setf(ios::showpos);
+
+    // 【打印】无人机状态,包括位置,速度等数据信息
+    prometheus_station_utils::prinft_drone_state(_DroneState);
+
+    // 【打印】来自上层的控制指令
+    prometheus_station_utils::printf_command_control(Command_Now);
+}
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "px4_sender");
+    ros::NodeHandle nh("~");
+
+    //【订阅】指令
+    // 本话题为任务模块生成的控制指令
+    ros::Subscriber Command_sub = nh.subscribe<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10, Command_cb);
+
+    //【订阅】指令
+    // 本话题为Prometheus地面站发送的控制指令
+    ros::Subscriber station_command_sub = nh.subscribe<prometheus_msgs::ControlCommand>("/prometheus/control_command_station", 10, station_command_cb);
+    
+    //【订阅】无人机状态
+    // 本话题来自px4_pos_estimator.cpp
+    ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
+
+    //【发布】参考位姿 RVIZ显示用
+    rivz_ref_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/prometheus/control/ref_pose_rviz", 10);
+
+    // 【发布】用于地面站显示的提示消息
+    message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
+
+    // 【发布】用于log的消息
+    log_message_pub = nh.advertise<prometheus_msgs::LogMessageControl>("/prometheus/log/control", 10);
+
+    // 10秒定时打印，以确保程序在正确运行
+    ros::Timer timer = nh.createTimer(ros::Duration(10.0), timerCallback);
+
+    // 参数读取
+    nh.param<float>("Takeoff_height", Takeoff_height, 1.0);
+    nh.param<float>("Disarm_height", Disarm_height, 0.15);
+    nh.param<float>("Land_speed", Land_speed, 0.2);
+    nh.param<int>("Land_mode",Land_mode,0);
+
+    nh.param<float>("geo_fence/x_min", geo_fence_x[0], -100.0);
+    nh.param<float>("geo_fence/x_max", geo_fence_x[1], 100.0);
+    nh.param<float>("geo_fence/y_min", geo_fence_y[0], -100.0);
+    nh.param<float>("geo_fence/y_max", geo_fence_y[1], 100.0);
+    nh.param<float>("geo_fence/z_min", geo_fence_z[0], -100.0);
+    nh.param<float>("geo_fence/z_max", geo_fence_z[1], 100.0);
+
+    // 设定起飞位置
+    Takeoff_position[0] = 0.0;
+    Takeoff_position[1] = 0.0;
+    Takeoff_position[2] = 0.15;
+
+    // 建议控制频率 ： 10 - 50Hz, 控制频率取决于控制形式，若控制方式为速度或加速度应适当提高频率
+    ros::Rate rate(20.0);
+
+    // 用于与mavros通讯的类，通过mavros发送控制指令至飞控【本程序->mavros->飞控】
+    command_to_mavros _command_to_mavros;
+
+    printf_param();
+    
+    // 初始化命令-
+    // 默认设置：Idle模式 电机怠速旋转 等待来自上层的控制指令
+    Command_Now.Mode                                = prometheus_msgs::ControlCommand::Idle;
+    Command_Now.Command_ID                          = 0;
+    Command_Now.Reference_State.Move_mode           = prometheus_msgs::PositionReference::XYZ_POS;
+    Command_Now.Reference_State.Move_frame          = prometheus_msgs::PositionReference::ENU_FRAME;
+    Command_Now.Reference_State.position_ref[0]     = 0;
+    Command_Now.Reference_State.position_ref[1]     = 0;
+    Command_Now.Reference_State.position_ref[2]     = 0;
+    Command_Now.Reference_State.velocity_ref[0]     = 0;
+    Command_Now.Reference_State.velocity_ref[1]     = 0;
+    Command_Now.Reference_State.velocity_ref[2]     = 0;
+    Command_Now.Reference_State.acceleration_ref[0] = 0;
+    Command_Now.Reference_State.acceleration_ref[1] = 0;
+    Command_Now.Reference_State.acceleration_ref[2] = 0;
+    Command_Now.Reference_State.yaw_ref             = 0;
+
+
+    // 记录启控时间
+    ros::Time begin_time = ros::Time::now();
+    float last_time = prometheus_control_utils::get_time_in_sec(begin_time);
+
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+    while(ros::ok())
+    {
+        // 当前时间
+        cur_time = prometheus_control_utils::get_time_in_sec(begin_time);
+        dt = cur_time  - last_time;
+        dt = constrain_function2(dt, 0.02, 0.1);
+        last_time = cur_time;
+
+        // 执行回调函数
+        ros::spinOnce();
+
+
+        // Check for geo fence: If drone is out of the geo fence, it will land now.
+        if(Command_Now.Mode !=prometheus_msgs::ControlCommand::Idle )
+        {
+            int safety_flag = check_failsafe();
+            if(safety_flag == 1)
+            {
+                Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
+            }else if(safety_flag == 2)
+            {
+                // 快速降落
+                Land_speed = 0.8;
+                quick_land = true;
+                Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
+            }
+        }
+
+        switch (Command_Now.Mode)
+        {
+        // 【Idle】 怠速旋转，此时可以切入offboard模式，但不会起飞。
+        case prometheus_msgs::ControlCommand::Idle:
+            
+            _command_to_mavros.idle();
+
+            // 设定yaw_ref=999时，切换offboard模式，并解锁
+            if(Command_Now.Reference_State.yaw_ref == 999)
+            {
+                if(_DroneState.mode != "OFFBOARD")
+                {
+                    _command_to_mavros.mode_cmd.request.custom_mode = "OFFBOARD";
+                    _command_to_mavros.set_mode_client.call(_command_to_mavros.mode_cmd);
+                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Setting to OFFBOARD Mode...");
+                }else
+                {
+                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "The Drone is in OFFBOARD Mode already...");
+                }
+
+                if(!_DroneState.armed)
+                {
+                    _command_to_mavros.arm_cmd.request.value = true;
+                    _command_to_mavros.arming_client.call(_command_to_mavros.arm_cmd);
+                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Arming...");
+                }else
+                {
+                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "The Drone is armd already...");
+                }
+            }
+            break;
+
+        // 【Takeoff】 从摆放初始位置原地起飞至指定高度，偏航角也保持当前角度    
+        case prometheus_msgs::ControlCommand::Takeoff:
+            
+            // 不能多次起飞！
+            // 此处起飞有一个bug，则是飞机起飞会有很严重的超调，没发现具体导致的因素
+
+            // 设定起飞点
+            if (Command_Last.Mode != prometheus_msgs::ControlCommand::Takeoff)
+            {
+                pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Takeoff to the desired point.");
+                // 设定起飞位置
+                Takeoff_position[0] = _DroneState.position[0];
+                Takeoff_position[1] = _DroneState.position[1];
+                Takeoff_position[2] = _DroneState.position[2];
+
+                //
+                Command_Now.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_POS;
+                Command_Now.Reference_State.Move_frame      = prometheus_msgs::PositionReference::ENU_FRAME;
+                Command_Now.Reference_State.position_ref[0] = Takeoff_position[0];
+                Command_Now.Reference_State.position_ref[1] = Takeoff_position[1];
+                Command_Now.Reference_State.position_ref[2] = Takeoff_position[2] + Takeoff_height;
+                Command_Now.Reference_State.yaw_ref         = _DroneState.attitude[2];
+            
+                state_sp = Eigen::Vector3d(Takeoff_position[0],Takeoff_position[1],Takeoff_position[2] + Takeoff_height);
+                yaw_sp = _DroneState.attitude[2]; //rad
+            }
+
+            _command_to_mavros.send_pos_setpoint(state_sp, yaw_sp);
+
+            //_command_to_mavros.takeoff(); 无用，未知原因
+
+            break;
+
+        // 【Hold】 悬停。当前位置悬停
+        case prometheus_msgs::ControlCommand::Hold:
+
+            if (Command_Last.Mode != prometheus_msgs::ControlCommand::Hold)
+            {
+                Command_Now.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_POS;
+                Command_Now.Reference_State.Move_frame      = prometheus_msgs::PositionReference::ENU_FRAME;
+                Command_Now.Reference_State.position_ref[0] = _DroneState.position[0];
+                Command_Now.Reference_State.position_ref[1] = _DroneState.position[1];
+                Command_Now.Reference_State.position_ref[2] = _DroneState.position[2];
+                Command_Now.Reference_State.yaw_ref         = _DroneState.attitude[2]; //rad
+
+                state_sp = Eigen::Vector3d(_DroneState.position[0],_DroneState.position[1],_DroneState.position[2]);
+                yaw_sp = _DroneState.attitude[2]; //rad
+            }
+            _command_to_mavros.send_pos_setpoint(state_sp, yaw_sp);
+            //_command_to_mavros.loiter(); 可用，但不启用
+
+            break;
+
+        // 【Land】 降落。两种降落方式： 只有加载了参数Land_mode为1时，启用第二种降落方式；默认启用第一种降落方式。
+        //  第一种：当前位置原地降落，降落后会自动上锁，且切换为mannual模式
+        //  第二种：当前位置原地降落，降落中到达Disarm_height后，切换为飞控中land模式
+        case prometheus_msgs::ControlCommand::Land:
+            
+            if(quick_land)
+            {
+                // 紧急降落
+                state_sp << 0.0, 0.0, -Land_speed;
+                yaw_sp = _DroneState.attitude[2]; //rad
+                _command_to_mavros.send_vel_setpoint(state_sp,yaw_sp);
+            }else if(Land_mode == 1)
+            {
+                if (Command_Last.Mode != prometheus_msgs::ControlCommand::Land)
+                {
+                    Command_Now.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XY_POS_Z_VEL;
+                    Command_Now.Reference_State.Move_frame      = prometheus_msgs::PositionReference::ENU_FRAME;
+                    Command_Now.Reference_State.position_ref[0] = _DroneState.position[0];
+                    Command_Now.Reference_State.position_ref[1] = _DroneState.position[1];              
+                    Command_Now.Reference_State.yaw_ref         = _DroneState.attitude[2]; //rad
+                }
+                //如果距离起飞高度小于30厘米，则直接切换为land模式；
+                if(_DroneState.position[2] <= Disarm_height)
+                {
+                    if(_DroneState.mode != "AUTO.LAND") // 无效
+                    {
+                        //此处切换会manual模式是因为:PX4默认在offboard模式且有控制的情况下没法上锁,直接使用飞控中的land模式
+                        _command_to_mavros.mode_cmd.request.custom_mode = "AUTO.LAND";
+                        _command_to_mavros.set_mode_client.call(_command_to_mavros.mode_cmd);
+                        pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "LAND: inter AUTO LAND filght mode");
+                    }
+                }
+                else if(_DroneState.position[2] > Disarm_height)
+                {
+                    Command_Now.Reference_State.position_ref[0] = _DroneState.position[0];
+                    Command_Now.Reference_State.position_ref[1] = _DroneState.position[1];
+                    Command_Now.Reference_State.position_ref[2] = _DroneState.position[2] - Land_speed * dt ;
+                    Command_Now.Reference_State.velocity_ref[0] = 0.0;
+                    Command_Now.Reference_State.velocity_ref[1] = 0.0;
+                    Command_Now.Reference_State.velocity_ref[2] = - Land_speed; //Land_speed
+                    state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
+                    state_sp_extra = Eigen::Vector3d(0.0,0.0,Command_Now.Reference_State.velocity_ref[2]);
+                    yaw_sp = _DroneState.attitude[2];
+                    _command_to_mavros.send_pos_vel_xyz_setpoint(state_sp,state_sp_extra,yaw_sp);
+                }
+                
+                if(_DroneState.landed)
+                {
+                    Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
+                }
+            }else{
+                if (Command_Last.Mode != prometheus_msgs::ControlCommand::Land)
+                {
+                    Command_Now.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_POS;
+                    Command_Now.Reference_State.Move_frame      = prometheus_msgs::PositionReference::ENU_FRAME;
+                    Command_Now.Reference_State.position_ref[0] = _DroneState.position[0];
+                    Command_Now.Reference_State.position_ref[1] = _DroneState.position[1];
+                    Command_Now.Reference_State.yaw_ref         = _DroneState.attitude[2]; //rad
+                }
+                if(_DroneState.position[2] > Disarm_height)
+                {
+                    Command_Now.Reference_State.position_ref[2] = _DroneState.position[2] - Land_speed * dt ;
+                    Command_Now.Reference_State.velocity_ref[0] = 0.0;
+                    Command_Now.Reference_State.velocity_ref[1] =  0.0;
+                    Command_Now.Reference_State.velocity_ref[2] = - Land_speed; //Land_speed
+
+                    state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1], Command_Now.Reference_State.position_ref[2] );
+                    state_sp_extra = Eigen::Vector3d(0.0, 0.0 , Command_Now.Reference_State.velocity_ref[2]);
+                    yaw_sp = Command_Now.Reference_State.yaw_ref;
+                    _command_to_mavros.send_pos_vel_xyz_setpoint(state_sp, state_sp_extra, yaw_sp);
+                }else
+                {
+                    //此处切换会manual模式是因为:PX4默认在offboard模式且有控制的情况下没法上锁,直接使用飞控中的land模式
+                    _command_to_mavros.mode_cmd.request.custom_mode = "MANUAL";
+                    _command_to_mavros.set_mode_client.call(_command_to_mavros.mode_cmd);
+
+                    _command_to_mavros.arm_cmd.request.value = false;
+                    _command_to_mavros.arming_client.call(_command_to_mavros.arm_cmd);
+                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Disarming...");
+
+                    pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "LAND: switch to MANUAL filght mode");
+                }
+            }
+            break;
+
+        case prometheus_msgs::ControlCommand::Move:
+
+            // PX4暂不支持轨迹模式
+            // PX4暂不支持位置－速度复合模式（详细见mavlink_receiver.cpp）
+            if(Command_Now.Reference_State.Move_frame  == prometheus_msgs::PositionReference::ENU_FRAME)
+            {
+                if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_POS )
+                {
+                    state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
+                    yaw_sp = Command_Now.Reference_State.yaw_ref;
+                }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_VEL )
+                {
+                    state_sp = Eigen::Vector3d(Command_Now.Reference_State.velocity_ref[0],Command_Now.Reference_State.velocity_ref[1],Command_Now.Reference_State.velocity_ref[2]);
+                    yaw_sp = Command_Now.Reference_State.yaw_ref;
+                }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XY_VEL_Z_POS )
+                {
+                    state_sp = Eigen::Vector3d(Command_Now.Reference_State.velocity_ref[0],Command_Now.Reference_State.velocity_ref[1],Command_Now.Reference_State.position_ref[2]);
+                    yaw_sp = Command_Now.Reference_State.yaw_ref;
+                }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XY_POS_Z_VEL )
+                {
+                    Command_Now.Reference_State.position_ref[2] = _DroneState.position[2] + Command_Now.Reference_State.velocity_ref[2] * dt;
+                    state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
+                    state_sp_extra = Eigen::Vector3d(0.0, 0.0 ,Command_Now.Reference_State.velocity_ref[2]);
+                    yaw_sp = Command_Now.Reference_State.yaw_ref;
+                }else if ( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_ACC )
+                {
+                    state_sp = Eigen::Vector3d(Command_Now.Reference_State.acceleration_ref[0],Command_Now.Reference_State.acceleration_ref[1],Command_Now.Reference_State.acceleration_ref[2]);
+                    yaw_sp = Command_Now.Reference_State.yaw_ref;
+                }
+            }else
+            {
+                if( Command_Now.Command_ID  >  Command_Last.Command_ID)
+                {
+                    if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_POS )
+                    {
+                        float d_pos_body[2] = {Command_Now.Reference_State.position_ref[0], Command_Now.Reference_State.position_ref[1]};         //the desired xy position in Body Frame
+                        float d_pos_enu[2];                       //the desired xy position in enu Frame (The origin point is the drone)
+                        prometheus_control_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);
+
+                        Command_Now.Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
+                        Command_Now.Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
+                        Command_Now.Reference_State.position_ref[2] = _DroneState.position[2] + Command_Now.Reference_State.position_ref[2];
+                        state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
+                        yaw_sp = _DroneState.attitude[2] + Command_Now.Reference_State.yaw_ref;
+                    }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_VEL )
+                    {
+                        //xy velocity mode
+                        float d_vel_body[2] = {Command_Now.Reference_State.velocity_ref[0], Command_Now.Reference_State.velocity_ref[1]};         //the desired xy velocity in Body Frame
+                        float d_vel_enu[2];                 //the desired xy velocity in NED Frame
+
+                        //根据无人机当前偏航角进行坐标系转换
+                        prometheus_control_utils::rotation_yaw(_DroneState.attitude[2], d_vel_body, d_vel_enu);
+                        Command_Now.Reference_State.velocity_ref[0] = d_vel_enu[0];
+                        Command_Now.Reference_State.velocity_ref[1] = d_vel_enu[1];
+                        state_sp = Eigen::Vector3d(Command_Now.Reference_State.velocity_ref[0],Command_Now.Reference_State.velocity_ref[1],Command_Now.Reference_State.velocity_ref[2]);
+                        yaw_sp = _DroneState.attitude[2] + Command_Now.Reference_State.yaw_ref;
+                    }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XY_VEL_Z_POS )
+                    {
+                        //xy velocity mode
+                        float d_vel_body[2] = {Command_Now.Reference_State.velocity_ref[0], Command_Now.Reference_State.velocity_ref[1]};         //the desired xy velocity in Body Frame
+                        float d_vel_enu[2];                   //the desired xy velocity in NED Frame
+
+                        //根据无人机当前偏航角进行坐标系转换
+                        prometheus_control_utils::rotation_yaw(_DroneState.attitude[2], d_vel_body, d_vel_enu);
+                        Command_Now.Reference_State.position_ref[0] = 0;
+                        Command_Now.Reference_State.position_ref[1] = 0;
+                        Command_Now.Reference_State.velocity_ref[0] = d_vel_enu[0];
+                        Command_Now.Reference_State.velocity_ref[1] = d_vel_enu[1];
+                        Command_Now.Reference_State.velocity_ref[2] = 0.0;
+                        Command_Now.Reference_State.position_ref[2] = _DroneState.position[2] + Command_Now.Reference_State.position_ref[2];
+                        // 高度锁定为给定值 2021.3.24 云台追踪控制修改
+                        state_sp = Eigen::Vector3d(Command_Now.Reference_State.velocity_ref[0],Command_Now.Reference_State.velocity_ref[1],Command_Now.Reference_State.position_ref[2]);
+                        
+                        if(Command_Now.Reference_State.Move_frame  == prometheus_msgs::PositionReference::MIX_FRAME)
+                        {
+                            // 2021.6.30 for color_line_following
+                            yaw_sp = _DroneState.attitude[2] + Command_Now.Reference_State.yaw_ref;
+                        }else
+                        {
+                            // 偏航角更新为锁定 2021.3.24 云台追踪控制修改
+                            yaw_sp = Command_Now.Reference_State.yaw_ref + _DroneState.attitude[2] ;
+                        }
+                        
+                    }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XY_POS_Z_VEL )
+                    {
+                        float d_pos_body[2] = {Command_Now.Reference_State.position_ref[0], Command_Now.Reference_State.position_ref[1]};         //the desired xy position in Body Frame
+                        float d_pos_enu[2];                     //the desired xy position in enu Frame (The origin point is the drone)
+                        prometheus_control_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);
+
+                        Command_Now.Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
+                        Command_Now.Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
+                        Command_Now.Reference_State.position_ref[2] = _DroneState.position[2] + Command_Now.Reference_State.velocity_ref[2] * dt;
+                        state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
+                        state_sp_extra = Eigen::Vector3d(0.0, 0.0 ,Command_Now.Reference_State.velocity_ref[2]);
+                        yaw_sp = _DroneState.attitude[2] + Command_Now.Reference_State.yaw_ref;
+                    }else if ( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_ACC )
+                    {
+                       pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Not Defined. Change to ENU frame");
+                    }
+                }
+            }
+
+            if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_POS )
+            {
+                _command_to_mavros.send_pos_setpoint(state_sp, yaw_sp);
+            }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_VEL )
+            {
+                if(Command_Now.Reference_State.Yaw_Rate_Mode)
+                {
+                    yaw_rate_sp = Command_Now.Reference_State.yaw_rate_ref;
+                    _command_to_mavros.send_vel_setpoint_yaw_rate(state_sp, yaw_rate_sp);
+                }else
+                {
+                    _command_to_mavros.send_vel_setpoint(state_sp, yaw_sp);
+                }
+            }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XY_VEL_Z_POS )
+            {
+                if(Command_Now.Reference_State.Yaw_Rate_Mode)
+                {
+                    yaw_rate_sp = Command_Now.Reference_State.yaw_rate_ref;
+                    _command_to_mavros.send_vel_xy_pos_z_setpoint_yawrate(state_sp, yaw_rate_sp);
+                }else
+                {
+                    _command_to_mavros.send_vel_xy_pos_z_setpoint(state_sp, yaw_sp);
+                }
+            }else if ( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XY_POS_Z_VEL )
+            {
+                // 特殊情况，一般也用不到
+                _command_to_mavros.send_pos_vel_xyz_setpoint(state_sp, state_sp_extra, yaw_sp);
+            }else if ( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_ACC )
+            {
+                _command_to_mavros.send_acc_xyz_setpoint(state_sp, yaw_sp);
+            }else if ( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::TRAJECTORY )
+            {
+                state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
+                state_sp_extra = Eigen::Vector3d(Command_Now.Reference_State.velocity_ref[0], Command_Now.Reference_State.velocity_ref[1] ,Command_Now.Reference_State.velocity_ref[2]);
+                yaw_sp = Command_Now.Reference_State.yaw_ref;
+                _command_to_mavros.send_pos_vel_xyz_setpoint(state_sp, state_sp_extra,yaw_sp);
+            }else
+            {
+                pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Not Defined. Hold there");
+                _command_to_mavros.loiter();
+            }
+            break;
+
+
+        // 【Disarm】 上锁
+        case prometheus_msgs::ControlCommand::Disarm:
+
+            pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Disarm: switch to MANUAL flight mode");
+            if(_DroneState.mode == "OFFBOARD")
+            {
+                _command_to_mavros.mode_cmd.request.custom_mode = "MANUAL";
+                _command_to_mavros.set_mode_client.call(_command_to_mavros.mode_cmd);
+            }
+
+            if(_DroneState.armed)
+            {
+                _command_to_mavros.arm_cmd.request.value = false;
+                _command_to_mavros.arming_client.call(_command_to_mavros.arm_cmd);
+            }
+            
+            break;
+
+        // 【User_Mode1】 暂空。可进行自定义
+        case prometheus_msgs::ControlCommand::User_Mode1:
+            
+            break;
+
+        // 【User_Mode2】 暂空。可进行自定义
+        case prometheus_msgs::ControlCommand::User_Mode2:
+            
+            break;
+        }
+
+        //发布用于RVIZ显示的位姿
+        ref_pose_rviz = get_rviz_ref_posistion(Command_Now);   
+        rivz_ref_pose_pub.publish(ref_pose_rviz);
+
+        //发布log消息，可用rosbag记录
+        LogMessage.control_type = PX4_SENDER;
+        LogMessage.time = cur_time;
+        LogMessage.Drone_State = _DroneState;
+        LogMessage.Control_Command = Command_Now;
+        LogMessage.ref_pose = ref_pose_rviz;
+
+        log_message_pub.publish(LogMessage);
+
+        Command_Last = Command_Now;
+        rate.sleep();
+    }
+
+    return 0;
+
+}
+
+
+void printf_param()
+{
+    cout <<">>>>>>>>>>>>>>>>>>>>>>>> px4_sender Parameter <<<<<<<<<<<<<<<<<<<<<<" <<endl;
+    cout << "Takeoff_height   : "<< Takeoff_height<<" [m] "<<endl;
+    cout << "Disarm_height    : "<< Disarm_height <<" [m] "<<endl;
+    cout << "Land_speed       : "<< Land_speed <<" [m/s] "<<endl;
+    cout << "Land_mode        : "<< Land_mode << endl;
+    cout << "geo_fence_x : "<< geo_fence_x[0] << " [m]  to  "<<geo_fence_x[1] << " [m]"<< endl;
+    cout << "geo_fence_y : "<< geo_fence_y[0] << " [m]  to  "<<geo_fence_y[1] << " [m]"<< endl;
+    cout << "geo_fence_z : "<< geo_fence_z[0] << " [m]  to  "<<geo_fence_z[1] << " [m]"<< endl;
+}
+
+int check_failsafe()
+{
+    if (_DroneState.position[0] < geo_fence_x[0] || _DroneState.position[0] > geo_fence_x[1] ||
+        _DroneState.position[1] < geo_fence_y[0] || _DroneState.position[1] > geo_fence_y[1] ||
+        _DroneState.position[2] < geo_fence_z[0] || _DroneState.position[2] > geo_fence_z[1])
+    {
+        cout << RED  <<":----> Out of the geo fence, the drone is landing..."<< TAIL << endl;
+        return 1;
+    }else if(!_DroneState.odom_valid)
+    {
+        cout << RED  <<":----> Mocap valid..., land quickly!"<< TAIL << endl;
+        return 2;
+    }
+    else{
+        return 0;
+    }
+}
+
+
+geometry_msgs::PoseStamped get_rviz_ref_posistion(const prometheus_msgs::ControlCommand& cmd)
+{
+    geometry_msgs::PoseStamped ref_pose;
+
+    ref_pose.header.stamp = ros::Time::now();
+    // world: 世界系,即gazebo坐标系,参见tf_transform.launch
+    ref_pose.header.frame_id = "world";
+
+    if(cmd.Mode == prometheus_msgs::ControlCommand::Idle)
+    {
+        ref_pose.pose.position.x = _DroneState.position[0];
+        ref_pose.pose.position.y = _DroneState.position[1];
+        ref_pose.pose.position.z = _DroneState.position[2];
+        ref_pose.pose.orientation = _DroneState.attitude_q;
+    }else if(cmd.Mode == prometheus_msgs::ControlCommand::Takeoff || cmd.Mode == prometheus_msgs::ControlCommand::Hold)
+    {
+        ref_pose.pose.position.x = cmd.Reference_State.position_ref[0];
+        ref_pose.pose.position.y = cmd.Reference_State.position_ref[1];
+        ref_pose.pose.position.z = cmd.Reference_State.position_ref[2];
+        ref_pose.pose.orientation = _DroneState.attitude_q;
+    }else if(cmd.Mode == prometheus_msgs::ControlCommand::Disarm  || cmd.Mode == prometheus_msgs::ControlCommand::Land )
+    {
+        ref_pose.pose.position.x = cmd.Reference_State.position_ref[0];
+        ref_pose.pose.position.y = cmd.Reference_State.position_ref[1];
+        ref_pose.pose.position.z = 0.0;
+        ref_pose.pose.orientation = _DroneState.attitude_q;
+    }
+    else if(cmd.Mode == prometheus_msgs::ControlCommand::Move)
+    {
+        if( cmd.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_POS )
+        {
+            ref_pose.pose.position.x = cmd.Reference_State.position_ref[0];
+            ref_pose.pose.position.y = cmd.Reference_State.position_ref[1];
+            ref_pose.pose.position.z = cmd.Reference_State.position_ref[2];
+        }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_VEL )
+        {
+            ref_pose.pose.position.x = _DroneState.position[0] + cmd.Reference_State.velocity_ref[0] * dt;
+            ref_pose.pose.position.y = _DroneState.position[1] + cmd.Reference_State.velocity_ref[1] * dt;
+            ref_pose.pose.position.z = _DroneState.position[2] + cmd.Reference_State.velocity_ref[2] * dt;
+        }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XY_VEL_Z_POS )
+        {
+            ref_pose.pose.position.x = _DroneState.position[0] + cmd.Reference_State.velocity_ref[0] * dt;
+            ref_pose.pose.position.y = _DroneState.position[1] + cmd.Reference_State.velocity_ref[1] * dt;
+            ref_pose.pose.position.z = cmd.Reference_State.position_ref[2];
+        }else if( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XY_POS_Z_VEL )
+        {
+            ref_pose.pose.position.x = cmd.Reference_State.position_ref[0];
+            ref_pose.pose.position.y = cmd.Reference_State.position_ref[1];
+            ref_pose.pose.position.z = _DroneState.position[2] + cmd.Reference_State.velocity_ref[2] * dt;
+        }else if ( Command_Now.Reference_State.Move_mode  == prometheus_msgs::PositionReference::XYZ_ACC )
+        {
+            ref_pose.pose.position.x = _DroneState.position[0] + 0.5 * cmd.Reference_State.acceleration_ref[0] * dt * dt;
+            ref_pose.pose.position.y = _DroneState.position[1] + 0.5 * cmd.Reference_State.acceleration_ref[1] * dt * dt;
+            ref_pose.pose.position.z = _DroneState.position[2] + 0.5 * cmd.Reference_State.acceleration_ref[2] * dt * dt;
+        }
+
+        ref_pose.pose.orientation = _DroneState.attitude_q;
+    }else
+    {
+        ref_pose.pose.position.x = 0.0;
+        ref_pose.pose.position.y = 0.0;
+        ref_pose.pose.position.z = 0.0;
+        ref_pose.pose.orientation = _DroneState.attitude_q;
+    }
+
+    return ref_pose;
+}

src/飞控/terminal_control.cpp

@@ -0,0 +1,889 @@
+/***************************************************************************************************************************
+* terminal_control.cpp
+*
+* Author: Qyp
+*
+* Update Time: 2020.11.4
+*
+* Introduction:  test function for sending ControlCommand.msg
+***************************************************************************************************************************/
+#include <ros/ros.h>
+#include <iostream>
+
+#include <prometheus_msgs/ControlCommand.h>
+#include <mavros_msgs/CommandBool.h>
+#include <mavros_msgs/SetMode.h>
+#include <mavros_msgs/State.h>
+#include <nav_msgs/Path.h>
+
+#include "controller_test.h"
+#include "KeyboardEvent.h"
+
+
+//json数据库
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <jsoncpp/json/json.h>
+#include <ctime>
+
+#define HOST "192.168.43.58"
+#define PORT 1900
+
+#define VEL_XY_STEP_SIZE 0.1
+#define VEL_Z_STEP_SIZE 0.1
+#define YAW_STEP_SIZE 0.08
+#define TRA_WINDOW 2000
+#define NODE_NAME "terminal_control"
+
+using namespace std;
+
+//即将发布的command
+prometheus_msgs::ControlCommand Command_to_pub;
+//轨迹容器
+std::vector<geometry_msgs::PoseStamped> posehistory_vector_;
+
+
+float time_trajectory = 0.0;
+// 轨迹追踪总时长，键盘控制时固定时长，指令输入控制可调
+float trajectory_total_time = 50.0;
+
+
+//发布
+ros::Publisher move_pub;
+ros::Publisher ref_trajectory_pub;
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>　函数声明　<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+void mainloop1();
+void mainloop2();
+void generate_com(int Move_mode, float state_desired[4]);
+void Draw_in_rviz(const prometheus_msgs::PositionReference& pos_ref, bool draw_trajectory);
+void timerCallback(const ros::TimerEvent& e)
+{
+    cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl;
+    cout << "ENTER key to control the drone: " <<endl;
+    cout << "1 for Arm, Space for Takeoff, L for Land, H for Hold, 0 for Disarm, 8/9 for Trajectory tracking" <<endl;
+    cout << "Move mode is fixed (XYZ_VEL,BODY_FRAME): w/s for body_x, a/d for body_y, k/m for z, q/e for body_yaw" <<endl;
+    cout << "CTRL-C to quit." <<endl;
+}
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>　主函数　<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "terminal_control");
+    ros::NodeHandle nh;
+
+    //　【发布】　控制指令
+    move_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
+
+    //　【发布】　参考轨迹
+    ref_trajectory_pub = nh.advertise<nav_msgs::Path>("/prometheus/reference_trajectory", 10);
+    
+    //用于控制器测试的类，功能例如：生成圆形轨迹，８字轨迹等
+    Controller_Test Controller_Test;    // 打印参数
+    Controller_Test.printf_param();
+
+    // 初始化命令 - Idle模式 电机怠速旋转 等待来自上层的控制指令
+    Command_to_pub.Mode                                = prometheus_msgs::ControlCommand::Idle;
+    Command_to_pub.Command_ID                          = 0;
+    Command_to_pub.source = NODE_NAME;
+    Command_to_pub.Reference_State.Move_mode           = prometheus_msgs::PositionReference::XYZ_POS;
+    Command_to_pub.Reference_State.Move_frame          = prometheus_msgs::PositionReference::ENU_FRAME;
+    Command_to_pub.Reference_State.position_ref[0]     = 0;
+    Command_to_pub.Reference_State.position_ref[1]     = 0;
+    Command_to_pub.Reference_State.position_ref[2]     = 0;
+    Command_to_pub.Reference_State.velocity_ref[0]     = 0;
+    Command_to_pub.Reference_State.velocity_ref[1]     = 0;
+    Command_to_pub.Reference_State.velocity_ref[2]     = 0;
+    Command_to_pub.Reference_State.acceleration_ref[0] = 0;
+    Command_to_pub.Reference_State.acceleration_ref[1] = 0;
+    Command_to_pub.Reference_State.acceleration_ref[2] = 0;
+    Command_to_pub.Reference_State.yaw_ref             = 0;
+
+    //固定的浮点显示
+    cout.setf(ios::fixed);
+    //setprecision(n) 设显示小数精度为n位
+    cout<<setprecision(2);
+    //左对齐
+    cout.setf(ios::left);
+    // 强制显示小数点
+    cout.setf(ios::showpoint);
+    // 强制显示符号
+    //cout.setf(ios::showpos);
+
+    // 选择通过终端输入控制或键盘控制
+    int Remote_Mode;
+    cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl;
+    cout << "Please choose the Remote Mode: 0 for command input control, 1 for keyboard input control"<<endl;
+    cin >> Remote_Mode;
+    if (Remote_Mode == 0)
+    {
+        cout << "Command input control mode"<<endl;
+        mainloop1();
+    }else if(Remote_Mode == 1)
+    {
+        ros::Timer timer = nh.createTimer(ros::Duration(30.0), timerCallback);
+        cout << "Keyboard input control mode"<<endl;
+        mainloop2();
+    }
+
+    return 0;
+}
+
+void mainloop1()
+{
+    int Control_Mode = 0;
+    int Move_mode = 0;
+    int Move_frame = 0;
+    int Trjectory_mode = 0;
+    float state_desired[4];
+    Controller_Test Controller_Test;
+
+    while(ros::ok())
+    {
+        // Waiting for input
+        cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl;
+        cout << "Please choose the Command.Mode: 0 for Idle, 1 for Takeoff, 2 for Hold, 3 for Land, 4 for Move, 5 for Disarm, 6 for User_Mode1, 7 for User_Mode2"<<endl;
+        cout << "Input 999 to switch to offboard mode and arm the drone (ONLY for simulation, please use RC in experiment!!!)"<<endl;
+        cin  >> Control_Mode;
+
+        if(Control_Mode == prometheus_msgs::ControlCommand::Move)
+        {
+            cout << "Please choose the Command.Reference_State.Move_mode: 0 for XYZ_POS, 1 for XY_POS_Z_VEL, 2 for XY_VEL_Z_POS, 3 for XYZ_VEL, 5 for TRAJECTORY"<<endl;
+            cin >> Move_mode;
+
+            if(Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
+            {
+                cout << "For safety, please move the drone near to the trajectory start point firstly!!!"<<endl;
+                cout << "Please choose the trajectory type: 0 for Circle, 1 for Eight Shape, 2 for Step, 3 for Line"<<endl;
+                cin >> Trjectory_mode;  
+                cout << "Input the trajectory_total_time:"<<endl;
+                cin >> trajectory_total_time;
+            }else
+            {
+                cout << "Please choose the Command.Reference_State.Move_frame: 0 for ENU_FRAME, 1 for BODY_FRAME"<<endl;
+                cin >> Move_frame; 
+                cout << "Please input the reference state [x y z yaw]: "<< endl;
+                cout << "setpoint_t[0] --- x [m] : "<< endl;
+                cin >> state_desired[0];
+                cout << "setpoint_t[1] --- y [m] : "<< endl;
+                cin >> state_desired[1];
+                cout << "setpoint_t[2] --- z [m] : "<< endl;
+                cin >> state_desired[2];
+                cout << "setpoint_t[3] --- yaw [du] : "<< endl;
+                cin >> state_desired[3];
+            }
+  
+        }
+        else if(Control_Mode == prometheus_msgs::ControlCommand::User_Mode1)
+        {
+            cout << "Please choose the Command.Reference_State.Move_mode: 0 for XYZ_POS, 1 for XY_POS_Z_VEL, 2 for XY_VEL_Z_POS, 3 for XYZ_VEL, 5 for TRAJECTORY"<<endl;
+            Move_mode = 5;
+
+            if(Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
+            {
+                cout << "For safety, please move the drone near to the trajectory start point firstly!!!"<<endl;
+                cout << "Please choose the trajectory type: 0 for Circle, 1 for Eight Shape, 2 for Step, 3 for Line"<<endl;
+                Trjectory_mode = 0;  
+                cout << "Input the trajectory_total_time:"<<endl;
+                trajectory_total_time = 100;
+                cout << "Please choose the Command.Reference_State.Move_frame: 0 for ENU_FRAME, 1 for BODY_FRAME"<<endl;
+                Move_frame = 1; 
+                cout << "Please input the reference state [x y z yaw]: "<< endl;
+                cout << "setpoint_t[0] --- x [m] : "<< endl;
+                cin >> state_desired[0];
+                cout << "setpoint_t[1] --- y [m] : "<< endl;
+                cin >> state_desired[1];
+                cout << "setpoint_t[2] --- z [m] : "<< endl;
+                cin >> state_desired[2];
+                cout << "setpoint_t[3] --- yaw [du] : "<< endl;
+                cin >> state_desired[3];
+            }
+        }
+        else if(Control_Mode == 999)
+        {
+            Command_to_pub.header.stamp = ros::Time::now();
+            Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle;
+            Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+            Command_to_pub.source = NODE_NAME;
+            Command_to_pub.Reference_State.yaw_ref = 999;
+            move_pub.publish(Command_to_pub);
+            Command_to_pub.Reference_State.yaw_ref = 0.0;
+        }
+
+        switch (Control_Mode)
+        {
+            case prometheus_msgs::ControlCommand::Idle:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+
+            case prometheus_msgs::ControlCommand::Takeoff:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Takeoff;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+
+            case prometheus_msgs::ControlCommand::Hold:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Hold;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+    
+            case prometheus_msgs::ControlCommand::Land:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Land;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+
+            case prometheus_msgs::ControlCommand::Move:
+                if(Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
+                {
+                     time_trajectory = 0.0;
+
+                    while(time_trajectory < trajectory_total_time)
+                    {
+                        Command_to_pub.header.stamp = ros::Time::now();
+                        Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+                        Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                        Command_to_pub.source = NODE_NAME;
+
+                        if(Trjectory_mode == 0)
+                        {
+                            Command_to_pub.Reference_State = Controller_Test.Circle_trajectory_generation(time_trajectory);
+                        }else if(Trjectory_mode == 1)
+                        {
+                            Command_to_pub.Reference_State = Controller_Test.Eight_trajectory_generation(time_trajectory);
+                        }else if(Trjectory_mode == 2)
+                        {
+                            Command_to_pub.Reference_State = Controller_Test.Step_trajectory_generation(time_trajectory);
+                        }else if(Trjectory_mode == 3)
+                        {
+                            Command_to_pub.Reference_State = Controller_Test.Line_trajectory_generation(time_trajectory);
+                        }
+
+                        move_pub.publish(Command_to_pub);
+                        time_trajectory = time_trajectory + 0.01;
+
+                        cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time  << " [ s ]" <<endl;
+
+                        Draw_in_rviz(Command_to_pub.Reference_State, true);
+
+                        ros::Duration(0.01).sleep();
+                    }
+                    
+                }else
+                {
+                    Command_to_pub.header.stamp = ros::Time::now();
+                    Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+                    Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                    Command_to_pub.source = NODE_NAME;
+                    Command_to_pub.Reference_State.Move_mode  = Move_mode;
+                    Command_to_pub.Reference_State.Move_frame = Move_frame;
+                    Command_to_pub.Reference_State.time_from_start = -1;
+                    generate_com(Move_mode, state_desired);
+        
+                    move_pub.publish(Command_to_pub);
+                }
+                break;
+            
+            case prometheus_msgs::ControlCommand::Disarm:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Disarm;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+
+            case prometheus_msgs::ControlCommand::User_Mode1:
+                int sock;
+                struct sockaddr_in server;
+                char message[4096*4], server_reply[4096*4];
+
+                sock = socket(AF_INET, SOCK_STREAM, 0);
+                if(sock == -1){
+                    perror("Could not create socket");
+			        break;                    
+			        //return 1;
+                }
+                puts("Socket created");
+                server.sin_addr.s_addr = inet_addr(HOST);
+                server.sin_family = AF_INET;
+                server.sin_port = htons(PORT);
+
+                if(connect(sock, (struct sockaddr *)&server, sizeof(server)) < 0){
+                    perror("connect failed. Error");
+			        break;                    
+			        //return 1;
+                }
+                puts("Connected");
+
+                if(Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
+                {
+                    Command_to_pub.header.stamp = ros::Time::now();
+                    Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+                    Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                    Command_to_pub.source = NODE_NAME;
+                    Command_to_pub.Reference_State.Move_mode  = Move_mode;
+                    Command_to_pub.Reference_State.Move_frame = Move_frame;
+                    Command_to_pub.Reference_State.time_from_start = -1;
+                    generate_com(Move_mode, state_desired);
+        
+                    move_pub.publish(Command_to_pub);
+                    sleep(20);
+                    time_trajectory = 0.0;
+                   
+
+                    while(time_trajectory < trajectory_total_time)
+                    {
+                        try{
+                            if(recv(sock, server_reply, 4096, 0) < 0){
+                                perror("recv failed");
+                                break;
+                            }
+                            std::string recvmsg(server_reply);
+                            Json::Value root;
+                            Json::Reader reader;
+                            bool parsingSuccessful = reader.parse(recvmsg, root);
+                            if(!parsingSuccessful){
+                                std::cout << "JSON parsing Error" << std::endl;
+                                break;
+                            }
+                            std::cout << recvmsg << std::endl;
+                            Json::Value target = root["targets"];
+                            Json::Value sendmsg;
+                            float x_ori = 0.0;
+                            float y_ori = 0.0;
+                            
+                            //pos_msg = "0";
+                            if (target.size()){
+                                std::cout<<"see something"<<endl;
+                                if (!target.isNull() && target.isArray()) {
+                                    std::string detectedClass = target[0]["class"].asString();
+                                    std::cout<<detectedClass<<endl;
+                                    }
+                                //pos_msg = std::to_string(state_desired[0]) + " " + std::to_string(state_desired[1]);
+                                srand(time(NULL));
+                                float rand_float = static_cast<float>(rand()) / 10;
+                                x_ori = state_desired[0] + 0.9761;
+                                y_ori = state_desired[1] + 0.3562;
+                                sendmsg["x_pos"] = x_ori;
+                                sendmsg["y_pos"] = y_ori;
+                                std::string pos_msg = Json::writeString(Json::StreamWriterBuilder(), sendmsg);
+                                std::cout<<pos_msg<<endl;
+                                if (send(sock, pos_msg.c_str(), pos_msg.length(), 0) == -1) {
+                                    std::cout << "Failed to send message" << std::endl;
+                                    break;                                    
+                                    //return 1;
+                                }
+                                break;
+                            }
+                            sendmsg["x_pos"] = x_ori;
+                            sendmsg["y_pos"] = y_ori;
+                            std::string pos_msg = Json::writeString(Json::StreamWriterBuilder(), sendmsg);
+                            std::cout<<pos_msg<<endl;
+                            if (send(sock, pos_msg.c_str(), pos_msg.length(), 0) == -1) {
+                                std::cout << "Failed to send message" << std::endl;
+                                break; 
+                            }
+			    
+                            
+                        }catch(Json::RuntimeError e){
+                            std::cout << "JSON runtime exception: " << e.what() << std::endl;
+                            break;
+                        }
+
+
+                        Command_to_pub.header.stamp = ros::Time::now();
+                        Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+                        Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                        Command_to_pub.source = NODE_NAME;
+
+
+                        //Command_to_pub.Reference_State = Controller_Test.Circle_trajectory_generation(time_trajectory);
+                        Command_to_pub.Reference_State.Move_mode  = Move_mode;
+                        Command_to_pub.Reference_State.Move_frame = Move_frame;
+                        Command_to_pub.Reference_State.time_from_start = -1;
+                        
+                        if (time_trajectory <= 25){
+                            state_desired[0] += time_trajectory/5;
+                        }
+                        else if (time_trajectory > 25 && time_trajectory <= 50){
+                            state_desired[1] += (time_trajectory-25)/5;
+                        }
+                        else if (time_trajectory > 50 && time_trajectory <= 75){
+                            state_desired[0] -= (time_trajectory-50)/5;
+                        }
+                        else if (time_trajectory > 75 && time_trajectory <= 100){
+                            state_desired[1] -= (time_trajectory-75)/5;
+                        }
+                        cout<<"******************************"<<endl;
+                        cout<<"x pos:"<<state_desired[0]<<"                   "<<"y pos:"<<state_desired[1]<<endl;
+                        cout<<"******************************"<<endl;
+                        generate_com(Move_mode, state_desired);
+            
+                        move_pub.publish(Command_to_pub);
+
+
+                        //move_pub.publish(Command_to_pub);
+                        time_trajectory = time_trajectory + 0.01;
+
+                        //cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time  << " [ s ]" <<endl;
+
+                        //Draw_in_rviz(Command_to_pub.Reference_State, true);
+
+                        ros::Duration(0.01).sleep();
+                    }
+                    
+
+
+                }
+                break;
+            
+            case prometheus_msgs::ControlCommand::User_Mode2:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::User_Mode2;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+        }
+        
+        cout << "....................................................." <<endl;
+        
+        sleep(1.0);
+    }
+}
+
+void mainloop2()
+{
+    KeyboardEvent keyboardcontrol;
+    Controller_Test Controller_Test;
+
+    char key_now;
+    char key_last;
+
+    cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl;
+    cout << "ENTER key to control the drone: " <<endl;
+    cout << "1 for Arm, Space for Takeoff, L for Land, H for Hold, 0 for Disarm, 8/9 for Trajectory tracking" <<endl;
+    cout << "Move mode is fixed (XYZ_VEL,BODY_FRAME): w/s for body_x, a/d for body_y, k/m for z, q/e for body_yaw" <<endl;
+    cout << "CTRL-C to quit." <<endl;
+
+    while (ros::ok())
+    {
+        keyboardcontrol.RosWhileLoopRun();
+        key_now = keyboardcontrol.GetPressedKey();
+        switch (key_now)
+        {
+
+        //悬停, 应当只发送一次, 不需要循环发送
+        case U_KEY_NONE:
+
+          if (key_last != U_KEY_NONE)
+          {
+              //to be continued.
+          }
+          sleep(0.5);
+
+          break;
+
+        // 数字1（非小键盘数字）：解锁及切换到OFFBOARD模式
+        case U_KEY_1:
+          cout << " " <<endl;
+          cout << "Arm and Switch to OFFBOARD." <<endl;
+      
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.yaw_ref = 999;
+          move_pub.publish(Command_to_pub);
+          sleep(1.0);
+          break;
+
+        // 空格：起飞
+        case U_KEY_SPACE:
+          cout << " " <<endl;
+          cout << "Switch to Takeoff Mode." <<endl;
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Takeoff;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.Reference_State.yaw_ref = 0.0;
+          Command_to_pub.source = NODE_NAME;
+          move_pub.publish(Command_to_pub);
+
+          sleep(1.0);
+
+          break;
+
+        // 键盘L：降落
+        case U_KEY_L:
+          cout << " " <<endl;
+          cout << "Switch to Land Mode." <<endl;
+      
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Land;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          move_pub.publish(Command_to_pub);
+
+          break;
+
+        // 键盘0（非小键盘数字）：紧急停止
+        case U_KEY_0:
+          cout << " " <<endl;
+          cout << "Switch to Disarm Mode." <<endl;
+      
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Disarm;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          move_pub.publish(Command_to_pub);
+
+          break;
+
+        //起飞要维持起飞的模式?
+        case U_KEY_T:
+          cout << " " <<endl;
+          cout << "Switch to Takeoff Mode." <<endl;
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Takeoff;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          move_pub.publish(Command_to_pub);
+
+          sleep(2.0);
+        
+          break;
+
+        //起飞要维持起飞的模式?
+        case U_KEY_H:
+          cout << " " <<endl;
+          cout << "Switch to Hold Mode." <<endl;
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Hold;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.position_ref[0]     = 0;
+          Command_to_pub.Reference_State.position_ref[1]     = 0;
+          Command_to_pub.Reference_State.position_ref[2]     = 0;
+          Command_to_pub.Reference_State.velocity_ref[0]     = 0;
+          Command_to_pub.Reference_State.velocity_ref[1]     = 0;
+          Command_to_pub.Reference_State.velocity_ref[2]     = 0;
+          Command_to_pub.Reference_State.acceleration_ref[0] = 0;
+          Command_to_pub.Reference_State.acceleration_ref[1] = 0;
+          Command_to_pub.Reference_State.acceleration_ref[2] = 0;
+          move_pub.publish(Command_to_pub);
+
+          sleep(1.0);
+        
+          break;
+
+        // 向前匀速运动
+        case U_KEY_W:
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[0]     += VEL_XY_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+          
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+        
+          break;
+        
+        // 向后匀速运动
+        case U_KEY_S:
+        
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[0]     -= VEL_XY_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+
+          break;
+
+        // 向左匀速运动
+        case U_KEY_A:
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[1]     += VEL_XY_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+        
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+
+          break;
+
+        // 向右匀速运动
+        case U_KEY_D:
+        
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[1]     -= VEL_XY_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+
+          break;
+
+        // 向上运动
+        case U_KEY_K:
+
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[2]     += VEL_Z_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+
+          break;
+
+        // 向下运动
+        case U_KEY_M:
+
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[2]     -= VEL_Z_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+        
+          break;
+
+        // 偏航运动，左转 （这个里偏航控制的是位置 不是速度）
+        case U_KEY_Q:
+
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.yaw_ref             = YAW_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+          
+          cout << " " <<endl;
+          cout << "Increase the Yaw angle." <<endl;
+
+          sleep(0.1);
+        
+          break;
+
+        // 偏航运动，右转
+        case U_KEY_E:
+
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_POS;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.yaw_ref             = YAW_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+          
+          cout << " " <<endl;
+          cout << "Decrease the Yaw angle." <<endl;
+
+          sleep(0.1);
+        
+          break;
+        
+        // 圆形追踪
+        case U_KEY_9:
+          time_trajectory = 0.0;
+          trajectory_total_time = 50.0;
+          // 需要设置
+          while(time_trajectory < trajectory_total_time)
+          {
+              Command_to_pub.header.stamp = ros::Time::now();
+              Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+              Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+              Command_to_pub.source = NODE_NAME;
+
+              Command_to_pub.Reference_State = Controller_Test.Circle_trajectory_generation(time_trajectory);
+
+              move_pub.publish(Command_to_pub);
+              time_trajectory = time_trajectory + 0.01;
+
+              cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time  << " [ s ]" <<endl;
+
+              Draw_in_rviz(Command_to_pub.Reference_State, true);
+
+              ros::Duration(0.01).sleep();
+          }
+          break;
+
+        // 8字追踪
+        case U_KEY_8:
+          time_trajectory = 0.0;
+          trajectory_total_time = 50.0;
+          // 需要设置
+          while(time_trajectory < trajectory_total_time)
+          {
+              Command_to_pub.header.stamp = ros::Time::now();
+              Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+              Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+              Command_to_pub.source = NODE_NAME;
+
+              Command_to_pub.Reference_State = Controller_Test.Eight_trajectory_generation(time_trajectory);
+
+              move_pub.publish(Command_to_pub);
+              time_trajectory = time_trajectory + 0.01;
+
+              cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time  << " [ s ]" <<endl;
+
+              Draw_in_rviz(Command_to_pub.Reference_State, true);
+
+              ros::Duration(0.01).sleep();
+          }
+          break;
+        }
+
+        key_last = key_now;
+        ros::spinOnce();
+        sleep(0.1);
+    }
+}
+
+
+void generate_com(int Move_mode, float state_desired[4])
+{
+    //# Move_mode 2-bit value:
+    //# 0 for position, 1 for vel, 1st for xy, 2nd for z.
+    //#                   xy position     xy velocity
+    //# z position       	0b00(0)       0b10(2)
+    //# z velocity		0b01(1)       0b11(3)
+
+    if(Move_mode == prometheus_msgs::PositionReference::XYZ_ACC)
+    {
+        cout << "ACC control not support yet." <<endl;
+    }
+
+    if((Move_mode & 0b10) == 0) //xy channel
+    {
+        Command_to_pub.Reference_State.position_ref[0] = state_desired[0];
+        Command_to_pub.Reference_State.position_ref[1] = state_desired[1];
+        Command_to_pub.Reference_State.velocity_ref[0] = 0;
+        Command_to_pub.Reference_State.velocity_ref[1] = 0;
+    }
+    else
+    {
+        Command_to_pub.Reference_State.position_ref[0] = 0;
+        Command_to_pub.Reference_State.position_ref[1] = 0;
+        Command_to_pub.Reference_State.velocity_ref[0] = state_desired[0];
+        Command_to_pub.Reference_State.velocity_ref[1] = state_desired[1];
+    }
+
+    if((Move_mode & 0b01) == 0) //z channel
+    {
+        Command_to_pub.Reference_State.position_ref[2] = state_desired[2];
+        Command_to_pub.Reference_State.velocity_ref[2] = 0;
+    }
+    else
+    {
+        Command_to_pub.Reference_State.position_ref[2] = 0;
+        Command_to_pub.Reference_State.velocity_ref[2] = state_desired[2];
+    }
+
+    Command_to_pub.Reference_State.acceleration_ref[0] = 0;
+    Command_to_pub.Reference_State.acceleration_ref[1] = 0;
+    Command_to_pub.Reference_State.acceleration_ref[2] = 0;
+
+
+    Command_to_pub.Reference_State.yaw_ref = state_desired[3]/180.0*M_PI;
+}
+
+void Draw_in_rviz(const prometheus_msgs::PositionReference& pos_ref, bool draw_trajectory)
+{
+    geometry_msgs::PoseStamped reference_pose;
+
+    reference_pose.header.stamp = ros::Time::now();
+    reference_pose.header.frame_id = "world";
+
+    reference_pose.pose.position.x = pos_ref.position_ref[0];
+    reference_pose.pose.position.y = pos_ref.position_ref[1];
+    reference_pose.pose.position.z = pos_ref.position_ref[2];
+
+    if(draw_trajectory)
+    {
+        posehistory_vector_.insert(posehistory_vector_.begin(), reference_pose);
+        if(posehistory_vector_.size() > TRA_WINDOW){
+            posehistory_vector_.pop_back();
+        }
+        
+        nav_msgs::Path reference_trajectory;
+        reference_trajectory.header.stamp = ros::Time::now();
+        reference_trajectory.header.frame_id = "world";
+        reference_trajectory.poses = posehistory_vector_;
+        ref_trajectory_pub.publish(reference_trajectory);
+    }else
+    {
+        posehistory_vector_.clear();
+        
+        nav_msgs::Path reference_trajectory;
+        reference_trajectory.header.stamp = ros::Time::now();
+        reference_trajectory.header.frame_id = "world";
+        reference_trajectory.poses = posehistory_vector_;
+        ref_trajectory_pub.publish(reference_trajectory);
+    }
+}