Merge remote-tracking branch 'sugihara-16/develop/gimbal_rotor' into …

…PR/birotor

.github/workflows/catkin_lint.yml

@@ -28,4 +28,4 @@ jobs:
           wstool init
           wstool merge aerial_robot_noetic.rosinstall
           wstool update
-          ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty  --skip-path rosserial
+          ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty  --skip-path rosserial --skip-path livox_ros_driver2 --skip-path fast_lio

.travis.sh

@@ -59,6 +59,16 @@ if [ ${ROS_DISTRO} = 'kinetic' ]; then
     wget https://raw.githubusercontent.com/osrf/gazebo_models/master/ground_plane/model.config -P ${path}
 fi
 
+if [[ "$ROS_DISTRO" = "kinetic" ]]; then
+    # to use c++17
+    sudo apt-get install -y software-properties-common
+    sudo add-apt-repository ppa:ubuntu-toolchain-r/test
+    sudo apt update
+    sudo apt install -y g++-7
+    export CXX='g++-7'
+    export CC='gcc-7'
+fi
+
 # Build
 catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
 catkin build -p1 -j1 --no-status

aerial_robot_base/CMakeLists.txt

@@ -14,7 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 catkin_python_setup()
 
-add_compile_options(-std=c++14)
+add_compile_options(-std=c++17)
 
 catkin_package(
   INCLUDE_DIRS include

aerial_robot_base/bin/rosbag_control_data.sh

@@ -1,3 +1,3 @@
 #!/bin/bash
 
-rosrun aerial_robot_base rosbag_raw_sensors.sh $1 rosout rosout_agg $1/four_axes/command $1/rpy/pid  $1/rpy/gain $1/debug/pose/pid $1/desire_coordinate $1/flight_config_cmd $1/kf/imu1/data  $1/kf/alt1/data $1/kf/gps1/data $1/kf/mocap1/data $1/kf/vo1/data $1/ground_truth $1/debug/four_axes/gain $1/motor_info $1/p_matrix_pseudo_inverse_inertia $1/servo/torque_enable $1/servo/target_states /tf_static /tf $1/uav/baselink/odom $1/uav/cog/odom $1/uav/full_state $1/uav_info $1/uav_power $1/uav/nav $1/joints_ctrl $1/joint_states  ${@:2}
+rosrun aerial_robot_base rosbag_raw_sensors.sh $1 rosout rosout_agg $1/four_axes/command $1/rpy/pid  $1/rpy/gain $1/debug/pose/pid $1/desire_coordinate $1/flight_config_cmd $1/kf/imu1/data  $1/kf/alt1/data $1/kf/gps1/data $1/kf/mocap1/data $1/kf/vo1/data $1/ground_truth $1/debug/four_axes/gain $1/motor_info $1/p_matrix_pseudo_inverse_inertia $1/servo/torque_enable $1/servo/target_states /tf_static /tf $1/uav/baselink/odom $1/uav/cog/odom $1/uav/full_state $1/uav_info $1/uav_power $1/uav/nav $1/joints_ctrl $1/joint_states $1/joy ${@:2}

aerial_robot_base/config/sensors/lio/livox_mid360.yaml

@@ -0,0 +1,16 @@
+sensor_plugin:
+  vo:
+    vo_sub_topic_name: Odometry_precede
+    level_pos_noise_sigma: 0.01  # 0.01 is better that 0.001
+    z_pos_noise_sigma: 0.01 # 0.01 is better than 0.001
+    z_vel_mode: false
+    vel_noise_sigma: 0.01 # TODO: modification
+    sensor_frame: lidar_imu
+    reference_frame: fc
+    vio_mode: true
+    local_vel_mode: false # the twist/twist/velocity is described in global frame
+    time_sync: false
+    reset_duration: 0.5
+
+    fusion_mode: 2 # ONLY_POS_MODE = 0, ONLY_VEL_MODE = 1, POS_VEL_MODE = 2
+    #param_verbose: true

aerial_robot_base/launch/external_module/mocap.launch

@@ -6,8 +6,7 @@
     type="mocap_node"
     name="mocap_node"
     respawn="false"
-    launch-prefix=""
-    required="true">
+    launch-prefix="">
     <rosparam subst_value="true">
       rigid_bodies:
          '$(arg robot_id)':

aerial_robot_control/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(aerial_robot_control)
 
-add_compile_options(-std=c++11)
+add_compile_options(-std=c++17)
 
 find_package(catkin REQUIRED COMPONENTS
   aerial_robot_estimation
@@ -38,7 +38,7 @@ set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -DNDEBUG")
 
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES control_utils flight_control_pluginlib flight_navigation
+  LIBRARIES control_utils flight_control_pluginlib flight_navigation trajectory_generation
   CATKIN_DEPENDS aerial_robot_estimation aerial_robot_model aerial_robot_msgs dynamic_reconfigure pluginlib roscpp spinal tf
 )
 
@@ -69,13 +69,38 @@ add_dependencies(flight_control_pluginlib  ${PROJECT_NAME}_gencfg)
 add_library (flight_navigation src/flight_navigation.cpp)
 target_link_libraries (flight_navigation ${catkin_LIBRARIES})
 
+### trajectory generate based on dodgelib
+add_library (trajectory_generation
+  src/trajectory/reference_base.cpp
+  src/trajectory/trajectory_reference/polynomial.cpp
+  src/trajectory/trajectory_reference/polynomial_trajectory.cpp
+  src/trajectory/trajectory_reference/sampled_trajectory.cpp
+  src/trajectory/base/parameter_base.cpp
+  src/trajectory/math/math.cpp
+  src/trajectory/types/command.cpp
+  src/trajectory/types/quad_state.cpp
+  src/trajectory/types/quadrotor.cpp
+  src/trajectory/utils/logger.cpp
+  src/trajectory/utils/timer.cpp
+  )
+target_link_libraries (trajectory_generation ${catkin_LIBRARIES})
+
+add_executable(trajectory_generation_node src/trajectory/test.cpp)
+target_link_libraries (trajectory_generation_node ${catkin_LIBRARIES} trajectory_generation)
+
+
+
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
 
-install(TARGETS control_utils flight_control_pluginlib flight_navigation
+install(TARGETS control_utils flight_control_pluginlib flight_navigation trajectory_generation
   DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
 
+install(TARGETS trajectory_generation_node
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 install(DIRECTORY scripts plugins
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS

aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h

@@ -84,7 +84,7 @@ namespace aerial_robot_control
 
     tf::Vector3 pos_, target_pos_;
     tf::Vector3 vel_, target_vel_;
-    tf::Vector3 target_acc_;
+    tf::Vector3 target_acc_, target_ang_acc_;
     tf::Vector3 rpy_, target_rpy_;
     tf::Vector3 omega_, target_omega_;
 

aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h

@@ -105,6 +105,7 @@ namespace aerial_robot_control
     virtual void sendCmd() override;
     virtual void sendFourAxisCommand();
 
+    Eigen::MatrixXd getQInv();
     virtual void allocateYawTerm();
     void cfgLQICallback(aerial_robot_control::LQIConfig &config, uint32_t level); //dynamic reconfigure
 

aerial_robot_control/include/aerial_robot_control/flight_navigation.h

@@ -6,13 +6,16 @@
 #include <aerial_robot_msgs/FlightNav.h>
 #include <angles/angles.h>
 #include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <ros/ros.h>
 #include <sensor_msgs/Joy.h>
 #include <spinal/FlightConfigCmd.h>
 #include <std_msgs/Empty.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/Int8.h>
 #include <std_msgs/UInt8.h>
+#include <nav_msgs/Path.h>
+#include <aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp>
 
 namespace aerial_robot_navigation
 {
@@ -75,6 +78,7 @@ namespace aerial_robot_navigation
     inline tf::Vector3 getTargetAcc() {return target_acc_;}
     inline tf::Vector3 getTargetRPY() {return target_rpy_;}
     inline tf::Vector3 getTargetOmega() {return target_omega_;}
+    inline tf::Vector3 getTargetAngAcc() {return target_ang_acc_;}
 
     inline void setTargetPos(tf::Vector3 pos) { target_pos_ = pos; }
     inline void setTargetPos(double x, double y, double z) { setTargetPos(tf::Vector3(x, y, z)); }
@@ -88,12 +92,22 @@ namespace aerial_robot_navigation
     inline void setTargetZeroAcc() { setTargetAcc(tf::Vector3(0,0,0)); }
 
     inline void setTargetRoll(float value) { target_rpy_.setX(value); }
+    inline void setTargetOmega(tf::Vector3 omega) { target_omega_ = omega; }
+    inline void setTargetOmega(double x, double y, double z) { setTargetOmega(tf::Vector3(x, y, z)); }
+    inline void setTargetZeroOmega() { setTargetOmega(0,0,0); }
     inline void setTargetOmegaX(float value) { target_omega_.setX(value); }
     inline void setTargetPitch(float value) { target_rpy_.setY(value); }
     inline void setTargetOmegaY(float value) { target_omega_.setY(value); }
     inline void setTargetYaw(float value) { target_rpy_.setZ(value); }
     inline void addTargetYaw(float value) { setTargetYaw(angles::normalize_angle(target_rpy_.z() + value)); }
     inline void setTargetOmegaZ(float value) { target_omega_.setZ(value); }
+    inline void setTargetAngAcc(tf::Vector3 acc) { target_ang_acc_ = acc; }
+    inline void setTargetAngAcc(double x, double y, double z) { setTargetAngAcc(tf::Vector3(x, y, z)); }
+    inline void setTargetZeroAngAcc() { setTargetAngAcc(tf::Vector3(0,0,0)); }
+    inline void setTargetAngAccX(double value) { target_ang_acc_.setX(value); }
+    inline void setTargetAngAccY(double value) { target_ang_acc_.setY(value); }
+    inline void setTargetAngAccZ(double value) { target_ang_acc_.setZ(value); }
+
     inline void setTargetPosX( float value){  target_pos_.setX(value);}
     inline void setTargetVelX( float value){  target_vel_.setX(value);}
     inline void setTargetAccX( float value){  target_acc_.setX(value);}
@@ -116,6 +130,8 @@ namespace aerial_robot_navigation
     uint8_t getEstimateMode(){ return estimate_mode_;}
     void setEstimateMode(uint8_t estimate_mode){ estimate_mode_ = estimate_mode;}
 
+    void generateNewTrajectory(geometry_msgs::PoseStamped pose);
+
     static constexpr uint8_t POS_CONTROL_COMMAND = 0;
     static constexpr uint8_t VEL_CONTROL_COMMAND = 1;
 
@@ -219,7 +235,10 @@ namespace aerial_robot_navigation
     ros::Publisher  flight_config_pub_;
     ros::Publisher  power_info_pub_;
     ros::Publisher  flight_state_pub_;
+    ros::Publisher  path_pub_;
     ros::Subscriber navi_sub_;
+    ros::Subscriber pose_sub_;
+    ros::Subscriber simple_move_base_goal_sub_;
     ros::Subscriber battery_sub_;
     ros::Subscriber flight_status_ack_sub_;
     ros::Subscriber takeoff_sub_;
@@ -266,7 +285,7 @@ namespace aerial_robot_navigation
 
     /* target value */
     tf::Vector3 target_pos_, target_vel_, target_acc_;
-    tf::Vector3 target_rpy_, target_omega_;
+    tf::Vector3 target_rpy_, target_omega_, target_ang_acc_;
 
     double takeoff_height_;
     double init_height_;
@@ -307,17 +326,28 @@ namespace aerial_robot_navigation
 
     std::string teleop_local_frame_;
 
+    /* trajectory */
+    double trajectory_mean_vel_;
+    double trajectory_mean_yaw_rate_;
+    double trajectory_min_du_;
+    bool enable_latch_yaw_trajectory_;
+    std::shared_ptr<agi::MinJerkTrajectory> traj_generator_ptr_;
+
     /* battery info */
     double low_voltage_thre_;
     bool low_voltage_flag_;
+    double high_voltage_cell_thre_;
+    bool high_voltage_flag_;
     int bat_cell_;
     double bat_resistance_;
     double bat_resistance_voltage_rate_;
     double hovering_current_;
 
     virtual void rosParamInit();
-    void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg);
-    void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg);
+    void poseCallback(const geometry_msgs::PoseStampedConstPtr & msg);
+    void simpleMoveBaseGoalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
+    virtual void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg);
+    virtual void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg);
     void batteryCheckCallback(const std_msgs::Float32ConstPtr &msg);
 
     virtual void halt() {}
@@ -433,9 +463,6 @@ namespace aerial_robot_navigation
       if(!teleop_flag_) return;
 
       setNaviState(LAND_STATE);
-
-      setTargetXyFromCurrentState();
-      setTargetYawFromCurrentState();
       ROS_INFO("Land state");
     }
 

aerial_robot_control/include/aerial_robot_control/trajectory/base/module.hpp

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <memory>
+
+#include "aerial_robot_control/trajectory/utils/logger.hpp"
+
+namespace agi {
+
+template<typename Derived>
+class Module {
+ public:
+  Module(const std::string& name) : logger_(name) {}
+  virtual ~Module() {}
+
+  inline const std::string& name() const { return logger_.name(); }
+  virtual void logTiming() const {};
+
+ protected:
+  Logger logger_;
+};
+
+}  // namespace agi

aerial_robot_control/include/aerial_robot_control/trajectory/base/parameter_base.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <aerial_robot_control/trajectory/utils/filesystem.hpp>
+#include <exception>
+#include <sstream>
+
+#include "aerial_robot_control/trajectory/math/types.hpp"
+#include "aerial_robot_control/trajectory/utils/filesystem.hpp"
+#include "aerial_robot_control/trajectory/utils/yaml.hpp"
+
+namespace agi {
+
+
+struct ParameterException : public std::exception {
+  ParameterException() = default;
+  ParameterException(const std::string& msg)
+    : msg(std::string("Dodgelib Parameter Exception: ") + msg) {}
+  const char* what() const throw() { return msg.c_str(); }
+
+  const std::string msg{"Dodgelib Parameter Exception"};
+};
+
+struct ParameterBase {
+  virtual ~ParameterBase() = default;
+  virtual bool load(const fs::path& filename);
+  virtual bool load(const Yaml& yaml);
+
+  virtual bool valid() const;
+};
+
+}  // namespace agi

aerial_robot_control/include/aerial_robot_control/trajectory/math/gravity.hpp

@@ -0,0 +1,22 @@
+#pragma once
+#include "aerial_robot_control/trajectory/math/types.hpp"
+
+namespace agi {
+
+/**
+ * Gravity Value [m/s^2]
+ *
+ * This is the gravity value used in the whole project.
+ */
+static constexpr Scalar G = 9.8066;
+
+
+/**
+ * Gravity Vector [m/s^2]
+ *
+ * This is the gravity vector pointing in negative z-direction.
+ * It uses the value of #G.
+ */
+const Vector<3> GVEC{0, 0, -G};
+
+}  // namespace agi

aerial_robot_control/include/aerial_robot_control/trajectory/math/math.hpp

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <aerial_robot_control/trajectory/math/types.hpp>
+
+namespace agi {
+
+Matrix<3, 3> skew(const Vector<3>& v);
+
+Matrix<4, 4> Q_left(const Quaternion& q);
+
+Matrix<4, 4> Q_right(const Quaternion& q);
+
+Matrix<4, 3> qFromQeJacobian(const Quaternion& q);
+
+Matrix<4, 4> qConjugateJacobian();
+
+Matrix<3, 3> qeRotJacobian(const Quaternion& q, const Matrix<3, 1>& t);
+
+Matrix<3, 3> qeInvRotJacobian(const Quaternion& q, const Matrix<3, 1>& t);
+
+void matrixToTripletList(const SparseMatrix& matrix,
+                         std::vector<SparseTriplet>* const list,
+                         const int row_offset = 0, const int col_offset = 0);
+
+void matrixToTripletList(const Matrix<>& matrix,
+                         std::vector<SparseTriplet>* const list,
+                         const int row_offset = 0, const int col_offset = 0);
+
+void insert(const SparseMatrix& from, SparseMatrix* const into,
+            const int row_offset = 0, const int col_offset = 0);
+
+void insert(const Matrix<>& from, SparseMatrix* const into,
+            const int row_offset = 0, const int col_offset = 0);
+
+void insert(const Matrix<>& from, Matrix<>* const into,
+            const int row_offset = 0, const int col_offset = 0);
+
+Vector<> clip(const Vector<>& v, const Vector<>& bound);
+
+inline constexpr Scalar toRad(const Scalar angle_deg) {
+  return angle_deg * M_PI / 180.0;
+}
+inline constexpr Scalar toDeg(const Scalar angle_deg) {
+  return angle_deg / M_PI * 180.0;
+}
+
+}  // namespace agi