YOPO

Controller/src/so3_control/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(so3_control)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  nav_msgs
+  quadrotor_msgs
+  tf
+  nodelet
+  cmake_utils
+)
+
+set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
+
+catkin_package()
+
+find_package(Eigen3 REQUIRED)
+
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(SO3Control src/SO3Control.cpp)
+add_library(so3_control_nodelet src/so3_control_nodelet.cpp)
+
+target_link_libraries(so3_control_nodelet
+  ${catkin_LIBRARIES}
+  SO3Control
+)
+
+# 普通PID控制器
+add_executable(control_example src/control_example.cpp)
+target_link_libraries(control_example
+  ${catkin_LIBRARIES}
+)
+
+# 接收加速度直接转为姿态，并叠加干扰观测器作为内环输入
+add_executable(network_control_node src/NetworkControl.cpp src/network_control_node.cpp)
+target_link_libraries(network_control_node
+  ${catkin_LIBRARIES}
+  SO3Control
+)

Controller/src/so3_control/config/corrections_hummingbird.yaml

@@ -0,0 +1,4 @@
+corrections:
+  z: 0.0
+  r: 0.0
+  p: 0.0

Controller/src/so3_control/config/corrections_pelican.yaml

@@ -0,0 +1,4 @@
+corrections:
+  z: 0.0
+  r: 0.0
+  p: 0.0

Controller/src/so3_control/config/gains.yaml

@@ -0,0 +1,6 @@
+# Gains for Laser-based Pelican 
+gains:
+  pos: {x: 5.0, y: 5.0, z: 15.0}
+  vel: {x: 5.0, y: 5.0, z: 5.0}
+  rot: {x: 3.5, y: 3.5, z: 1.0}
+  ang: {x: 0.4, y: 0.4, z: 0.1}

Controller/src/so3_control/config/gains_hummingbird.yaml

@@ -0,0 +1,6 @@
+# Vision Gain for Hummingbird
+gains:
+  pos: {x: 2.0, y: 2.0, z: 3.5}
+  vel: {x: 1.8, y: 1.8, z: 2.0}
+  rot: {x: 1.0, y: 1.0, z: 0.3}
+  ang: {x: 0.07, y: 0.07, z: 0.02}  

Controller/src/so3_control/config/gains_pelican.yaml

@@ -0,0 +1,6 @@
+# Gains for Laser-based Pelican 
+gains:
+  pos: {x: 5.0, y: 5.0, z: 15.0}
+  vel: {x: 5.0, y: 5.0, z: 5.0}
+  rot: {x: 3.5, y: 3.5, z: 1.0}
+  ang: {x: 0.4, y: 0.4, z: 0.1}

Controller/src/so3_control/include/so3_control/HGDO.h

@@ -0,0 +1,110 @@
+#ifndef HGDO_H_
+#define HGDO_H_
+
+#include "ros/ros.h"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+inline void get_dcm_from_q(Eigen::Matrix3d &dcm, const Eigen::Quaterniond &q) {
+    float a = q.w();
+    float b = q.x();
+    float c = q.y();
+    float d = q.z();
+    float aSq = a*a;
+    float bSq = b*b;
+    float cSq = c*c;
+    float dSq = d*d;
+    dcm(0, 0) = aSq + bSq - cSq - dSq; 
+    dcm(0, 1) = 2 * (b * c - a * d);
+    dcm(0, 2) = 2 * (a * c + b * d);
+    dcm(1, 0) = 2 * (b * c + a * d);
+    dcm(1, 1) = aSq - bSq + cSq - dSq;
+    dcm(1, 2) = 2 * (c * d - a * b);
+    dcm(2, 0) = 2 * (b * d - a * c);
+    dcm(2, 1) = 2 * (a * b + c * d);
+    dcm(2, 2) = aSq - bSq - cSq + dSq;
+}
+
+class HGDO
+{
+public:
+    HGDO(){};
+    HGDO(double control_dt){
+        control_dt_ = control_dt;
+    };
+
+    double Sat(double &Input)
+    {
+        double D = 1.0;
+        double abs_input = abs(Input);
+        if (abs_input <= D)
+        {
+            return Input;
+        }
+        else
+        {
+            return Input / abs_input;
+        }
+    }
+
+    // 用这个,放到控制器的定时器和控制器同步,注意有些全局变量在FxTDO
+    // 模型预测，这个z2_nhgdo作为北西天系，（用来算姿态的）加速度直接加z2_nhgdo作为实际的加速度
+    // U_input: 上一时刻期望加速度（控制量姿态对应的）
+    // vel: 当前时刻的速度
+    // dis: 当前时刻干扰返回值,就是3个方向加速度
+    void HGDO_ext_force_ob(const Eigen::Vector3d &U_input, const Eigen::Vector3d &vel, Eigen::Vector3d &dis)
+    {
+        Eigen::Vector3d dz1, dz2;
+        Eigen::Vector3d U = U_input;
+        // U(2) -= g;
+        for (int i = 0; i < 3; i++)
+        {
+            dz1(i) = U(i) + z2_hgdo(i) - (alpha_1 / theta) * (z1_hgdo(i) - vel(i));
+            dz2(i) = -(alpha_2 / pow(theta, 2)) * (z1_hgdo(i) - vel(i));
+        }
+        for (int i = 0; i < 3; i++)
+        {
+            z1_hgdo(i) += dz1(i) * control_dt_;
+            z2_hgdo(i) += dz2(i) * control_dt_;
+        }
+        dis = z2_hgdo;
+    }
+
+    // 自适应干扰观测，立文讲先不用
+    void AHGDO_ext_force_ob(Eigen::Vector3d &U_input, Eigen::Vector3d &vel, Eigen::Vector3d &dis)
+    {
+        Eigen::Vector3d dz1, dz2;
+        Eigen::Vector3d U = U_input;
+        // U(2) += g;
+        for (int i = 0; i < 3; i++)
+        {
+            double ob = (z1_nhgdo(i) - vel(i)) / d_bound;
+            dz1(i) = U(i) + z2_nhgdo(i) - (alpha_1 / theta_1) * (z1_nhgdo(i) - vel(i)) - (alpha_1 * d_bound * (1 / theta_2 - 1 / theta_1)) * Sat(ob);
+            dz2(i) = -(alpha_2 / pow(theta_1, 2)) * (z1_nhgdo(i) - vel(i)) - (alpha_2 * d_bound * (1 / pow(theta_2, 2) - 1 / pow(theta_2, 2))) * Sat(ob);
+        }
+        for (int i = 0; i < 3; i++)
+        {
+            z1_nhgdo(i) += dz1(i) * control_dt_;
+            z2_nhgdo(i) += dz2(i) * control_dt_;
+        }
+        dis = z2_nhgdo;
+    }
+
+private:
+    double g = 9.81;
+    double control_dt_{0.02};
+
+    Eigen::Vector3d z1_hgdo = Eigen::Vector3d::Zero();
+    Eigen::Vector3d z2_hgdo = Eigen::Vector3d::Zero();
+    Eigen::Vector3d z1_nhgdo = Eigen::Vector3d::Zero();
+    Eigen::Vector3d z2_nhgdo = Eigen::Vector3d::Zero();
+
+    double theta = 0.2;
+    double theta_1 = 0.2;
+    double theta_2 = 0.5;
+    double alpha_1 = 3.0;
+    double alpha_2 = 2.0;
+    double d_bound = 0.01;
+};
+
+#endif

Controller/src/so3_control/include/so3_control/NetworkControl.h

@@ -0,0 +1,156 @@
+#ifndef NETWORK_CONTROL_H_
+#define NETWORK_CONTROL_H_
+
+#include <Eigen/Eigen>
+#include <math.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Imu.h>
+#include <quadrotor_msgs/PositionCommand.h>
+#include <quadrotor_msgs/SO3Command.h>
+#include <tf/transform_datatypes.h>
+#include <ros/ros.h>
+#include <so3_control/SO3Control.h>
+#include <so3_control/HGDO.h>
+#include <so3_control/mavros_interface.h>
+#include <quadrotor_msgs/SetTakeoffLand.h>
+#include <string>
+#include <iostream>
+#include <boost/filesystem.hpp>
+#include <regex>
+#include <fstream>
+#include <thread>
+#include <mutex>
+#include <algorithm> 
+
+#define ONE_G 9.81
+
+class NetworkControl
+{
+public:
+    NetworkControl(ros::NodeHandle &node){
+        nh_ = node;
+
+        so3_controller_.setMass(mass_);
+        disturbance_observer_ = HGDO(control_dt_);
+
+        nh_.param("is_simulation", is_simulation_, false);
+        nh_.param("use_disturbance_observer", use_disturbance_observer_, false);
+        nh_.param("hover_thrust", hover_thrust_, 0.4);
+        nh_.param("kx_xy", kx_xy, 5.7);
+        nh_.param("kx_z", kx_z, 6.2);
+        nh_.param("kv_xy", kv_xy, 3.4);
+        nh_.param("kv_z", kv_z, 4.0);
+        nh_.param("record_log", record_log_, false);
+        nh_.param("logger_file_name", logger_file_name, std::string("/home/lu/"));
+        printf("kx: (%f, %f, %f), kv: (%f, %f, %f) \n", kx_xy, kx_xy, kx_z, kv_xy, kv_xy, kv_z);
+
+        so3_command_pub_ = nh_.advertise<quadrotor_msgs::SO3Command>("so3_cmd", 10);
+        position_cmd_sub_ = nh_.subscribe("position_cmd", 1, &NetworkControl::network_cmd_callback, this, ros::TransportHints().tcpNoDelay());
+        odom_sub_ = nh_.subscribe("odom", 1, &NetworkControl::odom_callback, this, ros::TransportHints().tcpNoDelay());
+        imu_sub_ = nh_.subscribe("imu", 1, &NetworkControl::imu_callback, this, ros::TransportHints().tcpNoDelay());
+
+        takeoff_land_control_timer = nh_.createTimer(ros::Duration(control_dt_), &NetworkControl::timerCallback, this);
+
+        takeoff_land_srv = nh_.advertiseService("takeoff_land", &NetworkControl::takeoff_land_srv_handle, this);
+
+        if (is_simulation_) {
+            ros::Duration(2.0).sleep();
+            std::thread(&NetworkControl::simulateTakeoff, this).detach();
+        }
+        
+    };
+
+    ~NetworkControl(){};
+
+private:
+    ros::NodeHandle nh_;
+    ros::Publisher so3_command_pub_;
+    ros::Subscriber position_cmd_sub_, odom_sub_, imu_sub_;
+    ros::ServiceServer takeoff_land_srv;
+    ros::Timer takeoff_land_control_timer;
+    std::mutex mutex_;
+
+    double mass_ = 0.98;
+    double control_dt_ = 0.02;
+    double hover_thrust_ = 0.4;
+    double kx_xy, kx_z, kv_xy, kv_z;
+    
+    double cur_yaw_ = 0;
+    Eigen::Vector3d cur_pos_ = Eigen::Vector3d(0, 0, 0);
+    Eigen::Vector3d cur_vel_ = Eigen::Vector3d(0, 0, 0);
+    Eigen::Vector3d cur_acc_ = Eigen::Vector3d(0, 0, 0);
+    Eigen::Quaterniond cur_att_ = Eigen::Quaterniond::Identity();
+    Eigen::Vector3d dis_acc_ = Eigen::Vector3d(0, 0, 0);
+    Eigen::Vector3d last_des_acc_ = Eigen::Vector3d(0, 0, 0);
+    double last_thrust_ = 0;
+
+    Eigen::Vector3d des_pos_ = Eigen::Vector3d(0, 0, 0);
+    Eigen::Vector3d des_vel_ = Eigen::Vector3d(0, 0, 0);
+    Eigen::Vector3d des_acc_ = Eigen::Vector3d(0, 0, 0);
+    double des_yaw_ = 0;
+    double des_yaw_dot_ = 0;
+
+    bool is_simulation_ = false;
+    bool state_init_ = false;
+    bool ref_valid_ = false;
+    bool ctrl_valid_ = false;
+    bool position_cmd_init_ = false;
+    bool takeoff_cmd_init_ = false;
+    bool use_disturbance_observer_ = false;
+    bool record_log_ = false;
+    
+    SO3Control so3_controller_;
+    HGDO disturbance_observer_;
+    Mavros_Interface mavros_interface_;
+    
+    std::ofstream logger;
+    std::string logger_file_name;
+
+    void initLogRecorder();
+
+    void recordLog(Eigen::Vector3d &cur_v, Eigen::Vector3d &cur_a, Eigen::Vector3d &des_a, Eigen::Vector3d &dis_a, double cur_yaw, double des_yaw);
+
+    Eigen::Vector3d publishHoverSO3Command(Eigen::Vector3d des_pos_, Eigen::Vector3d des_vel_, Eigen::Vector3d des_acc_, double des_yaw_, double des_yaw_dot_);
+
+    void get_Q_from_ACC(const Eigen::Vector3d &ref_acc, double ref_yaw, Eigen::Quaterniond &quat_des, Eigen::Vector3d &force_des);
+
+    void pub_SO3_command(Eigen::Vector3d ref_acc, double ref_yaw, double cur_yaw);
+
+    void limite_acc(Eigen::Vector3d &acc);
+
+    void network_cmd_callback(const quadrotor_msgs::PositionCommand::ConstPtr &cmd);
+
+    void odom_callback(const nav_msgs::Odometry::ConstPtr &odom);
+
+    void imu_callback(const sensor_msgs::Imu &imu);
+
+    void timerCallback(const ros::TimerEvent&);
+
+    // mavros interface
+    bool takeoff_land_srv_handle(quadrotor_msgs::SetTakeoffLand::Request &req,
+                                 quadrotor_msgs::SetTakeoffLand::Response &res){
+        std::thread t(&NetworkControl::takeoff_land_thread, this, std::ref(req));
+        t.detach();
+        res.res = true;
+        return true;
+    }
+
+    bool arm_disarm_vehicle(bool arm);
+
+    void takeoff_land_thread(quadrotor_msgs::SetTakeoffLand::Request &req);
+
+    void simulateTakeoff() {
+        ros::ServiceClient client = nh_.serviceClient<quadrotor_msgs::SetTakeoffLand>("takeoff_land");
+        quadrotor_msgs::SetTakeoffLand srv;
+        srv.request.takeoff = true;
+        srv.request.takeoff_altitude = 2.0;
+    
+        if (client.call(srv)) {
+            ROS_INFO("Takeoff called successfully");
+        } else {
+            ROS_ERROR("Failed to call takeoff service");
+        }
+    }
+};
+
+#endif

Controller/src/so3_control/include/so3_control/SO3Control.h

@@ -0,0 +1,42 @@
+#ifndef __SO3_CONTROL_H__
+#define __SO3_CONTROL_H__
+
+#include <Eigen/Geometry>
+
+class SO3Control
+{
+public:
+  SO3Control();
+
+  void setMass(const double mass);
+  void setGravity(const double g);
+  void setPosition(const Eigen::Vector3d& position);
+  void setVelocity(const Eigen::Vector3d& velocity);
+  void setAcc(const Eigen::Vector3d& acc);
+
+  void calculateControl(const Eigen::Vector3d& des_pos,
+                        const Eigen::Vector3d& des_vel,
+                        const Eigen::Vector3d& des_acc, const double des_yaw,
+                        const double des_yaw_dot, const Eigen::Vector3d& kx,
+                        const Eigen::Vector3d& kv);
+
+  const Eigen::Vector3d&    getComputedForce(void);
+  const Eigen::Quaterniond& getComputedOrientation(void);
+
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+private:
+  // Inputs for the controller
+  double          mass_;
+  double          g_;
+  Eigen::Vector3d pos_;
+  Eigen::Vector3d vel_;
+  Eigen::Vector3d acc_;
+
+  // Outputs of the controller
+  Eigen::Vector3d    force_;
+  Eigen::Quaterniond orientation_;
+};
+
+#endif

Controller/src/so3_control/include/so3_control/mavros_interface.h

@@ -0,0 +1,194 @@
+#ifndef MAVROS_INTERFACE_H_
+#define MAVROS_INTERFACE_H_
+
+#include "ros/ros.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
+#include <iostream>
+
+#include <mavros_msgs/AttitudeTarget.h>
+#include <mavros_msgs/State.h>
+#include <mavros_msgs/SetMode.h>
+#include <mavros_msgs/CommandBool.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+class Mavros_Interface
+{
+public:
+    Mavros_Interface() : _nh("~cmd")
+    {
+        _state.reset();
+        std::string base_name = "/mavros";
+        // char id_str[10];
+        // sprintf(id_str, "%d", id);
+        // base_name += id_str;
+
+        std::string att_target_pub_name;
+        att_target_pub_name = base_name + "/setpoint_raw/attitude";
+        att_target_pub = _nh.advertise<mavros_msgs::AttitudeTarget>(att_target_pub_name.c_str(), 10);
+
+        std::string state_sub_name;
+        state_sub_name = base_name + "/state";
+        state_sub = _nh.subscribe(state_sub_name.c_str(), 10, &Mavros_Interface::state_cb, this);
+
+        std::string set_mode_s_name;
+        set_mode_s_name = base_name + "/set_mode";
+        set_mode_client = _nh.serviceClient<mavros_msgs::SetMode>(set_mode_s_name.c_str());
+
+        std::string arm_disarm_s_name;
+        arm_disarm_s_name = base_name + "/cmd/arming";
+        arm_disarm_client = _nh.serviceClient<mavros_msgs::CommandBool>(arm_disarm_s_name.c_str());
+    }
+
+    ~Mavros_Interface() {}
+
+    typedef struct mavros_state_t
+    {
+        ros::Time header;
+        bool has_armed;
+        bool offboard_enabled;
+        void reset()
+        {
+            has_armed = false;
+            offboard_enabled = false;
+        }
+        mavros_state_t()
+        {
+            reset();
+        }
+    };
+
+    void state_cb(const mavros_msgs::State &state_data)
+    {
+        mavros_msgs::State temp_data = state_data;
+        _state.header = state_data.header.stamp;
+        _state.has_armed = state_data.armed;
+        if (state_data.mode == "OFFBOARD")
+        {
+            _state.offboard_enabled = true;
+        }
+        else
+        {
+            _state.offboard_enabled = false;
+        }
+    }
+
+    void get_status(bool &arm_state, bool &offboard_enabled)
+    {
+        arm_state = _state.has_armed;
+        offboard_enabled = _state.offboard_enabled;
+    }
+
+    bool set_arm_and_offboard()
+    {
+        ros::Rate _ofb_check_rate(1);
+        int try_arm_ofb_times = 0;
+        while (!_state.offboard_enabled || !_state.has_armed)
+        {
+            if (_state.offboard_enabled)
+            {
+                ros::Rate _arm_check_rate(1);
+                while (!_state.has_armed)
+                {
+                    mavros_msgs::CommandBool arm_srv;
+                    arm_srv.request.value = true;
+                    if (arm_disarm_client.call(arm_srv))
+                    {
+                        ROS_INFO("vehicle ARMED");
+                    }
+                    try_arm_ofb_times = try_arm_ofb_times + 1;
+                    if (try_arm_ofb_times >= 3)
+                    {
+                        ROS_ERROR("try 3 times, cannot armed uav, give up!");
+                        return false;
+                    }
+                    _arm_check_rate.sleep();
+                }
+            }
+            else
+            {
+                ROS_INFO("not in OFFBOARD mode");
+                mavros_msgs::SetMode set_mode_srv;
+                set_mode_srv.request.base_mode = 0;
+                set_mode_srv.request.custom_mode = "OFFBOARD";
+                if (!set_mode_client.call(set_mode_srv))
+                {
+                    return false;
+                }
+                ROS_INFO("switch to OFFBOARD mode");
+                _ofb_check_rate.sleep();
+            }
+        }
+        return true;
+    }
+
+    /* void set_arm(const ros::TimerEvent& event) {
+        if(_state.offboard_enabled) {
+            mavros_msgs::CommandBool arm_srv;
+            arm_srv.request.value = true;
+            if (arm_disarm_client.call(arm_srv)) {
+                ROS_INFO("vehicle ARMED");
+            }
+        } else {
+            ROS_INFO("not in OFFBOARD mode");
+        }
+    } */
+
+    bool set_disarm()
+    {
+        ros::Rate _arm_check_rate(1);
+        while (_state.has_armed)
+        {
+            mavros_msgs::CommandBool arm_srv;
+            arm_srv.request.value = false;
+            if (!arm_disarm_client.call(arm_srv))
+            {
+                return false;
+            }
+            ROS_INFO("vehicle DISARMED");
+            _arm_check_rate.sleep();
+        }
+        return true;
+    }
+
+    void pub_att_thrust_cmd(const Eigen::Quaterniond &q_d, const double &thrust_d)
+    {
+        /*
+            目前mavros用的是北东地的坐标系，为了和pid通用所以没有改mavros而是在这里改为北东地
+        */
+        mavros_msgs::AttitudeTarget at_cmd;
+        at_cmd.header.stamp = ros::Time::now();
+        at_cmd.type_mask = at_cmd.IGNORE_ROLL_RATE | at_cmd.IGNORE_PITCH_RATE | at_cmd.IGNORE_YAW_RATE;
+        at_cmd.thrust = (float)thrust_d;
+        at_cmd.orientation.w = q_d.w();
+        at_cmd.orientation.x = q_d.x();
+        at_cmd.orientation.y = -q_d.y();
+        at_cmd.orientation.z = -q_d.z();
+        att_target_pub.publish(at_cmd);
+    }
+
+    // for simulation
+    void set_arm_and_offboard_manually()
+    {
+        _state.has_armed = true;
+        _state.offboard_enabled = true;
+    }
+
+    void set_disarm_manually()
+    {
+        _state.has_armed = false;
+    }
+
+private:
+    ros::NodeHandle _nh;
+    ros::Publisher att_target_pub;
+    ros::Subscriber state_sub;
+    ros::ServiceClient set_mode_client;
+    ros::ServiceClient arm_disarm_client;
+    mavros_state_t _state;
+};
+
+#endif

Controller/src/so3_control/launch/controller_network.launch

@@ -0,0 +1,19 @@
+<launch>
+    <arg name="hover_thrust" default="0.38" />
+
+    <node pkg="so3_control" type="network_control_node" name="network_controller_node" output="screen">
+        <param name="is_simulation" value="false"/>
+        <param name="use_disturbance_observer" value="true"/>
+        <param name="hover_thrust" value="$(arg hover_thrust)"/>
+        <param name="kx_xy" value="5.7"/>
+        <param name="kx_z" value="6.2"/>
+        <param name="kv_xy" value="3.4"/>
+        <param name="kv_z" value="4.0"/>
+        <remap from="~odom" to="/vins_estimator/imu_propagate"/>
+        <remap from="~imu" to="/mavros/imu/data_raw"/>
+        <remap from="~position_cmd" to="/so3_control/pos_cmd"/>
+        <remap from="~so3_cmd" to="so3_cmd"/>
+        <param name="record_log" value = "true"/>
+        <param name="logger_file_name" value = "$(find so3_control)/logger/"/>   
+    </node>
+</launch>

Controller/src/so3_control/logger/log_plot.py

@@ -0,0 +1,28 @@
+import sys
+import numpy as np
+import matplotlib.pyplot as plt
+
+if __name__ == '__main__':
+    file_path = sys.argv[1]
+    temp = np.loadtxt(file_path, dtype=str, delimiter=",")
+    print('load file:', file_path)
+    label = temp[0, :].astype(str)
+    data = temp[1:, :].astype(np.float64)
+    timestamp = (data[:, 0] - data[0, 0])
+    print('data length:', len(data))
+    # print(timestamp)
+    while 1:
+        for i in range(len(label)):
+            if i != 0:
+                print('[', i, ']: ', label[i])
+        print('plot the curve')
+        print('example: 1 2 3')
+        wtp = input('want to plot(\'q\' to quit):')
+        if wtp == 'q':
+            break
+        wtp = [int(n) for n in wtp.split()]
+        for i in range(len(wtp)):
+            plt.plot(timestamp, data[:, wtp[i]], linewidth=2.5, linestyle='-', marker='.', label=label[wtp[i]])
+        plt.xlabel('t')
+        plt.legend()
+        plt.show()

Controller/src/so3_control/mainpage.dox

@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b so3_control 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/

Controller/src/so3_control/nodelet_plugin.xml

@@ -0,0 +1,7 @@
+<library path="lib/libso3_control_nodelet">
+  <class name="so3_control/SO3ControlNodelet" type="SO3ControlNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      so3_control
+    </description>
+  </class>
+</library>

Controller/src/so3_control/package.xml

@@ -0,0 +1,35 @@
+<package>
+  <version>0.0.0</version>
+  <name>so3_control</name>
+  <description >
+
+     so3_control
+
+  </description>
+  <maintainer email="todo@todo.todo">Kartik Mohta</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/so3_control</url>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>quadrotor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>cmake_utils</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>quadrotor_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>cmake_utils</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugin.xml"/>
+  </export>
+</package>
+
+

Controller/src/so3_control/src/NetworkControl.cpp

@@ -0,0 +1,437 @@
+#include "so3_control/NetworkControl.h"
+
+void NetworkControl::initLogRecorder()
+{   
+    // Use file count as name to avoid date confusion
+    std::cout << "logger_file_name: " << logger_file_name << std::endl;
+    int max_number = -1;
+    boost::filesystem::path dir(logger_file_name);
+    if (boost::filesystem::exists(dir) && boost::filesystem::is_directory(dir)) {
+        std::regex filename_pattern(R"(^(\d+)_.*$)"); // 匹配以数字开头，并以"_"分隔的文件名
+        for (const auto& entry : boost::filesystem::directory_iterator(dir)) {
+            if (boost::filesystem::is_regular_file(entry)) {
+                std::string filename = entry.path().filename().string();
+                std::smatch match;
+                if (std::regex_match(filename, match, filename_pattern)) {
+                    int number = std::stoi(match[1]); // 提取第一个"_"前的数字
+                    max_number = std::max(max_number, number);
+                }
+            }
+        }
+    } else {
+        std::cerr << "Error: invalid logger path" << std::endl;
+    }
+    std::string fileCountStr = std::to_string(max_number + 1);
+    std::string temp_file_name = logger_file_name + fileCountStr + "_Net_logger_";
+    time_t timep;
+    timep = time(0);
+    char tmp[64];
+    strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S", localtime(&timep));
+    temp_file_name += tmp;
+    temp_file_name += ".csv";
+    if (logger.is_open())
+    {
+        logger.close();
+    }
+    logger.open(temp_file_name.c_str(), std::ios::out);
+    std::cout << "logger: " << temp_file_name << std::endl;
+    if (!logger.is_open())
+    {
+        std::cout << "cannot open the logger." << std::endl;
+    }
+    else
+    {
+        logger << "timestamp" << ',';
+        logger << "cur_px" << ',';
+        logger << "cur_py" << ',';
+        logger << "cur_pz" << ',';
+        logger << "cur_vx" << ',';
+        logger << "cur_vy" << ',';
+        logger << "cur_vz" << ',';
+        logger << "cur_ax" << ',';
+        logger << "cur_ay" << ',';
+        logger << "cur_az" << ',';
+        logger << "des_px" << ',';
+        logger << "des_py" << ',';
+        logger << "des_pz" << ',';
+        logger << "des_vx" << ',';
+        logger << "des_vy" << ',';
+        logger << "des_vz" << ',';
+        logger << "des_ax" << ',';
+        logger << "des_ay" << ',';
+        logger << "des_az" << ',';
+        logger << "dis_ax" << ',';
+        logger << "dis_ay" << ',';
+        logger << "dis_az" << ',';
+        logger << "px4_ax" << ',';
+        logger << "px4_ay" << ',';
+        logger << "px4_az" << ',';
+        logger << "thrust" << ',';
+        logger << "cur_yaw" << ',';
+        logger << "des_yaw" << std::endl;
+    }
+}
+
+void NetworkControl::recordLog(Eigen::Vector3d &cur_v, Eigen::Vector3d &cur_a, Eigen::Vector3d &des_a, Eigen::Vector3d &dis_a, double cur_yaw, double des_yaw)
+{
+    if (logger.is_open())
+    {
+        logger << ros::Time::now().toNSec() << ',';
+        logger << cur_pos_(0) << ',';
+        logger << cur_pos_(1) << ',';
+        logger << cur_pos_(2) << ',';
+        logger << cur_v(0) << ',';
+        logger << cur_v(1) << ',';
+        logger << cur_v(2) << ',';
+        logger << cur_a(0) << ',';
+        logger << cur_a(1) << ',';
+        logger << cur_a(2) << ',';
+        logger << des_pos_(0) << ',';
+        logger << des_pos_(1) << ',';
+        logger << des_pos_(2) << ',';
+        logger << des_vel_(0) << ',';
+        logger << des_vel_(1) << ',';
+        logger << des_vel_(2) << ',';
+        logger << des_a(0) << ',';
+        logger << des_a(1) << ',';
+        logger << des_a(2) << ',';
+        logger << dis_a(0) << ',';
+        logger << dis_a(1) << ',';
+        logger << dis_a(2) << ',';
+        logger << des_a(0) - dis_a(0) << ',';
+        logger << des_a(1) - dis_a(1) << ',';
+        logger << des_a(2) - dis_a(2) << ',';
+        logger << last_thrust_ << ',';
+        logger << cur_yaw << ',';
+        logger << des_yaw << std::endl;
+    }
+}
+
+Eigen::Vector3d NetworkControl::publishHoverSO3Command(Eigen::Vector3d des_pos, Eigen::Vector3d des_vel, 
+                                                       Eigen::Vector3d des_acc, double des_yaw, double des_yaw_dot)
+{
+    Eigen::Vector3d kx(kx_xy, kx_xy, kx_z);
+    Eigen::Vector3d kv(kv_xy, kv_xy, kv_z);
+    so3_controller_.calculateControl(des_pos, des_vel, des_acc, des_yaw, des_yaw_dot, kx, kv);
+
+    Eigen::Vector3d force = so3_controller_.getComputedForce();
+    Eigen::Quaterniond orientation = so3_controller_.getComputedOrientation();
+
+    quadrotor_msgs::SO3Command::Ptr so3_command(new quadrotor_msgs::SO3Command); //! @note memory leak?
+    so3_command->header.stamp = ros::Time::now();
+    so3_command->force.x = force(0);
+    so3_command->force.y = force(1);
+    so3_command->force.z = force(2);
+    so3_command->orientation.x = orientation.x();
+    so3_command->orientation.y = orientation.y();
+    so3_command->orientation.z = orientation.z();
+    so3_command->orientation.w = orientation.w();
+    so3_command->kR[0] = 1.5;
+    so3_command->kR[1] = 1.5;
+    so3_command->kR[2] = 1.0;
+    so3_command->kOm[0] = 0.13;
+    so3_command->kOm[1] = 0.13;
+    so3_command->kOm[2] = 0.1;
+    so3_command->aux.current_yaw = cur_yaw_;
+    so3_command->aux.enable_motors = true;
+    so3_command_pub_.publish(so3_command);
+
+    double thrust_norm = force.norm() / (mass_ * ONE_G) * hover_thrust_;
+    mavros_interface_.pub_att_thrust_cmd(orientation, thrust_norm);
+    last_thrust_ = thrust_norm;
+
+    double thrust = force.norm() / mass_;
+    Eigen::Matrix3d Cbn;    
+    get_dcm_from_q(Cbn, orientation);
+    Eigen::Vector3d att_acc = Eigen::Vector3d(0, 0, thrust);
+    att_acc = Cbn * att_acc;
+    att_acc(2) -= ONE_G;
+    // std::cout<<"att_acc"<<att_acc.transpose()<<std::endl;
+    return att_acc;
+}
+
+void NetworkControl::get_Q_from_ACC(const Eigen::Vector3d &ref_acc, double ref_yaw, Eigen::Quaterniond &quat_des, Eigen::Vector3d &force_des)
+{
+    Eigen::Vector3d force_ = mass_ * ONE_G * Eigen::Vector3d(0, 0, 1);
+    force_.noalias() += mass_ * ref_acc;
+
+    // Limit control angle to theta degree
+    double theta = M_PI / 4;
+    double c = cos(theta);
+    Eigen::Vector3d f;
+    f.noalias() = force_ - mass_ * ONE_G * Eigen::Vector3d(0, 0, 1);
+    if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c)
+    {
+        double nf = f.norm();
+        double A = c * c * nf * nf - f(2) * f(2);
+        double B = 2 * (c * c - 1) * f(2) * mass_ * ONE_G;
+        double C = (c * c - 1) * mass_ * mass_ * ONE_G * ONE_G;
+        double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
+        force_.noalias() = s * f + mass_ * ONE_G * Eigen::Vector3d(0, 0, 1);
+    }
+
+    Eigen::Vector3d b1c, b2c, b3c;
+    Eigen::Vector3d b1d(cos(ref_yaw), sin(ref_yaw), 0);
+
+    if (force_.norm() > 1e-6)
+        b3c.noalias() = force_.normalized();
+    else
+        b3c.noalias() = Eigen::Vector3d(0, 0, 1);
+
+    b2c.noalias() = b3c.cross(b1d).normalized();
+    b1c.noalias() = b2c.cross(b3c).normalized();
+
+    Eigen::Matrix3d R;
+    R << b1c, b2c, b3c;
+
+    quat_des = Eigen::Quaterniond(R);
+    force_des = force_;
+
+}
+
+// 世界系的期望加速度：ref_acc（加上g）、期望yaw：ref_yaw
+void NetworkControl::pub_SO3_command(Eigen::Vector3d ref_acc, double ref_yaw, double cur_yaw)
+{
+    Eigen::Vector3d force;
+    Eigen::Quaterniond quat_des;
+    get_Q_from_ACC(ref_acc, ref_yaw, quat_des, force);
+    quadrotor_msgs::SO3Command::Ptr so3_command(new quadrotor_msgs::SO3Command);
+    so3_command->header.stamp = ros::Time::now();
+    so3_command->force.x = force(0);
+    so3_command->force.y = force(1);
+    so3_command->force.z = force(2);
+    so3_command->orientation.x = quat_des.x();
+    so3_command->orientation.y = quat_des.y();
+    so3_command->orientation.z = quat_des.z();
+    so3_command->orientation.w = quat_des.w();
+    so3_command->kR[0] = 1.5;
+    so3_command->kR[1] = 1.5;
+    so3_command->kR[2] = 1.0;
+    so3_command->kOm[0] = 0.13;
+    so3_command->kOm[1] = 0.13;
+    so3_command->kOm[2] = 0.1;
+    so3_command->aux.current_yaw = cur_yaw;
+    so3_command->aux.enable_motors = true;
+    so3_command_pub_.publish(so3_command);
+
+    double thrust_norm = force.norm() / (mass_ * ONE_G) * hover_thrust_;
+    mavros_interface_.pub_att_thrust_cmd(quat_des, thrust_norm);
+    last_thrust_ = thrust_norm;
+}
+
+void NetworkControl::limite_acc(Eigen::Vector3d &acc){
+    return;  // limited if needed
+    acc[0] = std::max(-8.0, std::min(acc[0], 8.0));
+    acc[1] = std::max(-8.0, std::min(acc[1], 8.0));
+    acc[2] = std::max(-4.0, std::min(acc[2], 4.0));
+}
+
+void NetworkControl::network_cmd_callback(const quadrotor_msgs::PositionCommand::ConstPtr &cmd)
+{
+    if (!ctrl_valid_)
+        return;
+
+    bool arm_state = false;
+    bool ofb_enable = false;
+    mavros_interface_.get_status(arm_state, ofb_enable);
+    if (!arm_state || !ofb_enable)
+        return;
+
+    position_cmd_init_ = true;
+
+    Eigen::Vector3d des_acc = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z);
+    limite_acc(des_acc);
+
+    double des_yaw = cmd->yaw;
+    
+    disturbance_observer_.HGDO_ext_force_ob(last_des_acc_, cur_vel_, dis_acc_);
+    // ROS_INFO_THROTTLE(0.5, "dis_acc: %.3f, %.3f, %.3f", dis_acc_.x(), dis_acc_.y(), dis_acc_.z());
+    // std::cout << "dis_acc: " << dis_acc_.transpose() << std::endl;
+
+    Eigen::Vector3d att_acc;
+    if (cmd->trajectory_flag == quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY)
+    {    
+        if (use_disturbance_observer_)
+            att_acc = des_acc - dis_acc_;
+        else
+            att_acc = des_acc;
+        pub_SO3_command(att_acc, des_yaw, cur_yaw_);
+        // std::cout<<"acc: "<<des_acc.transpose()<<"   yaw:"<<des_yaw<<std::endl;
+        if (record_log_)
+            recordLog(cur_vel_, cur_acc_, des_acc, dis_acc_, cur_yaw_, des_yaw);
+    }
+    else
+    {
+        Eigen::Vector3d des_pos = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
+        Eigen::Vector3d des_vel = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
+        double des_yaw = cmd->yaw;
+        double des_yaw_dot = cmd->yaw_dot;
+        att_acc = publishHoverSO3Command(des_pos, des_vel, des_acc, des_yaw, des_yaw_dot);
+        if (record_log_)
+            recordLog(cur_vel_, cur_acc_, att_acc, dis_acc_, cur_yaw_, des_yaw);
+    }
+
+    last_des_acc_ = att_acc;
+}
+
+void NetworkControl::odom_callback(const nav_msgs::Odometry::ConstPtr &odom)
+{
+    cur_yaw_ = tf::getYaw(odom->pose.pose.orientation);
+    cur_vel_ = Eigen::Vector3d(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z);
+
+    cur_pos_ = Eigen::Vector3d(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z);
+    cur_att_.w() = odom->pose.pose.orientation.w;
+    cur_att_.x() = odom->pose.pose.orientation.x;
+    cur_att_.y() = odom->pose.pose.orientation.y;
+    cur_att_.z() = odom->pose.pose.orientation.z;
+
+    // if(!is_simulation_)
+    //     cur_acc_ = Eigen::Vector3d(odom->twist.twist.angular.x, odom->twist.twist.angular.y, odom->twist.twist.angular.z);
+
+    so3_controller_.setPosition(cur_pos_);
+    so3_controller_.setVelocity(cur_vel_);
+    if (!state_init_)
+        ROS_INFO("Odom Recived! Ready to TakeOff...");
+    state_init_ = true;
+}
+
+void NetworkControl::imu_callback(const sensor_msgs::Imu &imu)
+{
+    Eigen::Vector3d acc(imu.linear_acceleration.x,
+                        imu.linear_acceleration.y,
+                        imu.linear_acceleration.z);
+    Eigen::Vector3d acc_world = cur_att_ * acc;
+    acc_world(2) -= 9.8;
+    cur_acc_ = acc_world;
+    // so3_controller_.setAcc(acc_world);
+}
+
+void NetworkControl::timerCallback(const ros::TimerEvent &)
+{
+    if (!state_init_ || !ref_valid_)
+        return;
+    if (position_cmd_init_ && ctrl_valid_)
+        return;
+
+    mutex_.lock();
+    Eigen::Vector3d des_pos_temp = des_pos_;
+    mutex_.unlock();
+
+    Eigen::Vector3d att_acc = publishHoverSO3Command(des_pos_temp, des_vel_, des_acc_, des_yaw_, des_yaw_dot_);
+
+    if (takeoff_cmd_init_)
+    {
+        disturbance_observer_.HGDO_ext_force_ob(last_des_acc_, cur_vel_, dis_acc_);
+        // ROS_INFO_THROTTLE(1.0, " dis_acc: (%f, %f, %f)", dis_acc_.x(), dis_acc_.y(), dis_acc_.z());
+    }
+
+    last_des_acc_ = att_acc;
+    if (record_log_)
+        recordLog(cur_vel_, cur_acc_, att_acc, dis_acc_, cur_yaw_, des_yaw_);
+    takeoff_cmd_init_ = true;
+}
+
+void NetworkControl::takeoff_land_thread(quadrotor_msgs::SetTakeoffLand::Request &req)
+{
+    mutex_.lock();
+    float takeoff_altitude = req.takeoff_altitude;
+    des_pos_ = cur_pos_;
+    des_pos_(2) -= 0.2;
+    des_yaw_ = cur_yaw_;
+    mutex_.unlock();
+    ref_valid_ = true;
+
+    if (req.takeoff)
+    {
+        std::cout << "takeoff process start" << std::endl;
+        if (!arm_disarm_vehicle(true))
+        {
+            std::cout << "Service failed because cannot Arm!" << std::endl;
+            return;
+        }
+        sleep(1);
+
+        double takeoff_vel = 0.8;
+        double takeoff_ddz = takeoff_vel * control_dt_;
+        ros::Rate takeoff_loop(1 / control_dt_);
+        std::cout << "takeoff altitude: " << takeoff_altitude << " m" << std::endl;
+        std::cout << "takeoff velocity: " << takeoff_vel << " m/s" << std::endl;
+        ros::Time start_takeoff_task_time = ros::Time::now();
+        while (ros::ok() && ros::Time::now() - start_takeoff_task_time < ros::Duration(8.0))
+        {       
+            mutex_.lock();
+            des_pos_(2) += takeoff_ddz;
+            mutex_.unlock();
+
+            if (des_pos_(2) > takeoff_altitude)
+            {
+                ROS_INFO("TakeOff Done! Ready to Flight...");
+                ctrl_valid_ = true;
+                break;
+            }
+            takeoff_loop.sleep();
+        }
+    }
+    else
+    {
+        ctrl_valid_ = false;
+        double land_vel = -0.4;
+        double land_ddz = land_vel * control_dt_;
+        ros::Rate land_loop(1 / control_dt_);
+        ros::Time start_land_task_time = ros::Time::now();
+        while (ros::ok() && ros::Time::now() - start_land_task_time < ros::Duration(8.0))
+        {
+            mutex_.lock();
+            des_pos_(2) += land_ddz;
+            mutex_.unlock();
+
+            if (fabs(cur_pos_(2)) < 0.1f && fabs(cur_vel_(2)) < 1.0f)
+            {
+                ROS_INFO("detect land: disarm");
+                arm_disarm_vehicle(false);
+                break;
+            }
+            land_loop.sleep();
+        }
+    }
+    ROS_INFO("take off thread out");
+    return;
+}
+
+bool NetworkControl::arm_disarm_vehicle(bool arm)
+{
+    if (arm)
+    {   
+        if (!state_init_){
+            ROS_WARN("State timeout, will not arm!");
+            return false;
+        }
+
+        ROS_INFO("UAV will be armed!");
+        if (is_simulation_)
+            mavros_interface_.set_arm_and_offboard_manually();
+        else if (mavros_interface_.set_arm_and_offboard())
+            ROS_INFO("Arm done!");
+        else{
+            ROS_ERROR("Arm failure!");
+            return false;
+        }
+        if (record_log_)
+            initLogRecorder();
+    }
+    else
+    {
+        ROS_INFO("UAV will be disarmed!");
+        if (is_simulation_)
+            mavros_interface_.set_disarm_manually();
+        else if (mavros_interface_.set_disarm())
+            ROS_INFO("Disarm done!");
+        else {
+            ROS_ERROR("Disarm failure!");
+            return false;
+        }
+        if (record_log_)
+            logger.close();
+    }
+    return true;
+}

Controller/src/so3_control/src/SO3Control.cpp

@@ -0,0 +1,124 @@
+#include <iostream>
+#include <so3_control/SO3Control.h>
+
+#include <ros/ros.h>
+
+SO3Control::SO3Control()
+  : mass_(0.5)
+  , g_(9.81)
+{
+  acc_.setZero();
+}
+
+void
+SO3Control::setMass(const double mass)
+{
+  mass_ = mass;
+}
+
+void
+SO3Control::setGravity(const double g)
+{
+  g_ = g;
+}
+
+void
+SO3Control::setPosition(const Eigen::Vector3d& position)
+{
+  pos_ = position;
+}
+
+void
+SO3Control::setVelocity(const Eigen::Vector3d& velocity)
+{
+  vel_ = velocity;
+}
+
+void
+SO3Control::calculateControl(const Eigen::Vector3d& des_pos,
+                             const Eigen::Vector3d& des_vel,
+                             const Eigen::Vector3d& des_acc,
+                             const double des_yaw, const double des_yaw_dot,
+                             const Eigen::Vector3d& kx,
+                             const Eigen::Vector3d& kv)
+{
+  //  ROS_INFO("Error %lf %lf %lf", (des_pos - pos_).norm(),
+  //           (des_vel - vel_).norm(), (des_acc - acc_).norm());
+
+  bool flag_use_pos = !(std::isnan(des_pos(0)) || std::isnan(des_pos(1)) || std::isnan(des_pos(2)));
+  bool flag_use_vel = !(std::isnan(des_vel(0)) || std::isnan(des_vel(1)) || std::isnan(des_vel(2)));
+  bool flag_use_acc = !(std::isnan(des_acc(0)) || std::isnan(des_acc(1)) || std::isnan(des_acc(2)));
+
+  Eigen::Vector3d totalError(Eigen::Vector3d::Zero());
+  if ( flag_use_pos ) totalError.noalias() += des_pos - pos_;
+  if ( flag_use_vel ) totalError.noalias() += des_vel - vel_;
+  if ( flag_use_acc ) totalError.noalias() += des_acc - acc_;
+
+  Eigen::Vector3d ka(fabs(totalError[0]) > 3 ? 0 : (fabs(totalError[0]) * 0.0),
+                     fabs(totalError[1]) > 3 ? 0 : (fabs(totalError[1]) * 0.0),
+                     fabs(totalError[2]) > 3 ? 0 : (fabs(totalError[2]) * 0.0));
+
+  // std::cout << des_pos.transpose() << std::endl;
+  // std::cout << des_vel.transpose() << std::endl;
+  // std::cout << des_acc.transpose() << std::endl;
+  // std::cout << des_yaw << std::endl;
+  // std::cout << pos_.transpose() << std::endl;
+  // std::cout << vel_.transpose() << std::endl;
+  // std::cout << acc_.transpose() << std::endl;
+  
+
+  force_ = mass_ * g_ * Eigen::Vector3d(0, 0, 1);
+  if ( flag_use_pos ) force_.noalias() += kx.asDiagonal() * (des_pos - pos_);
+  if ( flag_use_vel ) force_.noalias() += kv.asDiagonal() * (des_vel - vel_);
+  if ( flag_use_acc ) force_.noalias() += mass_ * ka.asDiagonal() * (des_acc - acc_) + mass_ * (des_acc);
+
+  // Limit control angle to 45 degree
+  double          theta = M_PI / 4;
+  double          c     = cos(theta);
+  Eigen::Vector3d f;
+  f.noalias() = force_ - mass_ * g_ * Eigen::Vector3d(0, 0, 1);
+  if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c)
+  {
+    double nf        = f.norm();
+    double A         = c * c * nf * nf - f(2) * f(2);
+    double B         = 2 * (c * c - 1) * f(2) * mass_ * g_;
+    double C         = (c * c - 1) * mass_ * mass_ * g_ * g_;
+    double s         = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
+    force_.noalias() = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1);
+  }
+  // Limit control angle to 45 degree
+
+  Eigen::Vector3d b1c, b2c, b3c;
+  Eigen::Vector3d b1d(cos(des_yaw), sin(des_yaw), 0);
+
+  if (force_.norm() > 1e-6)
+    b3c.noalias() = force_.normalized();
+  else
+    b3c.noalias() = Eigen::Vector3d(0, 0, 1);
+
+  b2c.noalias() = b3c.cross(b1d).normalized();
+  b1c.noalias() = b2c.cross(b3c).normalized();
+
+  Eigen::Matrix3d R;
+  R << b1c, b2c, b3c;
+
+  orientation_ = Eigen::Quaterniond(R);
+}
+
+const Eigen::Vector3d&
+SO3Control::getComputedForce(void)
+{
+  return force_;
+}
+
+const Eigen::Quaterniond&
+SO3Control::getComputedOrientation(void)
+{
+  return orientation_;
+}
+
+void
+SO3Control::setAcc(const Eigen::Vector3d& acc)
+{
+  acc_ = acc;
+}

Controller/src/so3_control/src/control_example.cpp

@@ -0,0 +1,78 @@
+#include <Eigen/Eigen>
+#include <quadrotor_msgs/PositionCommand.h>
+#include <ros/ros.h>
+
+int main(int argc, char **argv)
+{
+
+  ros::init(argc, argv, "quad_sim_example");
+  ros::NodeHandle nh("~");
+
+  ros::Publisher cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 10);
+
+  ros::Duration(2.0).sleep();
+
+  while (ros::ok())
+  {
+
+    /*** example 1: position control ***/
+    std::cout << "\033[42m"
+              << "Position Control to (2,0,1) meters"
+              << "\033[0m" << std::endl;
+    for (int i = 0; i < 500; i++)
+    {
+      quadrotor_msgs::PositionCommand cmd;
+      cmd.position.x = 2.0;
+      cmd.position.y = 0.0;
+      cmd.position.z = 1.0;
+      cmd_pub.publish(cmd);
+
+      ros::Duration(0.01).sleep();
+      ros::spinOnce();
+    }
+
+    /*** example 2: velocity control ***/
+    std::cout << "\033[42m"
+              << "Velocity Control to (-1,0,0) meters/second"
+              << "\033[0m" << std::endl;
+    for (int i = 0; i < 500; i++)
+    {
+      quadrotor_msgs::PositionCommand cmd;
+      cmd.position.x = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
+      cmd.position.y = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
+      cmd.position.z = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
+      cmd.velocity.x = -1.0;
+      cmd.velocity.y = 0.0;
+      cmd.velocity.z = 0.0;
+      cmd_pub.publish(cmd);
+
+      ros::Duration(0.01).sleep();
+      ros::spinOnce();
+    }
+
+    /*** example 3: accelleration control ***/
+    std::cout << "\033[42m"
+              << "Accelleration Control to (1,0,0) meters/second^2"
+              << "\033[0m" << std::endl;
+    for (int i = 0; i < 500; i++)
+    {
+      quadrotor_msgs::PositionCommand cmd;
+      cmd.position.x = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
+      cmd.position.y = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
+      cmd.position.z = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
+      cmd.velocity.x = std::numeric_limits<float>::quiet_NaN();
+      cmd.velocity.y = std::numeric_limits<float>::quiet_NaN();
+      cmd.velocity.z = std::numeric_limits<float>::quiet_NaN();
+      cmd.acceleration.x = 1.0;
+      cmd.acceleration.y = 0.0;
+      cmd.acceleration.z = 0.0;
+      cmd_pub.publish(cmd);
+
+      ros::Duration(0.01).sleep();
+      ros::spinOnce();
+    }
+
+  }
+
+  return 0;
+}

Controller/src/so3_control/src/network_control_node.cpp

@@ -0,0 +1,11 @@
+#include "so3_control/NetworkControl.h"
+#include "ros/ros.h"
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "network_ctrl_node");
+    ros::NodeHandle node("~");
+    NetworkControl controller(node);
+    ros::spin();
+    return 0;
+}

Controller/src/so3_control/src/so3_control_nodelet.cpp

@@ -0,0 +1,350 @@
+#include <Eigen/Geometry>
+#include <nav_msgs/Odometry.h>
+#include <nodelet/nodelet.h>
+#include <quadrotor_msgs/Corrections.h>
+#include <quadrotor_msgs/PositionCommand.h>
+#include <quadrotor_msgs/SO3Command.h>
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <so3_control/SO3Control.h>
+#include <std_msgs/Bool.h>
+#include <tf/transform_datatypes.h>
+#include <string>
+#include <iostream>
+#include <fstream>
+
+class SO3ControlNodelet : public nodelet::Nodelet
+{
+public:
+  SO3ControlNodelet()
+    : position_cmd_updated_(false)
+    , position_cmd_init_(false)
+    , des_yaw_(0)
+    , des_yaw_dot_(0)
+    , current_yaw_(0)
+    , enable_motors_(true)
+    , // FIXME
+    use_external_yaw_(false)
+  {
+  }
+
+  void onInit(void);
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+private:
+  void publishSO3Command(void);
+  void position_cmd_callback(
+    const quadrotor_msgs::PositionCommand::ConstPtr& cmd);
+  void odom_callback(const nav_msgs::Odometry::ConstPtr& odom);
+  void enable_motors_callback(const std_msgs::Bool::ConstPtr& msg);
+  void corrections_callback(const quadrotor_msgs::Corrections::ConstPtr& msg);
+  void imu_callback(const sensor_msgs::Imu& imu);
+
+  void initLogRecorder();
+  void recordLog();
+
+  SO3Control      controller_;
+  ros::Publisher  so3_command_pub_;
+  ros::Subscriber odom_sub_;
+  ros::Subscriber position_cmd_sub_;
+  ros::Subscriber enable_motors_sub_;
+  ros::Subscriber corrections_sub_;
+  ros::Subscriber imu_sub_;
+
+  bool        position_cmd_updated_, position_cmd_init_;
+  std::string frame_id_;
+
+  bool record_log_{false};
+  bool cur_acc_init_{false}, cur_odom_init_{false};
+  Eigen::Vector3d des_pos_, des_vel_, des_acc_, kx_, kv_;
+  Eigen::Vector3d cur_pos_, cur_vel_, cur_acc_;
+  double          des_yaw_, des_yaw_dot_;
+  double          current_yaw_;
+  bool            enable_motors_;
+  bool            use_external_yaw_;
+  double          kR_[3], kOm_[3], corrections_[3];
+  double          init_x_, init_y_, init_z_;
+
+  std::ofstream logger;
+  std::string logger_file_name;
+};
+
+void
+SO3ControlNodelet::publishSO3Command(void)
+{
+  controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_,
+                               des_yaw_dot_, kx_, kv_);
+
+  const Eigen::Vector3d&    force       = controller_.getComputedForce();
+  const Eigen::Quaterniond& orientation = controller_.getComputedOrientation();
+
+  quadrotor_msgs::SO3Command::Ptr so3_command(
+    new quadrotor_msgs::SO3Command); //! @note memory leak?
+  so3_command->header.stamp    = ros::Time::now();
+  so3_command->header.frame_id = frame_id_;
+  so3_command->force.x         = force(0);
+  so3_command->force.y         = force(1);
+  so3_command->force.z         = force(2);
+  so3_command->orientation.x   = orientation.x();
+  so3_command->orientation.y   = orientation.y();
+  so3_command->orientation.z   = orientation.z();
+  so3_command->orientation.w   = orientation.w();
+  for (int i = 0; i < 3; i++)
+  {
+    so3_command->kR[i]  = kR_[i];
+    so3_command->kOm[i] = kOm_[i];
+  }
+  so3_command->aux.current_yaw          = current_yaw_;
+  so3_command->aux.kf_correction        = corrections_[0];
+  so3_command->aux.angle_corrections[0] = corrections_[1];
+  so3_command->aux.angle_corrections[1] = corrections_[2];
+  so3_command->aux.enable_motors        = enable_motors_;
+  so3_command->aux.use_external_yaw     = use_external_yaw_;
+  so3_command_pub_.publish(so3_command);
+}
+
+void
+SO3ControlNodelet::position_cmd_callback(
+  const quadrotor_msgs::PositionCommand::ConstPtr& cmd)
+{
+  des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
+  des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
+  des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
+                             cmd->acceleration.z);
+
+  if ( cmd->kx[0] > 1e-5 || cmd->kx[1] > 1e-5 || cmd->kx[2] > 1e-5 )
+  {
+    kx_ = Eigen::Vector3d(cmd->kx[0], cmd->kx[1], cmd->kx[2]);
+  }
+  if ( cmd->kv[0] > 1e-5 || cmd->kv[1] > 1e-5 || cmd->kv[2] > 1e-5 )
+  {
+    kv_ = Eigen::Vector3d(cmd->kv[0], cmd->kv[1], cmd->kv[2]);
+  }
+
+  des_yaw_              = cmd->yaw;
+  des_yaw_dot_          = cmd->yaw_dot;
+  position_cmd_updated_ = true;
+  position_cmd_init_    = true;
+
+  publishSO3Command();
+
+  if (record_log_ && cur_acc_init_ && cur_odom_init_)
+  {
+    recordLog();
+  }
+  
+}
+
+void
+SO3ControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr& odom)
+{
+  const Eigen::Vector3d position(odom->pose.pose.position.x,
+                                 odom->pose.pose.position.y,
+                                 odom->pose.pose.position.z);
+  const Eigen::Vector3d velocity(odom->twist.twist.linear.x,
+                                 odom->twist.twist.linear.y,
+                                 odom->twist.twist.linear.z);
+
+  cur_odom_init_ = true;
+  current_yaw_ = tf::getYaw(odom->pose.pose.orientation);
+  cur_pos_ = position;
+  cur_vel_ = velocity;
+
+  controller_.setPosition(position);
+  controller_.setVelocity(velocity);
+
+  if (position_cmd_init_)
+  {
+    // We set position_cmd_updated_ = false and expect that the
+    // position_cmd_callback would set it to true since typically a position_cmd
+    // message would follow an odom message. If not, the position_cmd_callback
+    // hasn't been called and we publish the so3 command ourselves
+    // TODO: Fallback to hover if position_cmd hasn't been received for some
+    // time
+    if (!position_cmd_updated_)
+      publishSO3Command();
+    position_cmd_updated_ = false;
+  }
+  else if ( init_z_ > -9999.0 )
+  {
+    des_pos_ = Eigen::Vector3d(init_x_, init_y_, init_z_);
+    des_vel_ = Eigen::Vector3d(0,0,0);
+    des_acc_ = Eigen::Vector3d(0,0,0);
+    publishSO3Command();
+  }
+  
+}
+
+void
+SO3ControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr& msg)
+{
+  if (msg->data)
+    ROS_INFO("Enabling motors");
+  else
+    ROS_INFO("Disabling motors");
+
+  enable_motors_ = msg->data;
+}
+
+void
+SO3ControlNodelet::corrections_callback(
+  const quadrotor_msgs::Corrections::ConstPtr& msg)
+{
+  corrections_[0] = msg->kf_correction;
+  corrections_[1] = msg->angle_corrections[0];
+  corrections_[2] = msg->angle_corrections[1];
+}
+
+void
+SO3ControlNodelet::imu_callback(const sensor_msgs::Imu& imu)
+{
+  cur_acc_init_ = true;
+  const Eigen::Vector3d acc(imu.linear_acceleration.x,
+                            imu.linear_acceleration.y,
+                            imu.linear_acceleration.z);
+  cur_acc_ = acc;
+  controller_.setAcc(acc);
+}
+
+void
+SO3ControlNodelet::onInit(void)
+{
+  ros::NodeHandle n(getPrivateNodeHandle());
+
+  std::string quadrotor_name;
+  n.param("quadrotor_name", quadrotor_name, std::string("quadrotor"));
+  frame_id_ = "/" + quadrotor_name;
+
+  double mass;
+  n.param("mass", mass, 0.5);
+  controller_.setMass(mass);
+  
+  n.param("record_log", record_log_, false);
+  n.param("PID_logger_file_name", logger_file_name, std::string("/home/lu/"));
+  n.param("use_external_yaw", use_external_yaw_, true);
+
+  n.param("gains/rot/x", kR_[0], 1.5);
+  n.param("gains/rot/y", kR_[1], 1.5);
+  n.param("gains/rot/z", kR_[2], 1.0);
+  n.param("gains/ang/x", kOm_[0], 0.13);
+  n.param("gains/ang/y", kOm_[1], 0.13);
+  n.param("gains/ang/z", kOm_[2], 0.1);
+  n.param("gains/kx/x", kx_[0], 5.7);
+  n.param("gains/kx/y", kx_[1], 5.7);
+  n.param("gains/kx/z", kx_[2], 6.2);
+  n.param("gains/kv/x", kv_[0], 3.4);
+  n.param("gains/kv/y", kv_[1], 3.4);
+  n.param("gains/kv/z", kv_[2], 4.0);
+
+  n.param("corrections/z", corrections_[0], 0.0);
+  n.param("corrections/r", corrections_[1], 0.0);
+  n.param("corrections/p", corrections_[2], 0.0);
+
+  n.param("so3_control/init_state_x", init_x_, 0.0);
+  n.param("so3_control/init_state_y", init_y_, 0.0);
+  n.param("so3_control/init_state_z", init_z_, -10000.0);
+
+  so3_command_pub_ = n.advertise<quadrotor_msgs::SO3Command>("so3_cmd", 10);
+
+  odom_sub_ = n.subscribe("odom", 10, &SO3ControlNodelet::odom_callback, this,
+                          ros::TransportHints().tcpNoDelay());
+  position_cmd_sub_ =
+    n.subscribe("position_cmd", 10, &SO3ControlNodelet::position_cmd_callback,
+                this, ros::TransportHints().tcpNoDelay());
+
+  enable_motors_sub_ =
+    n.subscribe("motors", 2, &SO3ControlNodelet::enable_motors_callback, this,
+                ros::TransportHints().tcpNoDelay());
+  corrections_sub_ =
+    n.subscribe("corrections", 10, &SO3ControlNodelet::corrections_callback,
+                this, ros::TransportHints().tcpNoDelay());
+
+  imu_sub_ = n.subscribe("imu", 10, &SO3ControlNodelet::imu_callback, this,
+                         ros::TransportHints().tcpNoDelay());
+  
+  if (record_log_)
+    initLogRecorder();
+  
+}
+
+void SO3ControlNodelet::initLogRecorder()
+{
+  std::cout << "logger_file_name: " << logger_file_name << std::endl;
+  std::string temp_file_name = logger_file_name + "PID_logger_";
+  time_t timep;
+  timep = time(0);
+  char tmp[64];
+  strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S", localtime(&timep));
+  temp_file_name += tmp;
+  temp_file_name += ".csv";
+  if (logger.is_open())
+  {
+    logger.close();
+  }
+  logger.open(temp_file_name.c_str(), std::ios::out);
+  std::cout << "PID logger: " << temp_file_name << std::endl;
+  if (!logger.is_open())
+  {
+    std::cout << "cannot open the logger." << std::endl;
+  }
+  else
+  {
+    logger << "timestamp" << ',';
+    logger << "cur_x" << ',';
+    logger << "cur_y" << ',';
+    logger << "cur_z" << ',';
+    logger << "cur_vx" << ',';
+    logger << "cur_vy" << ',';
+    logger << "cur_vz" << ',';
+    logger << "cur_ax" << ',';
+    logger << "cur_ay" << ',';
+    logger << "cur_az" << ',';
+    logger << "not_use" << ',';
+    logger << "not_use" << ',';
+    logger << "cur_yaw" << ',';
+    logger << "des_yaw" << ',';
+    logger << "des_pos_x" << ',';
+    logger << "des_pos_y" << ',';
+    logger << "des_pos_z" << ',';
+    logger << "des_vel_x" << ',';
+    logger << "des_vel_y" << ',';
+    logger << "des_vel_z" << ',';
+    logger << "des_acc_x" << ',';
+    logger << "des_acc_y" << ',';
+    logger << "des_acc_z" << std::endl;
+  }
+}
+void SO3ControlNodelet::recordLog(){
+  if (logger.is_open())
+  {
+    logger << ros::Time::now().toNSec() << ',';
+    logger << cur_pos_(0) << ',';
+    logger << cur_pos_(1) << ',';
+    logger << cur_pos_(2) << ',';
+    logger << cur_vel_(0) << ',';
+    logger << cur_vel_(1) << ',';
+    logger << cur_vel_(2) << ',';
+    logger << cur_acc_(0) << ',';
+    logger << cur_acc_(1) << ',';
+    logger << cur_acc_(2) << ',';
+    logger << 0.0 << ',';
+    logger << 0.0 << ',';
+    logger << current_yaw_ << ',';
+    logger << des_yaw_ << ',';
+    logger << des_pos_(0) << ',';
+    logger << des_pos_(1) << ',';
+    logger << des_pos_(2) << ',';
+    logger << des_vel_(0) << ',';
+    logger << des_vel_(1) << ',';
+    logger << des_vel_(2) << ',';
+    logger << des_acc_(0) << ',';
+    logger << des_acc_(1) << ',';
+    logger << des_acc_(2) << std::endl;
+  }
+}
+
+#include <pluginlib/class_list_macros.h>
+//PLUGINLIB_DECLARE_CLASS(so3_control, SO3ControlNodelet, SO3ControlNodelet,
+//                        nodelet::Nodelet);
+PLUGINLIB_EXPORT_CLASS(SO3ControlNodelet, nodelet::Nodelet);