add ego_planner

README.en.md

@@ -22,53 +22,57 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio
 
 Developers can quickly verify algorithms with XTDrone, such as:
 
-1. Stereo SLAM
+Stereo SLAM
 
 <img src="./images/vslam.gif" width="640" height="360" /> 
 
-2. VIO 
+VIO 
 
 <img src="./images/vio.gif" width="640" height="360" />  
 
-3. Dense Reconstruction
+Dense Reconstruction
 
 <img src="./images/dense_reconstruction.gif" width="640" height="360" /> 
 
-4. 2D Laser SLAM
+2D Laser SLAM
 
 <img src="./images/laser_slam_2d.gif" width="640" height="360" /> 
 
-5. 3D Laser SLAM
+3D Laser SLAM
 
 <img src="./images/laser_slam_3d.gif" width="640" height="360"/>  
 
-6. Motion Planning
+2D Motion Planning
 
-<img src="./images/motion_planning.gif" width="640" height="360" />  
+<img src="./images/2d_motion_planning.gif" width="640" height="360" />  
 
-7. Object Detection and Tracking
+3D Motion Planning
+
+<img src="./images/3d_motion_planning.gif" width="640" height="360" /> 
+
+Object Detection and Tracking
 
 <img src="./images/human_tracking.gif" width="640" height="360" /> 
 
-8. Formation
+Formation
 
 <img src="./images/formation_1.gif" width="640" height="360" />  
 
 <img src="./images/formation_2.gif" width="640" height="360" />  
 
-9. Fixed wing
+Fixed wing
 
 <img src="./images/planes.gif" width="640" height="360" />  
 
-10. VTOL
+VTOL
 
 <img src="./images/vtols.gif" width="640" height="360" />  
 
-11. UGV
+UGV
 
 <img src="./images/ugv.gif" width="640" height="360" />  
 
-12. USV
+USV
 
 <img src="./images/usv.gif" width="640" height="360" />  
 

README.md

@@ -21,54 +21,58 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio
 
 在这个平台上，开发者可以快速验证算法。如：
 
-1. 双目SLAM
+双目SLAM
 
 <img src="./images/vslam.gif" width="640" height="360" /> 
 
-2. 视觉惯性导航
+视觉惯性导航
 
 <img src="./images/vio.gif" width="640" height="360" />  
 
-3. 三维稠密重建
+三维稠密重建
 
 <img src="./images/dense_reconstruction.gif" width="640" height="360" /> 
 
-4. 2D激光SLAM
+2D激光SLAM
 
 <img src="./images/laser_slam_2d.gif" width="640" height="360" /> 
 
-5. 3D激光SLAM
+3D激光SLAM
 
 <img src="./images/laser_slam_3d.gif" width="640" height="360"/>  
 
-6. 运动规划
+2D运动规划
 
-<img src="./images/motion_planning.gif" width="640" height="360" />  
+<img src="./images/2d_motion_planning.gif" width="640" height="360" />  
 
-7. 目标检测与追踪
+3D运动规划
+
+<img src="./images/3d_motion_planning.gif" width="640" height="360" />  
+
+目标检测与追踪
 
 <img src="./images/human_tracking.gif" width="640" height="360" /> 
 
-8. 多机协同
+多机协同
 
 <img src="./images/formation_1.gif" width="640" height="360" />  
 
 <img src="./images/formation_2.gif" width="640" height="360" />  
 
-9. 固定翼
+固定翼
 
 <img src="./images/planes.gif" width="640" height="360" />  
 
-10. 复合翼
+复合翼
 
 <img src="./images/vtols.gif" width="640" height="360" />  
 
 
-11. 无人车
+无人车
 
 <img src="./images/ugv.gif" width="640" height="360" />  
 
-12. 无人船
+无人船
 
 <img src="./images/usv.gif" width="640" height="360" />  
 

communication/multirotor_communication.py

@@ -119,11 +119,15 @@ class Communication:
 
     def cmd_pose_flu_callback(self, msg):
         self.coordinate_frame = 9
-        self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
+        self.motion_type = 0
+        yaw = self.q2yaw(msg.orientation)
+        self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw)
  
     def cmd_pose_enu_callback(self, msg):
         self.coordinate_frame = 1
-        self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
+        self.motion_type = 0
+        yaw = self.q2yaw(msg.orientation)
+        self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw)
         
     def cmd_vel_flu_callback(self, msg):
         self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)

motion_planning/3d/ego_planner/ego_rviz.rviz

@@ -0,0 +1,188 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /iris_0_ego_planner_node/optimal_list
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /iris_0_ego_planner_node/grid_map/occupancy_inflate
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /iris_0/mavros/vision_pose/pose
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 4.135247707366943
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.7557446956634521
+        Y: 7.2424845695495605
+        Z: 0.9154675006866455
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.8753979206085205
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.0754048824310303
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 67
+  Y: 30

motion_planning/3d/ego_planner/ego_transfer.py

@@ -0,0 +1,34 @@
+import rospy
+from geometry_msgs.msg import PoseStamped
+import math
+from pyquaternion import Quaternion
+import tf
+import sys
+
+vehicle_type = sys.argv[1]
+local_pose = PoseStamped()
+local_pose.header.frame_id = 'world'
+quaternion = tf.transformations.quaternion_from_euler(-math.pi/2, 0, -math.pi/2)
+q = Quaternion([quaternion[3],quaternion[0],quaternion[1],quaternion[2]])
+
+def vision_callback(data):    
+    local_pose.pose.position.x = data.pose.position.x
+    local_pose.pose.position.y = data.pose.position.y
+    local_pose.pose.position.z = data.pose.position.z
+    q_= Quaternion([data.pose.orientation.w,data.pose.orientation.x,data.pose.orientation.y,data.pose.orientation.z])
+    q_ = q_*q
+    local_pose.pose.orientation.w = q_[0]
+    local_pose.pose.orientation.x = q_[1]
+    local_pose.pose.orientation.y = q_[2]
+    local_pose.pose.orientation.z = q_[3]
+    
+rospy.init_node('ego_transfer')
+rospy.Subscriber(vehicle_type+"_0/mavros/vision_pose/pose", PoseStamped, vision_callback)
+position_pub = rospy.Publisher(vehicle_type+"_0/camera_pose", PoseStamped, queue_size=2)
+rate = rospy.Rate(60) 
+
+while True:
+    local_pose.header.stamp = rospy.Time.now()
+    position_pub.publish(local_pose) 
+    rate.sleep()
+  

motion_planning/3d/ego_planner/planner/bspline_opt/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(bspline_opt)
+
+set(CMAKE_BUILD_TYPE "Release")
+ADD_COMPILE_OPTIONS(-std=c++11 )
+ADD_COMPILE_OPTIONS(-std=c++14 )
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  visualization_msgs
+  plan_env
+  cv_bridge
+  traj_utils
+  path_searching
+)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES bspline_opt
+ CATKIN_DEPENDS plan_env 
+#  DEPENDS system_lib
+)
+
+include_directories( 
+    SYSTEM 
+    include 
+    ${catkin_INCLUDE_DIRS}
+    ${Eigen3_INCLUDE_DIRS} 
+    ${PCL_INCLUDE_DIRS}
+)
+
+add_library( bspline_opt 
+    src/uniform_bspline.cpp 
+    src/bspline_optimizer.cpp 
+    src/gradient_descent_optimizer.cpp
+    )
+target_link_libraries( bspline_opt
+    ${catkin_LIBRARIES} 
+    )  

motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/bspline_optimizer.h

@@ -0,0 +1,212 @@
+#ifndef _BSPLINE_OPTIMIZER_H_
+#define _BSPLINE_OPTIMIZER_H_
+
+#include <Eigen/Eigen>
+#include <path_searching/dyn_a_star.h>
+#include <bspline_opt/uniform_bspline.h>
+#include <plan_env/grid_map.h>
+#include <plan_env/obj_predictor.h>
+#include <ros/ros.h>
+#include "bspline_opt/lbfgs.hpp"
+#include <traj_utils/plan_container.hpp>
+
+// Gradient and elasitc band optimization
+
+// Input: a signed distance field and a sequence of points
+// Output: the optimized sequence of points
+// The format of points: N x 3 matrix, each row is a point
+namespace ego_planner
+{
+
+  class ControlPoints
+  {
+  public:
+    double clearance;
+    int size;
+    Eigen::MatrixXd points;
+    std::vector<std::vector<Eigen::Vector3d>> base_point; // The point at the statrt of the direction vector (collision point)
+    std::vector<std::vector<Eigen::Vector3d>> direction;  // Direction vector, must be normalized.
+    std::vector<bool> flag_temp;                          // A flag that used in many places. Initialize it everytime before using it.
+    // std::vector<bool> occupancy;
+
+    void resize(const int size_set)
+    {
+      size = size_set;
+
+      base_point.clear();
+      direction.clear();
+      flag_temp.clear();
+      // occupancy.clear();
+
+      points.resize(3, size_set);
+      base_point.resize(size);
+      direction.resize(size);
+      flag_temp.resize(size);
+      // occupancy.resize(size);
+    }
+
+    void segment(ControlPoints &buf, const int start, const int end)
+    {
+      if (start < 0 || end >= size || points.rows() != 3)
+      {
+        ROS_ERROR("Wrong segment index! start=%d, end=%d", start, end);
+        return;
+      }
+
+      buf.resize(end - start + 1);
+      buf.points = points.block(0, start, 3, end - start + 1);
+      buf.clearance = clearance;
+      buf.size = end - start + 1;
+      for (int i = start; i <= end; i++)
+      {
+        buf.base_point[i - start] = base_point[i];
+        buf.direction[i - start] = direction[i];
+
+        // if ( buf.base_point[i - start].size() > 1 )
+        // {
+        //   ROS_ERROR("buf.base_point[i - start].size()=%d, base_point[i].size()=%d", buf.base_point[i - start].size(), base_point[i].size());
+        // }
+      }
+
+      // cout << "RichInfoOneSeg_temp, insede" << endl;
+      // for ( int k=0; k<buf.size; k++ )
+      //   if ( buf.base_point[k].size() > 0 )
+      //   {
+      //     cout << "###" << buf.points.col(k).transpose() << endl;
+      //     for (int k2 = 0; k2 < buf.base_point[k].size(); k2++)
+      //     {
+      //       cout << "      " << buf.base_point[k][k2].transpose() << " @ " << buf.direction[k][k2].transpose() << endl;
+      //     }
+      //   }
+    }
+  };
+
+  class BsplineOptimizer
+  {
+
+  public:
+    BsplineOptimizer() {}
+    ~BsplineOptimizer() {}
+
+    /* main API */
+    void setEnvironment(const GridMap::Ptr &map);
+    void setEnvironment(const GridMap::Ptr &map, const fast_planner::ObjPredictor::Ptr mov_obj);
+    void setParam(ros::NodeHandle &nh);
+    Eigen::MatrixXd BsplineOptimizeTraj(const Eigen::MatrixXd &points, const double &ts,
+                                        const int &cost_function, int max_num_id, int max_time_id);
+
+    /* helper function */
+
+    // required inputs
+    void setControlPoints(const Eigen::MatrixXd &points);
+    void setBsplineInterval(const double &ts);
+    void setSwarmTrajs(SwarmTrajData *swarm_trajs_ptr);
+    void setDroneId(const int drone_id);
+
+    // optional inputs
+    void setGuidePath(const vector<Eigen::Vector3d> &guide_pt);
+    void setWaypoints(const vector<Eigen::Vector3d> &waypts,
+                      const vector<int> &waypt_idx); // N-2 constraints at most
+    void setLocalTargetPt(const Eigen::Vector3d local_target_pt) { local_target_pt_ = local_target_pt; };
+
+    void optimize();
+
+    ControlPoints getControlPoints() { return cps_; };
+
+    AStar::Ptr a_star_;
+    std::vector<Eigen::Vector3d> ref_pts_;
+
+    std::vector<ControlPoints> distinctiveTrajs(vector<std::pair<int, int>> segments);
+    std::vector<std::pair<int, int>> initControlPoints(Eigen::MatrixXd &init_points, bool flag_first_init = true);
+    bool BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts); // must be called after initControlPoints()
+    bool BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double &final_cost, const ControlPoints &control_points, double ts);
+    bool BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points);
+
+    inline int getOrder(void) { return order_; }
+    inline double getSwarmClearance(void) { return swarm_clearance_; }
+
+  private:
+    GridMap::Ptr grid_map_;
+    fast_planner::ObjPredictor::Ptr moving_objs_;
+    SwarmTrajData *swarm_trajs_{NULL}; // Can not use shared_ptr and no need to free
+    int drone_id_;
+
+    enum FORCE_STOP_OPTIMIZE_TYPE
+    {
+      DONT_STOP,
+      STOP_FOR_REBOUND,
+      STOP_FOR_ERROR
+    } force_stop_type_;
+
+    // main input
+    // Eigen::MatrixXd control_points_;     // B-spline control points, N x dim
+    double bspline_interval_; // B-spline knot span
+    Eigen::Vector3d end_pt_;  // end of the trajectory
+    // int             dim_;                // dimension of the B-spline
+    //
+    vector<Eigen::Vector3d> guide_pts_; // geometric guiding path points, N-6
+    vector<Eigen::Vector3d> waypoints_; // waypts constraints
+    vector<int> waypt_idx_;             // waypts constraints index
+                                        //
+    int max_num_id_, max_time_id_;      // stopping criteria
+    int cost_function_;                 // used to determine objective function
+    double start_time_;                 // global time for moving obstacles
+
+    /* optimization parameters */
+    int order_;                    // bspline degree
+    double lambda1_;               // jerk smoothness weight
+    double lambda2_, new_lambda2_; // distance weight
+    double lambda3_;               // feasibility weight
+    double lambda4_;               // curve fitting
+
+    int a;
+    //
+    double dist0_, swarm_clearance_; // safe distance
+    double max_vel_, max_acc_;       // dynamic limits
+
+    int variable_num_;              // optimization variables
+    int iter_num_;                  // iteration of the solver
+    Eigen::VectorXd best_variable_; //
+    double min_cost_;               //
+
+    Eigen::Vector3d local_target_pt_; 
+
+#define INIT_min_ellip_dist_ 123456789.0123456789
+    double min_ellip_dist_;
+
+    ControlPoints cps_;
+
+    /* cost function */
+    /* calculate each part of cost function with control points q as input */
+
+    static double costFunction(const std::vector<double> &x, std::vector<double> &grad, void *func_data);
+    void combineCost(const std::vector<double> &x, vector<double> &grad, double &cost);
+
+    // q contains all control points
+    void calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient, bool falg_use_jerk = true);
+    void calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
+    void calcTerminalCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
+    void calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost);
+    void calcMovingObjCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
+    void calcSwarmCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
+    void calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
+    bool check_collision_and_rebound(void);
+
+    static int earlyExit(void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls);
+    static double costFunctionRebound(void *func_data, const double *x, double *grad, const int n);
+    static double costFunctionRefine(void *func_data, const double *x, double *grad, const int n);
+
+    bool rebound_optimize(double &final_cost);
+    bool refine_optimize();
+    void combineCostRebound(const double *x, double *grad, double &f_combine, const int n);
+    void combineCostRefine(const double *x, double *grad, double &f_combine, const int n);
+
+    /* for benckmark evaluation only */
+  public:
+    typedef unique_ptr<BsplineOptimizer> Ptr;
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+} // namespace ego_planner
+#endif

motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h

@@ -0,0 +1,52 @@
+#ifndef _GRADIENT_DESCENT_OPT_H_
+#define _GRADIENT_DESCENT_OPT_H_
+
+#include <iostream>
+#include <vector>
+#include <Eigen/Eigen>
+
+using namespace std;
+
+class GradientDescentOptimizer
+{
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+  typedef double (*objfunDef)(const Eigen::VectorXd &x, Eigen::VectorXd &grad, bool &force_return, void *data);
+  enum RESULT
+  {
+    FIND_MIN,
+    FAILED,
+    RETURN_BY_ORDER,
+    REACH_MAX_ITERATION
+  };
+
+  GradientDescentOptimizer(int v_num, objfunDef objf, void *f_data)
+  {
+    variable_num_ = v_num;
+    objfun_ = objf;
+    f_data_ = f_data;
+  };
+
+  void set_maxiter(int limit) { iter_limit_ = limit; }
+  void set_maxeval(int limit) { invoke_limit_ = limit; }
+  void set_xtol_rel(double xtol_rel) { xtol_rel_ = xtol_rel; }
+  void set_xtol_abs(double xtol_abs) { xtol_abs_ = xtol_abs; }
+  void set_min_grad(double min_grad) { min_grad_ = min_grad; }
+
+  RESULT optimize(Eigen::VectorXd &x_init_optimal, double &opt_f);
+
+private:
+  int variable_num_{0};
+  int iter_limit_{1e10};
+  int invoke_limit_{1e10};
+  double xtol_rel_;
+  double xtol_abs_;
+  double min_grad_;
+  double time_limit_;
+  void *f_data_;
+  objfunDef objfun_;
+};
+
+#endif

motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/uniform_bspline.h

@@ -0,0 +1,80 @@
+#ifndef _UNIFORM_BSPLINE_H_
+#define _UNIFORM_BSPLINE_H_
+
+#include <Eigen/Eigen>
+#include <algorithm>
+#include <iostream>
+
+using namespace std;
+
+namespace ego_planner
+{
+  // An implementation of non-uniform B-spline with different dimensions
+  // It also represents uniform B-spline which is a special case of non-uniform
+  class UniformBspline
+  {
+  private:
+    // control points for B-spline with different dimensions.
+    // Each row represents one single control point
+    // The dimension is determined by column number
+    // e.g. B-spline with N points in 3D space -> Nx3 matrix
+    Eigen::MatrixXd control_points_;
+
+    int p_, n_, m_;     // p degree, n+1 control points, m = n+p+1
+    Eigen::VectorXd u_; // knots vector
+    double interval_;   // knot span \delta t
+
+    Eigen::MatrixXd getDerivativeControlPoints();
+
+    double limit_vel_, limit_acc_, limit_ratio_, feasibility_tolerance_; // physical limits and time adjustment ratio
+
+  public:
+    UniformBspline() {}
+    UniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval);
+    ~UniformBspline();
+
+    Eigen::MatrixXd get_control_points(void) { return control_points_; }
+
+    // initialize as an uniform B-spline
+    void setUniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval);
+
+    // get / set basic bspline info
+
+    void setKnot(const Eigen::VectorXd &knot);
+    Eigen::VectorXd getKnot();
+    Eigen::MatrixXd getControlPoint();
+    double getInterval();
+    bool getTimeSpan(double &um, double &um_p);
+
+    // compute position / derivative
+
+    Eigen::VectorXd evaluateDeBoor(const double &u);                                               // use u \in [up, u_mp]
+    inline Eigen::VectorXd evaluateDeBoorT(const double &t) { return evaluateDeBoor(t + u_(p_)); } // use t \in [0, duration]
+    UniformBspline getDerivative();
+
+    // 3D B-spline interpolation of points in point_set, with boundary vel&acc
+    // constraints
+    // input : (K+2) points with boundary vel/acc; ts
+    // output: (K+6) control_pts
+    static void parameterizeToBspline(const double &ts, const vector<Eigen::Vector3d> &point_set,
+                                      const vector<Eigen::Vector3d> &start_end_derivative,
+                                      Eigen::MatrixXd &ctrl_pts);
+
+    /* check feasibility, adjust time */
+
+    void setPhysicalLimits(const double &vel, const double &acc, const double &tolerance);
+    bool checkFeasibility(double &ratio, bool show = false);
+    void lengthenTime(const double &ratio);
+
+    /* for performance evaluation */
+
+    double getTimeSum();
+    double getLength(const double &res = 0.01);
+    double getJerk();
+    void getMeanAndMaxVel(double &mean_v, double &max_v);
+    void getMeanAndMaxAcc(double &mean_a, double &max_a);
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+} // namespace ego_planner
+#endif

motion_planning/3d/ego_planner/planner/bspline_opt/package.xml

@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>bspline_opt</name>
+  <version>0.0.0</version>
+  <description>The bspline_opt package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/bspline_opt</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>plan_env</build_depend>
+  <build_depend>path_searching</build_depend>
+  <build_depend>traj_utils</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>plan_env</build_export_depend>
+  <build_export_depend>path_searching</build_export_depend>
+  <build_export_depend>traj_utils</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>plan_env</exec_depend>
+  <exec_depend>path_searching</exec_depend>
+  <exec_depend>traj_utils</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

motion_planning/3d/ego_planner/planner/bspline_opt/src/gradient_descent_optimizer.cpp

@@ -0,0 +1,94 @@
+#include <bspline_opt/gradient_descent_optimizer.h>
+
+#define RESET "\033[0m"
+#define RED "\033[31m"
+
+GradientDescentOptimizer::RESULT
+GradientDescentOptimizer::optimize(Eigen::VectorXd &x_init_optimal, double &opt_f)
+{
+    if (min_grad_ < 1e-10)
+    {
+        cout << RED << "min_grad_ is invalid:" << min_grad_ << RESET << endl;
+        return FAILED;
+    }
+    if (iter_limit_ <= 2)
+    {
+        cout << RED << "iter_limit_ is invalid:" << iter_limit_ << RESET << endl;
+        return FAILED;
+    }
+
+    void *f_data = f_data_;
+    int iter = 2;
+    int invoke_count = 2;
+    bool force_return;
+    Eigen::VectorXd x_k(x_init_optimal), x_kp1(x_init_optimal.rows());
+    double cost_k, cost_kp1, cost_min;
+    Eigen::VectorXd grad_k(x_init_optimal.rows()), grad_kp1(x_init_optimal.rows());
+
+    cost_k = objfun_(x_k, grad_k, force_return, f_data);
+    if (force_return)
+        return RETURN_BY_ORDER;
+    cost_min = cost_k;
+    double max_grad = max(abs(grad_k.maxCoeff()), abs(grad_k.minCoeff()));
+    constexpr double MAX_MOVEMENT_AT_FIRST_ITERATION = 0.1; // meter
+    double alpha0 = max_grad < MAX_MOVEMENT_AT_FIRST_ITERATION ? 1.0 : (MAX_MOVEMENT_AT_FIRST_ITERATION / max_grad);
+    x_kp1 = x_k - alpha0 * grad_k;
+    cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data);
+    if (force_return)
+        return RETURN_BY_ORDER;
+    if (cost_min > cost_kp1)
+        cost_min = cost_kp1;
+
+    /*** start iteration ***/
+    while (++iter <= iter_limit_ && invoke_count <= invoke_limit_)
+    {
+        Eigen::VectorXd s = x_kp1 - x_k;
+        Eigen::VectorXd y = grad_kp1 - grad_k;
+        double alpha = s.dot(y) / y.dot(y);
+        if (isnan(alpha) || isinf(alpha))
+        {
+            cout << RED << "step size invalid! alpha=" << alpha << RESET << endl;
+            return FAILED;
+        }
+
+        if (iter % 2) // to aviod copying operations
+        {
+            do
+            {
+                x_k = x_kp1 - alpha * grad_kp1;
+                cost_k = objfun_(x_k, grad_k, force_return, f_data);
+                invoke_count++;
+                if (force_return)
+                    return RETURN_BY_ORDER;
+                alpha *= 0.5;
+            } while (cost_k > cost_kp1 - 1e-4 * alpha * grad_kp1.transpose() * grad_kp1); // Armijo condition
+
+            if (grad_k.norm() < min_grad_)
+            {
+                opt_f = cost_k;
+                return FIND_MIN;
+            }
+        }
+        else
+        {
+            do
+            {
+                x_kp1 = x_k - alpha * grad_k;
+                cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data);
+                invoke_count++;
+                if (force_return)
+                    return RETURN_BY_ORDER;
+                alpha *= 0.5;
+            } while (cost_kp1 > cost_k - 1e-4 * alpha * grad_k.transpose() * grad_k); // Armijo condition
+
+            if (grad_kp1.norm() < min_grad_)
+            {
+                opt_f = cost_kp1;
+                return FIND_MIN;
+            }
+        }
+    }
+
+    opt_f = iter_limit_ % 2 ? cost_k : cost_kp1;
+    return REACH_MAX_ITERATION;
+}

motion_planning/3d/ego_planner/planner/bspline_opt/src/uniform_bspline.cpp

@@ -0,0 +1,377 @@
+#include "bspline_opt/uniform_bspline.h"
+#include <ros/ros.h>
+
+namespace ego_planner
+{
+
+  UniformBspline::UniformBspline(const Eigen::MatrixXd &points, const int &order,
+                                 const double &interval)
+  {
+    setUniformBspline(points, order, interval);
+  }
+
+  UniformBspline::~UniformBspline() {}
+
+  void UniformBspline::setUniformBspline(const Eigen::MatrixXd &points, const int &order,
+                                         const double &interval)
+  {
+    control_points_ = points;
+    p_ = order;
+    interval_ = interval;
+
+    n_ = points.cols() - 1;
+    m_ = n_ + p_ + 1;
+
+    u_ = Eigen::VectorXd::Zero(m_ + 1);
+    for (int i = 0; i <= m_; ++i)
+    {
+
+      if (i <= p_)
+      {
+        u_(i) = double(-p_ + i) * interval_;
+      }
+      else if (i > p_ && i <= m_ - p_)
+      {
+        u_(i) = u_(i - 1) + interval_;
+      }
+      else if (i > m_ - p_)
+      {
+        u_(i) = u_(i - 1) + interval_;
+      }
+    }
+  }
+
+  void UniformBspline::setKnot(const Eigen::VectorXd &knot) { this->u_ = knot; }
+
+  Eigen::VectorXd UniformBspline::getKnot() { return this->u_; }
+
+  bool UniformBspline::getTimeSpan(double &um, double &um_p)
+  {
+    if (p_ > u_.rows() || m_ - p_ > u_.rows())
+      return false;
+
+    um = u_(p_);
+    um_p = u_(m_ - p_);
+
+    return true;
+  }
+
+  Eigen::MatrixXd UniformBspline::getControlPoint() { return control_points_; }
+
+  Eigen::VectorXd UniformBspline::evaluateDeBoor(const double &u)
+  {
+
+    double ub = min(max(u_(p_), u), u_(m_ - p_));
+
+    // determine which [ui,ui+1] lay in
+    int k = p_;
+    while (true)
+    {
+      if (u_(k + 1) >= ub)
+        break;
+      ++k;
+    }
+
+    /* deBoor's alg */
+    vector<Eigen::VectorXd> d;
+    for (int i = 0; i <= p_; ++i)
+    {
+      d.push_back(control_points_.col(k - p_ + i));
+      // cout << d[i].transpose() << endl;
+    }
+
+    for (int r = 1; r <= p_; ++r)
+    {
+      for (int i = p_; i >= r; --i)
+      {
+        double alpha = (ub - u_[i + k - p_]) / (u_[i + 1 + k - r] - u_[i + k - p_]);
+        // cout << "alpha: " << alpha << endl;
+        d[i] = (1 - alpha) * d[i - 1] + alpha * d[i];
+      }
+    }
+
+    return d[p_];
+  }
+
+  // Eigen::VectorXd UniformBspline::evaluateDeBoorT(const double& t) {
+  //   return evaluateDeBoor(t + u_(p_));
+  // }
+
+  Eigen::MatrixXd UniformBspline::getDerivativeControlPoints()
+  {
+    // The derivative of a b-spline is also a b-spline, its order become p_-1
+    // control point Qi = p_*(Pi+1-Pi)/(ui+p_+1-ui+1)
+    Eigen::MatrixXd ctp(control_points_.rows(), control_points_.cols() - 1);
+    for (int i = 0; i < ctp.cols(); ++i)
+    {
+      ctp.col(i) =
+          p_ * (control_points_.col(i + 1) - control_points_.col(i)) / (u_(i + p_ + 1) - u_(i + 1));
+    }
+    return ctp;
+  }
+
+  UniformBspline UniformBspline::getDerivative()
+  {
+    Eigen::MatrixXd ctp = getDerivativeControlPoints();
+    UniformBspline derivative(ctp, p_ - 1, interval_);
+
+    /* cut the first and last knot */
+    Eigen::VectorXd knot(u_.rows() - 2);
+    knot = u_.segment(1, u_.rows() - 2);
+    derivative.setKnot(knot);
+
+    return derivative;
+  }
+
+  double UniformBspline::getInterval() { return interval_; }
+
+  void UniformBspline::setPhysicalLimits(const double &vel, const double &acc, const double &tolerance)
+  {
+    limit_vel_ = vel;
+    limit_acc_ = acc;
+    limit_ratio_ = 1.1;
+    feasibility_tolerance_ = tolerance;
+  }
+
+  bool UniformBspline::checkFeasibility(double &ratio, bool show)
+  {
+    bool fea = true;
+
+    Eigen::MatrixXd P = control_points_;
+    int dimension = control_points_.rows();
+
+    /* check vel feasibility and insert points */
+    double max_vel = -1.0;
+    double enlarged_vel_lim = limit_vel_ * (1.0 + feasibility_tolerance_) + 1e-4;
+    for (int i = 0; i < P.cols() - 1; ++i)
+    {
+      Eigen::VectorXd vel = p_ * (P.col(i + 1) - P.col(i)) / (u_(i + p_ + 1) - u_(i + 1));
+
+      if (fabs(vel(0)) > enlarged_vel_lim || fabs(vel(1)) > enlarged_vel_lim ||
+          fabs(vel(2)) > enlarged_vel_lim)
+      {
+
+        if (show)
+          cout << "[Check]: Infeasible vel " << i << " :" << vel.transpose() << endl;
+        fea = false;
+
+        for (int j = 0; j < dimension; ++j)
+        {
+          max_vel = max(max_vel, fabs(vel(j)));
+        }
+      }
+    }
+
+    /* acc feasibility */
+    double max_acc = -1.0;
+    double enlarged_acc_lim = limit_acc_ * (1.0 + feasibility_tolerance_) + 1e-4;
+    for (int i = 0; i < P.cols() - 2; ++i)
+    {
+
+      Eigen::VectorXd acc = p_ * (p_ - 1) *
+                            ((P.col(i + 2) - P.col(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
+                             (P.col(i + 1) - P.col(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
+                            (u_(i + p_ + 1) - u_(i + 2));
+
+      if (fabs(acc(0)) > enlarged_acc_lim || fabs(acc(1)) > enlarged_acc_lim ||
+          fabs(acc(2)) > enlarged_acc_lim)
+      {
+
+        if (show)
+          cout << "[Check]: Infeasible acc " << i << " :" << acc.transpose() << endl;
+        fea = false;
+
+        for (int j = 0; j < dimension; ++j)
+        {
+          max_acc = max(max_acc, fabs(acc(j)));
+        }
+      }
+    }
+
+    ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));
+
+    return fea;
+  }
+
+  void UniformBspline::lengthenTime(const double &ratio)
+  {
+    int num1 = 5;
+    int num2 = getKnot().rows() - 1 - 5;
+
+    double delta_t = (ratio - 1.0) * (u_(num2) - u_(num1));
+    double t_inc = delta_t / double(num2 - num1);
+    for (int i = num1 + 1; i <= num2; ++i)
+      u_(i) += double(i - num1) * t_inc;
+    for (int i = num2 + 1; i < u_.rows(); ++i)
+      u_(i) += delta_t;
+  }
+
+  // void UniformBspline::recomputeInit() {}
+
+  void UniformBspline::parameterizeToBspline(const double &ts, const vector<Eigen::Vector3d> &point_set,
+                                             const vector<Eigen::Vector3d> &start_end_derivative,
+                                             Eigen::MatrixXd &ctrl_pts)
+  {
+    if (ts <= 0)
+    {
+      cout << "[B-spline]:time step error." << endl;
+      return;
+    }
+
+    if (point_set.size() <= 3)
+    {
+      cout << "[B-spline]:point set have only " << point_set.size() << " points." << endl;
+      return;
+    }
+
+    if (start_end_derivative.size() != 4)
+    {
+      cout << "[B-spline]:derivatives error." << endl;
+    }
+
+    int K = point_set.size();
+
+    // write A
+    Eigen::Vector3d prow(3), vrow(3), arow(3);
+    prow << 1, 4, 1;
+    vrow << -1, 0, 1;
+    arow << 1, -2, 1;
+
+    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2);
+
+    for (int i = 0; i < K; ++i)
+      A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();
+
+    A.block(K, 0, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
+    A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
+
+    A.block(K + 2, 0, 1, 3) = (1 / ts / ts) * arow.transpose();
+    A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();
+
+    //cout << "A" << endl << A << endl << endl;
+
+    // write b
+    Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4);
+    for (int i = 0; i < K; ++i)
+    {
+      bx(i) = point_set[i](0);
+      by(i) = point_set[i](1);
+      bz(i) = point_set[i](2);
+    }
+
+    for (int i = 0; i < 4; ++i)
+    {
+      bx(K + i) = start_end_derivative[i](0);
+      by(K + i) = start_end_derivative[i](1);
+      bz(K + i) = start_end_derivative[i](2);
+    }
+
+    // solve Ax = b
+    Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
+    Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
+    Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);
+
+    // convert to control pts
+    ctrl_pts.resize(3, K + 2);
+    ctrl_pts.row(0) = px.transpose();
+    ctrl_pts.row(1) = py.transpose();
+    ctrl_pts.row(2) = pz.transpose();
+
+    // cout << "[B-spline]: parameterization ok." << endl;
+  }
+
+  double UniformBspline::getTimeSum()
+  {
+    double tm, tmp;
+    if (getTimeSpan(tm, tmp))
+      return tmp - tm;
+    else
+      return -1.0;
+  }
+
+  double UniformBspline::getLength(const double &res)
+  {
+    double length = 0.0;
+    double dur = getTimeSum();
+    Eigen::VectorXd p_l = evaluateDeBoorT(0.0), p_n;
+    for (double t = res; t <= dur + 1e-4; t += res)
+    {
+      p_n = evaluateDeBoorT(t);
+      length += (p_n - p_l).norm();
+      p_l = p_n;
+    }
+    return length;
+  }
+
+  double UniformBspline::getJerk()
+  {
+    UniformBspline jerk_traj = getDerivative().getDerivative().getDerivative();
+
+    Eigen::VectorXd times = jerk_traj.getKnot();
+    Eigen::MatrixXd ctrl_pts = jerk_traj.getControlPoint();
+    int dimension = ctrl_pts.rows();
+
+    double jerk = 0.0;
+    for (int i = 0; i < ctrl_pts.cols(); ++i)
+    {
+      for (int j = 0; j < dimension; ++j)
+      {
+        jerk += (times(i + 1) - times(i)) * ctrl_pts(j, i) * ctrl_pts(j, i);
+      }
+    }
+
+    return jerk;
+  }
+
+  void UniformBspline::getMeanAndMaxVel(double &mean_v, double &max_v)
+  {
+    UniformBspline vel = getDerivative();
+    double tm, tmp;
+    vel.getTimeSpan(tm, tmp);
+
+    double max_vel = -1.0, mean_vel = 0.0;
+    int num = 0;
+    for (double t = tm; t <= tmp; t += 0.01)
+    {
+      Eigen::VectorXd vxd = vel.evaluateDeBoor(t);
+      double vn = vxd.norm();
+
+      mean_vel += vn;
+      ++num;
+      if (vn > max_vel)
+      {
+        max_vel = vn;
+      }
+    }
+
+    mean_vel = mean_vel / double(num);
+    mean_v = mean_vel;
+    max_v = max_vel;
+  }
+
+  void UniformBspline::getMeanAndMaxAcc(double &mean_a, double &max_a)
+  {
+    UniformBspline acc = getDerivative().getDerivative();
+    double tm, tmp;
+    acc.getTimeSpan(tm, tmp);
+
+    double max_acc = -1.0, mean_acc = 0.0;
+    int num = 0;
+    for (double t = tm; t <= tmp; t += 0.01)
+    {
+      Eigen::VectorXd axd = acc.evaluateDeBoor(t);
+      double an = axd.norm();
+
+      mean_acc += an;
+      ++num;
+      if (an > max_acc)
+      {
+        max_acc = an;
+      }
+    }
+
+    mean_acc = mean_acc / double(num);
+    mean_a = mean_acc;
+    max_a = max_acc;
+  }
+} // namespace ego_planner

motion_planning/3d/ego_planner/planner/drone_detect/CMakeLists.txt

@@ -0,0 +1,130 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(drone_detect)
+
+set(CMAKE_BUILD_TYPE "Release")
+ADD_COMPILE_OPTIONS(-std=c++11 )
+ADD_COMPILE_OPTIONS(-std=c++14 )
+## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
+## enforcing cleaner code.
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED
+  COMPONENTS
+    roscpp
+    sensor_msgs
+    roslint
+    cv_bridge
+    message_filters
+)
+
+## Find system libraries
+find_package(Eigen3 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Boost REQUIRED)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS
+    include
+    ## This is only necessary because Eigen3 sets a non-standard EIGEN3_INCLUDE_DIR variable
+    ${EIGEN3_INCLUDE_DIR}
+  LIBRARIES
+  CATKIN_DEPENDS
+    roscpp
+    sensor_msgs
+  DEPENDS OpenCV Eigen Boost
+  ## find_package(Eigen3) provides a non standard EIGEN3_INCLUDE_DIR instead of Eigen3_INCLUDE_DIRS.
+  ## Therefore, the DEPEND does not work as expected and we need to add the directory to the INCLUDE_DIRS
+  # Eigen3
+
+  ## Boost is not part of the DEPENDS since it is only used in source files,
+  ## Dependees do not depend on Boost when they depend on this package.
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  # Set manually because Eigen sets a non standard INCLUDE DIR
+  ${EIGEN3_INCLUDE_DIR}
+  # Set because Boost is an internal dependency, not transitive.
+  ${Boost_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+
+)
+
+## Declare cpp executables
+add_executable(${PROJECT_NAME}
+  src/${PROJECT_NAME}_node.cpp
+  src/drone_detector.cpp
+)
+target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_11)
+
+## Add dependencies to exported targets, like ROS msgs or srvs
+add_dependencies(${PROJECT_NAME}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}
+)
+
+#############
+## Install ##
+#############
+
+# Mark executables and/or libraries for installation
+install(
+  TARGETS ${PROJECT_NAME} 
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Mark cpp header files for installation
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+)
+
+# Mark other files for installation
+install(
+  DIRECTORY doc
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+# if(${CATKIN_ENABLE_TESTING})
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+# ## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test
+#   test/test_drone_detector.cpp)
+
+# target_link_libraries(${PROJECT_NAME}-test)
+# endif()
+
+##########################
+## Static code analysis ##
+##########################
+
+roslint_cpp()

motion_planning/3d/ego_planner/planner/drone_detect/LICENSE

@@ -0,0 +1,29 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2017, Peter Fankhauser
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of Autonomous Systems Lab nor ETH Zurich nor the names
+  of its contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

motion_planning/3d/ego_planner/planner/drone_detect/README.md

@@ -0,0 +1,97 @@
+# Drone Detect
+
+## Overview
+
+This is a package for detecting other drones in depth image and then calculating their true pose error in the world frame of the observer drone.
+
+
+
+![Example image](doc/demo.jpg)
+
+
+
+## Usage
+
+You can launch the node alongside the main after you assigning the right topic name
+
+```
+roslaunch drone_detect drone_detect.launch
+```
+
+
+
+or add the following code in `run_in_sim.launch`
+
+```xml
+  <node pkg="drone_detect" type="drone_detect" name="drone_$(arg drone_id)_drone_detect" output="screen">
+      <rosparam command="load" file="$(find local_sensing_node)/params/camera.yaml" />
+      <rosparam command="load" file="$(find drone_detect)/config/default.yaml"/>
+      <param name="my_id"      value="$(arg drone_id)" />
+
+      <remap from="~odometry"   to="/drone_$(arg drone_id)_$(arg odom_topic)"/>
+      <remap from="~depth" to="/drone_$(arg drone_id)_pcl_render_node/depth"/>
+      <remap from="~colordepth" to="/drone_$(arg drone_id)_pcl_render_node/colordepth"/>
+      <remap from="~camera_pose" to="/drone_$(arg drone_id)_pcl_render_node/camera_pose"/>
+
+      <remap from="~drone0" to="/drone_0_$(arg odom_topic)"/>
+      <remap from="~drone1" to="/drone_1_$(arg odom_topic)"/>
+      <remap from="~drone2" to="/drone_2_$(arg odom_topic)"/>
+  </node>
+```
+
+
+
+## Config files
+
+* **camera.yaml** : The camera intrinsics are stored in this file
+* **default.yaml**: Some parameters related to drone detection.
+
+```yaml
+debug_flag: true
+# the proportion of the pixel threshold to the total pixels projected from the drone to the depth map 
+estimate/pixel_ratio: 0.1
+# the radius of the pose errror sphere
+estimate/max_pose_error: 0.4
+# the width and height of the model of drone
+estimate/drone_width: 0.5  
+estimate/drone_height: 0.1 
+```
+
+
+
+## Nodes
+
+
+#### Subscribed Topics
+
+* **`/depth`** ([sensor_msgs::Image])
+
+	The depth image from pcl_render_node.
+	
+* **`/colordepth`**([sensor_msgs::Image])
+
+  The color image from pcl_render_node
+
+* **`/camera_pose`** ([geometry_msgs::PoseStamped])
+
+  The camere pose from pcl_render_node
+
+The above three topics are synchronized when in use, the callback function is **`rcvDepthColorCamPoseCallback`**
+
+- **`/dronex_odom_sub_`** ([nav_msgs::Odometry])
+
+  The odometry of other drones
+
+#### Published Topics
+
+* **`/new_depth`** ([sensor_msgs::Image])
+
+  The new depth image after erasing the moving drones
+
+* **`/new_colordepth`**([sensor_msgs::Image])
+
+  The color image with some debug marks
+
+* **`/camera_pose_error`** ([geometry_msgs::PoseStamped])
+
+  The pose error of detected drones in world frame of the observer drone.

motion_planning/3d/ego_planner/planner/drone_detect/config/camera.yaml

@@ -0,0 +1,7 @@
+cam_width: 640
+cam_height: 480
+cam_fx: 386.02557373046875
+cam_fy: 386.02557373046875
+cam_cx: 321.8554382324219
+cam_cy: 241.2396240234375
+

motion_planning/3d/ego_planner/planner/drone_detect/config/default.yaml

@@ -0,0 +1,5 @@
+# debug_flag: true
+pixel_ratio: 0.1
+estimate/max_pose_error: 0.4
+estimate/drone_width: 0.5
+estimate/drone_height: 0.2

motion_planning/3d/ego_planner/planner/drone_detect/include/drone_detector/drone_detector.h

@@ -0,0 +1,156 @@
+#pragma once
+#include <iostream>
+#include <vector>
+// ROS
+#include <ros/ros.h>
+#include "std_msgs/String.h"
+#include "std_msgs/Bool.h"
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <cv_bridge/cv_bridge.h>
+// synchronize topic
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <std_srvs/Trigger.h>
+
+//include opencv and eigen
+#include <Eigen/Eigen>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/eigen.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+namespace detect {
+
+const int max_drone_num_ = 3;
+
+/*!
+ * Main class for the node to handle the ROS interfacing.
+ */
+class DroneDetector
+{
+ public:
+  /*!
+   * Constructor.
+   * @param nodeHandle the ROS node handle.
+   */
+  DroneDetector(ros::NodeHandle& nodeHandle);
+
+  /*!
+   * Destructor.
+   */
+  virtual ~DroneDetector();
+
+  void test();
+ private:
+  void readParameters();
+
+  // inline functions
+  double getDist2(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2);
+  double getDist2(const Eigen::Vector4d &p1, const Eigen::Vector4d &p2);
+  Eigen::Vector4d depth2Pos(int u, int v, float depth);
+  Eigen::Vector4d depth2Pos(const Eigen::Vector2i &pixel, float depth) ;
+  Eigen::Vector2i pos2Depth(const Eigen::Vector4d &pose_in_camera) ;
+  bool isInSensorRange(const Eigen::Vector2i &pixel);
+
+  bool countPixel(int drone_id, Eigen::Vector2i &true_pixel, Eigen::Vector4d &true_pose_cam);
+  void detect(int drone_id, Eigen::Vector2i &true_pixel);
+  
+  // subscribe callback function
+  void rcvDepthColorCamPoseCallback(const sensor_msgs::ImageConstPtr& depth_img, \
+                                const sensor_msgs::ImageConstPtr& color_img, \
+                                const geometry_msgs::PoseStampedConstPtr& camera_pose);
+
+  void rcvDepthCamPoseCallback(const sensor_msgs::ImageConstPtr& depth_img, \
+                                const geometry_msgs::PoseStampedConstPtr& camera_pose);
+  
+  void rcvMyOdomCallback(const nav_msgs::Odometry& odom);
+  void rcvDepthImgCallback(const sensor_msgs::ImageConstPtr& depth_img);
+
+
+  void rcvDroneOdomCallbackBase(const nav_msgs::Odometry& odom, const int drone_id);
+
+  void rcvDrone0OdomCallback(const nav_msgs::Odometry& odom);
+  void rcvDrone1OdomCallback(const nav_msgs::Odometry& odom);
+  void rcvDrone2OdomCallback(const nav_msgs::Odometry& odom);
+  void rcvDroneXOdomCallback(const nav_msgs::Odometry& odom);
+  
+  //! ROS node handle.
+  ros::NodeHandle& nh_;
+
+  //! ROS topic subscriber.
+  // depth, colordepth, camera_pos subscriber
+  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, geometry_msgs::PoseStamped> SyncPolicyDepthColorImagePose;
+  typedef std::shared_ptr<message_filters::Synchronizer<SyncPolicyDepthColorImagePose>> SynchronizerDepthColorImagePose;
+  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::PoseStamped> SyncPolicyDepthImagePose;
+  typedef std::shared_ptr<message_filters::Synchronizer<SyncPolicyDepthImagePose>> SynchronizerDepthImagePose;
+  
+  // std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> depth_img_sub_;
+  std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> colordepth_img_sub_;
+  std::shared_ptr<message_filters::Subscriber<geometry_msgs::PoseStamped>> camera_pos_sub_;
+
+  SynchronizerDepthColorImagePose sync_depth_color_img_pose_;
+  SynchronizerDepthImagePose sync_depth_img_pose_;
+  // other drones subscriber
+  ros::Subscriber drone0_odom_sub_, drone1_odom_sub_, drone2_odom_sub_, droneX_odom_sub_;
+  std::string drone1_odom_topic_, drone2_odom_topic_;
+
+  ros::Subscriber my_odom_sub_, depth_img_sub_;
+  bool has_odom_;
+  nav_msgs::Odometry my_odom_;
+  // ROS topic publisher
+  // new_depth_img: erase the detected drones
+  // new_colordepth_img: for debug
+  ros::Publisher new_depth_img_pub_;
+  ros::Publisher debug_depth_img_pub_;
+
+  // parameters
+  //camera param
+  int img_width_, img_height_;
+  double fx_,fy_,cx_,cy_;
+
+  double max_pose_error_;
+  double max_pose_error2_;
+  double drone_width_, drone_height_;
+  double pixel_ratio_;
+  int pixel_threshold_;
+
+  // for debug
+  bool debug_flag_;
+  int debug_detect_result_[max_drone_num_];
+  std::stringstream debug_img_text_[max_drone_num_];
+  ros::Time debug_start_time_, debug_end_time_;
+
+  ros::Publisher debug_info_pub_;
+  ros::Publisher drone_pose_err_pub_[max_drone_num_];
+
+  int my_id_;
+  cv::Mat depth_img_, color_img_;
+
+  Eigen::Matrix4d cam2body_;
+  Eigen::Matrix4d cam2world_;
+  Eigen::Quaterniond cam2world_quat_;
+  Eigen::Vector4d my_pose_world_;
+  Eigen::Quaterniond my_attitude_world_;
+  Eigen::Vector4d my_last_pose_world_;
+  ros::Time my_last_odom_stamp_ = ros::TIME_MAX;
+  ros::Time my_last_camera_stamp_ = ros::TIME_MAX;
+
+  Eigen::Matrix4d drone2world_[max_drone_num_];
+  Eigen::Vector4d drone_pose_world_[max_drone_num_];
+  Eigen::Quaterniond drone_attitude_world_[max_drone_num_];
+  Eigen::Vector4d drone_pose_cam_[max_drone_num_];
+  Eigen::Vector2i drone_ref_pixel_[max_drone_num_];
+
+  std::vector<Eigen::Vector2i> hit_pixels_[max_drone_num_];
+  int valid_pixel_cnt_[max_drone_num_];
+  
+  bool in_depth_[max_drone_num_] = {false};
+  cv::Point searchbox_lu_[max_drone_num_], searchbox_rd_[max_drone_num_];
+  cv::Point boundingbox_lu_[max_drone_num_], boundingbox_rd_[max_drone_num_];
+};
+
+} /* namespace */

motion_planning/3d/ego_planner/planner/drone_detect/launch/drone_detect.launch

@@ -0,0 +1,24 @@
+<launch>
+  <arg name="my_id" value="1"/>
+  <arg name="odom_topic" value="/vins_estimator/imu_propagate"/>
+
+  <!-- Launch ROS Package Template Node -->
+  <node pkg="drone_detect" type="drone_detect" name="test_drone_detect" output="screen">
+    <rosparam command="load" file="$(find drone_detect)/config/camera.yaml" />
+    <rosparam command="load" file="$(find drone_detect)/config/default.yaml"/>
+    <param name="my_id"      value="$(arg my_id)" />
+    <param name="debug_flag" value="false" />
+    <remap from="~odometry"   to="$(arg odom_topic)"/>
+    <remap from="~depth" to="/camera/depth/image_rect_raw"/>
+    <!-- <remap from="~camera_pose" to="/drone_$(arg my_id)_pcl_render_node/camera_pose"/> -->
+
+    <!-- <remap from="~drone0" to="/drone_0_$(arg odom_topic)"/>
+    <remap from="~drone1" to="/drone_1_$(arg odom_topic)"/>
+    <remap from="~drone2" to="/drone_2_$(arg odom_topic)"/> -->
+
+    <!-- <remap from="~drone1" to="/test/dynamic0_odom"/> -->
+    <!-- <remap from="~drone2" to="/test/dynamic1_odom"/> -->
+
+  </node>
+
+</launch>

motion_planning/3d/ego_planner/planner/drone_detect/launch/ros_package_template_overlying_params.launch

@@ -0,0 +1,27 @@
+<launch>
+
+  <!-- This launch file shows how to launch ROS nodes with default parameters and some user's
+       custom parameters:
+        * Default parameters are loaded form the file specified by 'default_param_file';
+        * User's overlying parameters file path is specified by 'overlying_param_file', which can
+          be set from the launch file that includes this file.
+
+       The user can overwrite even just a subset of the default parameters. Only parameters
+       contained in 'overlying_param_file' will overwrite the corresponding default ones.
+       This means that if the user does not specify some parameters, then the default ones
+       will be loaded. -->
+
+  <!-- Default parameters. Note the usage of 'value', to avoid they can be wrongly changed. -->
+  <arg name="default_param_file" value="$(dirname)/../config/default.yaml" />
+
+  <!-- User's parameters that can overly default ones: use default ones in case user does not specify them. -->
+  <arg name="overlying_param_file" default="$(arg default_param_file)" />
+
+  <!-- Launch ROS Package Template node. -->
+  <node pkg="ros_package_template" type="ros_package_template" name="ros_package_template" output="screen">
+    <rosparam command="load" file="$(arg default_param_file)" />
+    <!-- Overlay parameters if user specified them. They must be loaded after default parameters! -->
+    <rosparam command="load" file="$(arg overlying_param_file)" />
+  </node>
+
+</launch>

motion_planning/3d/ego_planner/planner/drone_detect/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>drone_detect</name>
+  <version>0.1.0</version>
+  <description>Detect other drones in depth image.</description>
+  <maintainer email="zjujiangchaozhu@gmail.com">Jiangchao Zhu</maintainer>
+  <license>BSD</license>
+  <author email="zjujiangchaozhu@gmail.com">Jiangchao Zhu</author>
+
+  <!-- buildtool_depend: dependencies of the build process -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <!-- build_depend: dependencies only used in source files -->
+  <build_depend>boost</build_depend>
+  <!-- depend: build, export, and execution dependency -->
+  <depend>eigen</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+
+  <build_depend>roslint</build_depend>
+</package>

motion_planning/3d/ego_planner/planner/drone_detect/src/drone_detect_node.cpp

@@ -0,0 +1,14 @@
+#include <ros/ros.h>
+#include "drone_detector/drone_detector.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "drone_detect");
+  ros::NodeHandle nh("~");
+
+  detect::DroneDetector drone_detector(nh);
+  drone_detector.test();
+
+  ros::spin();
+  return 0;
+}

motion_planning/3d/ego_planner/planner/drone_detect/src/drone_detector.cpp

@@ -0,0 +1,423 @@
+#include "drone_detector/drone_detector.h"
+
+// STD
+#include <string>
+
+namespace detect {
+
+DroneDetector::DroneDetector(ros::NodeHandle& nodeHandle)
+    : nh_(nodeHandle)
+{
+  readParameters();
+
+  // depth_img_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(nh_, "depth", 50, ros::TransportHints().tcpNoDelay()));
+  // colordepth_img_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(nh_, "colordepth", 50));
+  // camera_pos_sub_.reset(new message_filters::Subscriber<geometry_msgs::PoseStamped>(nh_, "camera_pose", 50));
+
+  my_odom_sub_ = nh_.subscribe("odometry", 100, &DroneDetector::rcvMyOdomCallback, this, ros::TransportHints().tcpNoDelay());
+  depth_img_sub_ = nh_.subscribe("depth", 50, &DroneDetector::rcvDepthImgCallback, this, ros::TransportHints().tcpNoDelay());
+  // sync_depth_color_img_pose_->registerCallback(boost::bind(&DroneDetector::rcvDepthColorCamPoseCallback, this, _1, _2, _3));
+
+  // drone0_odom_sub_ = nh_.subscribe("drone0", 50, &DroneDetector::rcvDrone0OdomCallback, this); 
+  // drone1_odom_sub_ = nh_.subscribe("drone1", 50, &DroneDetector::rcvDrone1OdomCallback, this); 
+  // drone2_odom_sub_ = nh_.subscribe("drone2", 50, &DroneDetector::rcvDrone2OdomCallback, this); 
+  droneX_odom_sub_ = nh_.subscribe("/others_odom", 100, &DroneDetector::rcvDroneXOdomCallback, this, ros::TransportHints().tcpNoDelay());
+
+  new_depth_img_pub_ = nh_.advertise<sensor_msgs::Image>("new_depth_image", 50);
+  debug_depth_img_pub_ = nh_.advertise<sensor_msgs::Image>("debug_depth_image", 50);
+
+  debug_info_pub_ = nh_.advertise<std_msgs::String>("/debug_info", 50);
+
+  cam2body_ << 0.0, 0.0, 1.0, 0.0,
+      -1.0, 0.0, 0.0, 0.0,
+      0.0, -1.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 1.0;
+
+  // init drone_pose_err_pub
+  for(int i = 0; i < max_drone_num_; i++) {
+    if(i != my_id_)
+      drone_pose_err_pub_[i] = nh_.advertise<geometry_msgs::PoseStamped>("drone"+std::to_string(i)+"to"+std::to_string(my_id_)+"_pose_err", 50);
+  }
+
+  ROS_INFO("Successfully launched node.");
+}
+
+DroneDetector::~DroneDetector()
+{
+}
+
+void DroneDetector::readParameters()
+{
+  // camera params
+  nh_.getParam("cam_width", img_width_);
+  nh_.getParam("cam_height", img_height_);
+  nh_.getParam("cam_fx", fx_);
+  nh_.getParam("cam_fy", fy_);
+  nh_.getParam("cam_cx", cx_);
+  nh_.getParam("cam_cy", cy_);
+
+  // 
+  nh_.getParam("debug_flag", debug_flag_);
+  nh_.getParam("pixel_ratio", pixel_ratio_);
+  nh_.getParam("my_id", my_id_);
+  nh_.getParam("estimate/drone_width", drone_width_);
+  nh_.getParam("estimate/drone_height", drone_height_);
+  nh_.getParam("estimate/max_pose_error", max_pose_error_);
+
+  max_pose_error2_ = max_pose_error_*max_pose_error_;
+}
+
+// inline functions
+inline double DroneDetector::getDist2(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2)
+{
+    double delta_x = p1(0)-p2(0);
+    double delta_y = p1(1)-p2(1);
+    double delta_z = p1(2)-p2(2);
+    return delta_x*delta_x+delta_y*delta_y+delta_z*delta_z;
+}
+
+inline double DroneDetector::getDist2(const Eigen::Vector4d &p1, const Eigen::Vector4d &p2)
+{
+    double delta_x = p1(0)-p2(0);
+    double delta_y = p1(1)-p2(1);
+    double delta_z = p1(2)-p2(2);
+    return delta_x*delta_x+delta_y*delta_y+delta_z*delta_z;
+}
+
+inline Eigen::Vector4d DroneDetector::depth2Pos(int u, int v, float depth) 
+{
+  Eigen::Vector4d pose_in_camera;
+  pose_in_camera(0) = (u - cx_) * depth / fx_;
+  pose_in_camera(1) = (v - cy_) * depth / fy_;
+  pose_in_camera(2) = depth; 
+  pose_in_camera(3) = 1.0;
+  return pose_in_camera;
+}
+
+inline Eigen::Vector4d DroneDetector::depth2Pos(const Eigen::Vector2i &pixel, float depth) 
+{
+  Eigen::Vector4d pose_in_camera;
+  pose_in_camera(0) = (pixel(0) - cx_) * depth / fx_;
+  pose_in_camera(1) = (pixel(1) - cy_) * depth / fy_;
+  pose_in_camera(2) = depth; 
+  pose_in_camera(3) = 1.0;
+  return pose_in_camera;
+}
+
+inline Eigen::Vector2i DroneDetector::pos2Depth(const Eigen::Vector4d &pose_in_camera) 
+{
+  float depth = pose_in_camera(2);
+  Eigen::Vector2i pixel;
+  pixel(0) = pose_in_camera(0) * fx_ / depth + cx_ + 0.5;
+  pixel(1) = pose_in_camera(1) * fy_ / depth + cy_ + 0.5;
+  return pixel;
+}
+
+inline bool DroneDetector::isInSensorRange(const Eigen::Vector2i &pixel)
+{
+  if (pixel(0)>=0 && pixel(1) >= 0 && pixel(0) <= img_width_ && pixel(1) <= img_height_) return true;
+  else 
+    return false;
+}
+
+void DroneDetector::rcvMyOdomCallback(const nav_msgs::Odometry& odom)
+{
+  my_odom_ = odom;
+  Eigen::Matrix4d body2world = Eigen::Matrix4d::Identity();
+
+  my_pose_world_(0) = odom.pose.pose.position.x;
+  my_pose_world_(1) = odom.pose.pose.position.y;
+  my_pose_world_(2) = odom.pose.pose.position.z;
+  my_pose_world_(3) = 1.0;
+  my_attitude_world_.x() = odom.pose.pose.orientation.x;
+  my_attitude_world_.y() = odom.pose.pose.orientation.y;
+  my_attitude_world_.z() = odom.pose.pose.orientation.z;
+  my_attitude_world_.w() = odom.pose.pose.orientation.w;
+  body2world.block<3,3>(0,0) = my_attitude_world_.toRotationMatrix();
+  body2world(0,3) = my_pose_world_(0);
+  body2world(1,3) = my_pose_world_(1);
+  body2world(2,3) = my_pose_world_(2);
+
+  //convert to cam pose
+  cam2world_ = body2world * cam2body_;
+  cam2world_quat_ = cam2world_.block<3,3>(0,0);
+
+  // my_last_odom_stamp_ = odom.header.stamp;
+
+  // my_last_pose_world_(0) = odom.pose.pose.position.x;
+  // my_last_pose_world_(1) = odom.pose.pose.position.y;
+  // my_last_pose_world_(2) = odom.pose.pose.position.z;
+  // my_last_pose_world_(3) = 1.0;
+
+  //publish tf
+  // static tf::TransformBroadcaster br;
+  // tf::Transform transform;
+  // transform.setOrigin( tf::Vector3(cam2world(0,3), cam2world(1,3), cam2world(2,3) ));
+  // transform.setRotation(tf::Quaternion(cam2world_quat.x(), cam2world_quat.y(), cam2world_quat.z(), cam2world_quat.w()));
+  // br.sendTransform(tf::StampedTransform(transform, my_last_odom_stamp, "world", "camera")); 
+  //publish transform from world frame to quadrotor frame.
+}
+void DroneDetector::rcvDepthImgCallback(const sensor_msgs::ImageConstPtr& depth_img)
+{
+  /* get depth image */
+  cv_bridge::CvImagePtr cv_ptr;
+  cv_ptr = cv_bridge::toCvCopy(depth_img, depth_img->encoding);
+  cv_ptr->image.copyTo(depth_img_);
+
+  debug_start_time_ = ros::Time::now();
+
+  Eigen::Vector2i true_pixel[max_drone_num_];
+  for (int i = 0; i < max_drone_num_; i++) {
+    if (in_depth_[i]) {
+      detect(i, true_pixel[i]);
+    }
+  }   
+
+  cv_bridge::CvImage out_msg;
+  for (int i = 0; i < max_drone_num_; i++) {
+    if (in_depth_[i]) {
+      // erase hit pixels in depth
+      for(int k = 0; k < int(hit_pixels_[i].size()); k++) {
+        // depth_img_.at<float>(hit_pixels_[i][k](1), hit_pixels_[i][k](0)) = 0;
+        uint16_t *row_ptr;
+        row_ptr = depth_img_.ptr<uint16_t>(hit_pixels_[i][k](1));
+        (*(row_ptr+hit_pixels_[i][k](0))) = 0.0;
+      } 
+    }
+  }  
+  debug_end_time_ = ros::Time::now();
+  // ROS_WARN("cost_total_time = %lf", (debug_end_time_ - debug_start_time_).toSec()*1000.0);
+  out_msg.header = depth_img->header;
+  out_msg.encoding = depth_img->encoding;
+  out_msg.image = depth_img_.clone();
+  new_depth_img_pub_.publish(out_msg.toImageMsg());
+
+  std_msgs::String msg;
+  std::stringstream ss;
+  if(debug_flag_) {
+    for (int i = 0; i < max_drone_num_; i++) {
+      if (in_depth_[i]) {
+        // add bound box in colormap
+        // cv::Rect rect(_bbox_lu.x, _bbox_lu.y, _bbox_rd.x, _bbox_rd.y);//左上坐标（x,y）和矩形的长(x)宽(y)
+        cv::rectangle(depth_img_, cv::Rect(searchbox_lu_[i], searchbox_rd_[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0);
+        cv::rectangle(depth_img_, cv::Rect(boundingbox_lu_[i], boundingbox_rd_[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0);
+        if (debug_detect_result_[i] == 1) {
+          ss << "no enough " << hit_pixels_[i].size();
+        } else if(debug_detect_result_[i] == 2) {
+          ss << "success";
+        }
+      } else {
+        ss << "no detect";
+      }
+    } 
+      out_msg.header = depth_img->header;
+      out_msg.encoding = depth_img->encoding;
+      out_msg.image = depth_img_.clone();
+      debug_depth_img_pub_.publish(out_msg.toImageMsg()); 
+      msg.data = ss.str();
+      debug_info_pub_.publish(msg);
+  }
+}
+
+void DroneDetector::rcvDroneOdomCallbackBase(const nav_msgs::Odometry& odom, int drone_id)
+{
+  if (drone_id == my_id_) {
+    return;
+  }
+  Eigen::Matrix4d drone2world = Eigen::Matrix4d::Identity();
+  drone_pose_world_[drone_id](0) = odom.pose.pose.position.x;
+  drone_pose_world_[drone_id](1) = odom.pose.pose.position.y;
+  drone_pose_world_[drone_id](2) = odom.pose.pose.position.z;
+  drone_pose_world_[drone_id](3) = 1.0;
+
+  drone_attitude_world_[drone_id].x() = odom.pose.pose.orientation.x;
+  drone_attitude_world_[drone_id].y() = odom.pose.pose.orientation.y;
+  drone_attitude_world_[drone_id].z() = odom.pose.pose.orientation.z;
+  drone_attitude_world_[drone_id].w() = odom.pose.pose.orientation.w;
+  drone2world.block<3,3>(0,0) = drone_attitude_world_[drone_id].toRotationMatrix();
+  
+  drone2world(0,3) = drone_pose_world_[drone_id](0);
+  drone2world(1,3) = drone_pose_world_[drone_id](1);
+  drone2world(2,3) = drone_pose_world_[drone_id](2);
+
+  drone_pose_cam_[drone_id] = cam2world_.inverse() * drone_pose_world_[drone_id];
+  // if the drone is in sensor range
+  drone_ref_pixel_[drone_id] = pos2Depth(drone_pose_cam_[drone_id]);
+  if (drone_pose_cam_[drone_id](2) > 0 && isInSensorRange(drone_ref_pixel_[drone_id])) {
+    in_depth_[drone_id] = true;
+  } else {
+    in_depth_[drone_id] = false;
+    debug_detect_result_[drone_id] = 0;
+  }
+}
+
+void DroneDetector::rcvDrone0OdomCallback(const nav_msgs::Odometry& odom)
+{
+  rcvDroneOdomCallbackBase(odom, 0);
+}
+
+void DroneDetector::rcvDrone1OdomCallback(const nav_msgs::Odometry& odom)
+{
+  rcvDroneOdomCallbackBase(odom, 1);
+}
+
+void DroneDetector::rcvDrone2OdomCallback(const nav_msgs::Odometry& odom)
+{
+  rcvDroneOdomCallbackBase(odom, 2);
+}
+
+void DroneDetector::rcvDroneXOdomCallback(const nav_msgs::Odometry& odom)
+{
+  std::string numstr = odom.child_frame_id.substr(6);
+  try
+  {
+    int drone_id = std::stoi(numstr);
+    rcvDroneOdomCallbackBase(odom, drone_id);
+  }
+  catch(const std::exception& e)
+  {
+    std::cout << e.what() << '\n';
+  }
+}
+
+bool DroneDetector::countPixel(int drone_id, Eigen::Vector2i &true_pixel, Eigen::Vector4d &true_pose_cam) 
+{
+  boundingbox_lu_[drone_id].x = img_width_;
+  boundingbox_rd_[drone_id].x = 0;
+  boundingbox_lu_[drone_id].y = img_height_;
+  boundingbox_rd_[drone_id].y = 0;
+
+  valid_pixel_cnt_[drone_id] = 0;
+  hit_pixels_[drone_id].clear();
+
+  Eigen::Vector2i tmp_pixel;
+  Eigen::Vector4d tmp_pose_cam;
+  int search_radius = 2*max_pose_error_*fx_/drone_pose_cam_[drone_id](2);
+  float depth;
+  searchbox_lu_[drone_id].x = drone_ref_pixel_[drone_id](0) - search_radius;
+  searchbox_lu_[drone_id].y = drone_ref_pixel_[drone_id](1) - search_radius;
+  searchbox_rd_[drone_id].x = drone_ref_pixel_[drone_id](0) + search_radius;
+  searchbox_rd_[drone_id].y = drone_ref_pixel_[drone_id](1) + search_radius;
+  // check the tmp_p around ref_pixel
+  for(int i = -search_radius; i <= search_radius; i++)
+    for(int j = -search_radius; j <= search_radius; j++)
+    {
+      tmp_pixel(0) = drone_ref_pixel_[drone_id](0) + j;
+      tmp_pixel(1) = drone_ref_pixel_[drone_id](1) + i;
+      if(tmp_pixel(0) < 0 || tmp_pixel(0) >= img_width_ || tmp_pixel(1) < 0 || tmp_pixel(1) >= img_height_)
+        continue;
+      // depth = depth_img_.at<float>(tmp_pixel(1), tmp_pixel(0));
+      uint16_t *row_ptr;
+      row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
+      depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
+      // ROS_WARN("depth = %lf", depth);
+      // get tmp_pose in cam frame
+      tmp_pose_cam = depth2Pos(tmp_pixel(0), tmp_pixel(1), depth);
+      double dist2 = getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]);
+      // ROS_WARN("dist2 = %lf", dist2);
+      if (dist2 < max_pose_error2_) {
+        valid_pixel_cnt_[drone_id]++;
+        hit_pixels_[drone_id].push_back(tmp_pixel);
+        boundingbox_lu_[drone_id].x = tmp_pixel(0) < boundingbox_lu_[drone_id].x ? tmp_pixel(0) : boundingbox_lu_[drone_id].x;
+        boundingbox_lu_[drone_id].y = tmp_pixel(1) < boundingbox_lu_[drone_id].y ? tmp_pixel(1) : boundingbox_lu_[drone_id].y;
+        boundingbox_rd_[drone_id].x = tmp_pixel(0) > boundingbox_rd_[drone_id].x ? tmp_pixel(0) : boundingbox_rd_[drone_id].x;
+        boundingbox_rd_[drone_id].y = tmp_pixel(1) > boundingbox_rd_[drone_id].y ? tmp_pixel(1) : boundingbox_rd_[drone_id].y;
+      }
+    } 
+  pixel_threshold_ = (drone_width_*fx_/drone_pose_cam_[drone_id](2)) * (drone_height_*fy_/drone_pose_cam_[drone_id](2))*pixel_ratio_; 
+  if (valid_pixel_cnt_[drone_id] > pixel_threshold_) {
+    int step = 1, size = (boundingbox_rd_[drone_id].y-boundingbox_lu_[drone_id].y) < (boundingbox_rd_[drone_id].x-boundingbox_lu_[drone_id].x) ? (boundingbox_rd_[drone_id].y-boundingbox_lu_[drone_id].y) : (boundingbox_rd_[drone_id].x-boundingbox_lu_[drone_id].x);
+    int init_x = (boundingbox_lu_[drone_id].x+boundingbox_rd_[drone_id].x)/2, init_y = (boundingbox_lu_[drone_id].y+boundingbox_rd_[drone_id].y)/2;
+    int x_flag = 1, y_flag = 1;
+    int x_idx = 0, y_idx = 0;
+    uint16_t *row_ptr;
+    row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
+    depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
+    tmp_pose_cam = depth2Pos(init_x, init_y, depth);
+    if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){
+      true_pixel(0) = init_x;
+      true_pixel(1) = init_y;
+      true_pose_cam = tmp_pose_cam;
+      return true;
+    }
+    while(step<size) {
+        while(x_idx<step){
+            init_x = init_x+x_flag;
+            uint16_t *row_ptr;
+            row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
+            depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
+            tmp_pose_cam = depth2Pos(init_x, init_y, depth);
+            if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_) {
+              true_pixel(0) = init_x;
+              true_pixel(1) = init_y;
+              true_pose_cam = tmp_pose_cam;
+              return true;
+            }
+            x_idx++;
+        }
+        x_idx = 0;
+        x_flag = -x_flag;
+        while(y_idx<step){
+            init_y = init_y+y_flag;
+            uint16_t *row_ptr;
+            row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
+            depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
+            tmp_pose_cam = depth2Pos(init_x, init_y, depth);
+            if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){
+              true_pixel(0) = init_x;
+              true_pixel(1) = init_y;
+              true_pose_cam = tmp_pose_cam;
+              return true;
+            }
+            y_idx++;
+        }
+        y_idx = 0;
+        y_flag = -y_flag;
+        step++;
+    }
+    while(x_idx<step-1){
+        init_x = init_x+x_flag;
+        uint16_t *row_ptr;
+        row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
+        depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
+        tmp_pose_cam = depth2Pos(init_x, init_y, depth);
+        if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){
+          true_pixel(0) = init_x;
+          true_pixel(1) = init_y;
+          true_pose_cam = tmp_pose_cam;
+          return true;
+        }
+        x_idx++;
+    }  
+  }
+  return false;
+}
+
+void DroneDetector::detect(int drone_id, Eigen::Vector2i &true_pixel)
+{
+  Eigen::Vector4d true_pose_cam, pose_error;
+  bool found = countPixel(drone_id, true_pixel, true_pose_cam); 
+  if (found) {
+    // ROS_WARN("FOUND");
+    pose_error = cam2world_*true_pose_cam - drone_pose_world_[drone_id];
+    debug_detect_result_[drone_id] = 2;
+
+    geometry_msgs::PoseStamped out_msg;
+    out_msg.header.stamp = my_last_camera_stamp_;
+    out_msg.header.frame_id = "/drone_detect";
+    out_msg.pose.position.x = pose_error(0);
+    out_msg.pose.position.y = pose_error(1);
+    out_msg.pose.position.z = pose_error(2);
+    drone_pose_err_pub_[drone_id].publish(out_msg);
+
+  } else {
+    // ROS_WARN("NOT FOUND");
+    debug_detect_result_[drone_id] = 1;
+  }
+}
+
+void DroneDetector::test() {
+  ROS_WARN("my_id = %d", my_id_);
+}
+
+} /* namespace */

motion_planning/3d/ego_planner/planner/drone_detect/test/test_drone_detector.cpp

@@ -0,0 +1,10 @@
+// gtest
+#include <gtest/gtest.h>
+
+// Run all the tests that were declared with TEST()
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  srand((int)time(0));
+  return RUN_ALL_TESTS();
+}

motion_planning/3d/ego_planner/planner/path_searching/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(path_searching)
+
+set(CMAKE_BUILD_TYPE "Release")
+ADD_COMPILE_OPTIONS(-std=c++11 )
+ADD_COMPILE_OPTIONS(-std=c++14 )
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  visualization_msgs
+  plan_env
+  cv_bridge
+)
+
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES path_searching
+ CATKIN_DEPENDS plan_env
+#  DEPENDS system_lib
+)
+
+include_directories( 
+    SYSTEM 
+    include 
+    ${catkin_INCLUDE_DIRS}
+    ${Eigen3_INCLUDE_DIRS} 
+    ${PCL_INCLUDE_DIRS}
+)
+
+add_library( path_searching 
+    src/dyn_a_star.cpp
+    )
+target_link_libraries( path_searching
+    ${catkin_LIBRARIES} 
+    )  

motion_planning/3d/ego_planner/planner/path_searching/include/path_searching/dyn_a_star.h

@@ -0,0 +1,115 @@
+#ifndef _DYN_A_STAR_H_
+#define _DYN_A_STAR_H_
+
+#include <iostream>
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <Eigen/Eigen>
+#include <plan_env/grid_map.h>
+#include <queue>
+
+constexpr double inf = 1 >> 20;
+struct GridNode;
+typedef GridNode *GridNodePtr;
+
+struct GridNode
+{
+	enum enum_state
+	{
+		OPENSET = 1,
+		CLOSEDSET = 2,
+		UNDEFINED = 3
+	};
+
+	int rounds{0}; // Distinguish every call
+	enum enum_state state
+	{
+		UNDEFINED
+	};
+	Eigen::Vector3i index;
+
+	double gScore{inf}, fScore{inf};
+	GridNodePtr cameFrom{NULL};
+};
+
+class NodeComparator
+{
+public:
+	bool operator()(GridNodePtr node1, GridNodePtr node2)
+	{
+		return node1->fScore > node2->fScore;
+	}
+};
+
+class AStar
+{
+private:
+	GridMap::Ptr grid_map_;
+
+	inline void coord2gridIndexFast(const double x, const double y, const double z, int &id_x, int &id_y, int &id_z);
+
+	double getDiagHeu(GridNodePtr node1, GridNodePtr node2);
+	double getManhHeu(GridNodePtr node1, GridNodePtr node2);
+	double getEuclHeu(GridNodePtr node1, GridNodePtr node2);
+	inline double getHeu(GridNodePtr node1, GridNodePtr node2);
+
+	bool ConvertToIndexAndAdjustStartEndPoints(const Eigen::Vector3d start_pt, const Eigen::Vector3d end_pt, Eigen::Vector3i &start_idx, Eigen::Vector3i &end_idx);
+
+	inline Eigen::Vector3d Index2Coord(const Eigen::Vector3i &index) const;
+	inline bool Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const;
+
+	//bool (*checkOccupancyPtr)( const Eigen::Vector3d &pos );
+
+	inline bool checkOccupancy(const Eigen::Vector3d &pos) { return (bool)grid_map_->getInflateOccupancy(pos); }
+
+	std::vector<GridNodePtr> retrievePath(GridNodePtr current);
+
+	double step_size_, inv_step_size_;
+	Eigen::Vector3d center_;
+	Eigen::Vector3i CENTER_IDX_, POOL_SIZE_;
+	const double tie_breaker_ = 1.0 + 1.0 / 10000;
+
+	std::vector<GridNodePtr> gridPath_;
+
+	GridNodePtr ***GridNodeMap_;
+	std::priority_queue<GridNodePtr, std::vector<GridNodePtr>, NodeComparator> openSet_;
+
+	int rounds_{0};
+
+public:
+	typedef std::shared_ptr<AStar> Ptr;
+
+	AStar(){};
+	~AStar();
+
+	void initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size);
+
+	bool AstarSearch(const double step_size, Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
+
+	std::vector<Eigen::Vector3d> getPath();
+};
+
+inline double AStar::getHeu(GridNodePtr node1, GridNodePtr node2)
+{
+	return tie_breaker_ * getDiagHeu(node1, node2);
+}
+
+inline Eigen::Vector3d AStar::Index2Coord(const Eigen::Vector3i &index) const
+{
+	return ((index - CENTER_IDX_).cast<double>() * step_size_) + center_;
+};
+
+inline bool AStar::Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const
+{
+	idx = ((pt - center_) * inv_step_size_ + Eigen::Vector3d(0.5, 0.5, 0.5)).cast<int>() + CENTER_IDX_;
+
+	if (idx(0) < 0 || idx(0) >= POOL_SIZE_(0) || idx(1) < 0 || idx(1) >= POOL_SIZE_(1) || idx(2) < 0 || idx(2) >= POOL_SIZE_(2))
+	{
+		ROS_ERROR("Ran out of pool, index=%d %d %d", idx(0), idx(1), idx(2));
+		return false;
+	}
+
+	return true;
+};
+
+#endif

motion_planning/3d/ego_planner/planner/path_searching/package.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>path_searching</name>
+  <version>0.0.0</version>
+  <description>The path_searching package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/path_searching</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>plan_env</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>plan_env</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>plan_env</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

motion_planning/3d/ego_planner/planner/path_searching/src/dyn_a_star.cpp

@@ -0,0 +1,260 @@
+#include "path_searching/dyn_a_star.h"
+
+using namespace std;
+using namespace Eigen;
+
+AStar::~AStar()
+{
+    for (int i = 0; i < POOL_SIZE_(0); i++)
+        for (int j = 0; j < POOL_SIZE_(1); j++)
+            for (int k = 0; k < POOL_SIZE_(2); k++)
+                delete GridNodeMap_[i][j][k];
+}
+
+void AStar::initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size)
+{
+    POOL_SIZE_ = pool_size;
+    CENTER_IDX_ = pool_size / 2;
+
+    GridNodeMap_ = new GridNodePtr **[POOL_SIZE_(0)];
+    for (int i = 0; i < POOL_SIZE_(0); i++)
+    {
+        GridNodeMap_[i] = new GridNodePtr *[POOL_SIZE_(1)];
+        for (int j = 0; j < POOL_SIZE_(1); j++)
+        {
+            GridNodeMap_[i][j] = new GridNodePtr[POOL_SIZE_(2)];
+            for (int k = 0; k < POOL_SIZE_(2); k++)
+            {
+                GridNodeMap_[i][j][k] = new GridNode;
+            }
+        }
+    }
+
+    grid_map_ = occ_map;
+}
+
+double AStar::getDiagHeu(GridNodePtr node1, GridNodePtr node2)
+{
+    double dx = abs(node1->index(0) - node2->index(0));
+    double dy = abs(node1->index(1) - node2->index(1));
+    double dz = abs(node1->index(2) - node2->index(2));
+
+    double h = 0.0;
+    int diag = min(min(dx, dy), dz);
+    dx -= diag;
+    dy -= diag;
+    dz -= diag;
+
+    if (dx == 0)
+    {
+        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dy, dz) + 1.0 * abs(dy - dz);
+    }
+    if (dy == 0)
+    {
+        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dz) + 1.0 * abs(dx - dz);
+    }
+    if (dz == 0)
+    {
+        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dy) + 1.0 * abs(dx - dy);
+    }
+    return h;
+}
+
+double AStar::getManhHeu(GridNodePtr node1, GridNodePtr node2)
+{
+    double dx = abs(node1->index(0) - node2->index(0));
+    double dy = abs(node1->index(1) - node2->index(1));
+    double dz = abs(node1->index(2) - node2->index(2));
+
+    return dx + dy + dz;
+}
+
+double AStar::getEuclHeu(GridNodePtr node1, GridNodePtr node2)
+{
+    return (node2->index - node1->index).norm();
+}
+
+vector<GridNodePtr> AStar::retrievePath(GridNodePtr current)
+{
+    vector<GridNodePtr> path;
+    path.push_back(current);
+
+    while (current->cameFrom != NULL)
+    {
+        current = current->cameFrom;
+        path.push_back(current);
+    }
+
+    return path;
+}
+
+bool AStar::ConvertToIndexAndAdjustStartEndPoints(Vector3d start_pt, Vector3d end_pt, Vector3i &start_idx, Vector3i &end_idx)
+{
+    if (!Coord2Index(start_pt, start_idx) || !Coord2Index(end_pt, end_idx))
+        return false;
+
+    if (checkOccupancy(Index2Coord(start_idx)))
+    {
+        //ROS_WARN("Start point is insdide an obstacle.");
+        do
+        {
+            start_pt = (start_pt - end_pt).normalized() * step_size_ + start_pt;
+            if (!Coord2Index(start_pt, start_idx))
+                return false;
+        } while (checkOccupancy(Index2Coord(start_idx)));
+    }
+
+    if (checkOccupancy(Index2Coord(end_idx)))
+    {
+        //ROS_WARN("End point is insdide an obstacle.");
+        do
+        {
+            end_pt = (end_pt - start_pt).normalized() * step_size_ + end_pt;
+            if (!Coord2Index(end_pt, end_idx))
+                return false;
+        } while (checkOccupancy(Index2Coord(end_idx)));
+    }
+
+    return true;
+}
+
+bool AStar::AstarSearch(const double step_size, Vector3d start_pt, Vector3d end_pt)
+{
+    ros::Time time_1 = ros::Time::now();
+    ++rounds_;
+
+    step_size_ = step_size;
+    inv_step_size_ = 1 / step_size;
+    center_ = (start_pt + end_pt) / 2;
+
+    Vector3i start_idx, end_idx;
+    if (!ConvertToIndexAndAdjustStartEndPoints(start_pt, end_pt, start_idx, end_idx))
+    {
+        ROS_ERROR("Unable to handle the initial or end point, force return!");
+        return false;
+    }
+
+    // if ( start_pt(0) > -1 && start_pt(0) < 0 )
+    //     cout << "start_pt=" << start_pt.transpose() << " end_pt=" << end_pt.transpose() << endl;
+
+    GridNodePtr startPtr = GridNodeMap_[start_idx(0)][start_idx(1)][start_idx(2)];
+    GridNodePtr endPtr = GridNodeMap_[end_idx(0)][end_idx(1)][end_idx(2)];
+
+    std::priority_queue<GridNodePtr, std::vector<GridNodePtr>, NodeComparator> empty;
+    openSet_.swap(empty);
+
+    GridNodePtr neighborPtr = NULL;
+    GridNodePtr current = NULL;
+
+    startPtr->index = start_idx;
+    startPtr->rounds = rounds_;
+    startPtr->gScore = 0;
+    startPtr->fScore = getHeu(startPtr, endPtr);
+    startPtr->state = GridNode::OPENSET; //put start node in open set
+    startPtr->cameFrom = NULL;
+    openSet_.push(startPtr); //put start in open set
+
+    endPtr->index = end_idx;
+
+    double tentative_gScore;
+
+    int num_iter = 0;
+    while (!openSet_.empty())
+    {
+        num_iter++;
+        current = openSet_.top();
+        openSet_.pop();
+
+        // if ( num_iter < 10000 )
+        //     cout << "current=" << current->index.transpose() << endl;
+
+        if (current->index(0) == endPtr->index(0) && current->index(1) == endPtr->index(1) && current->index(2) == endPtr->index(2))
+        {
+            // ros::Time time_2 = ros::Time::now();
+            // printf("\033[34mA star iter:%d, time:%.3f\033[0m\n",num_iter, (time_2 - time_1).toSec()*1000);
+            // if((time_2 - time_1).toSec() > 0.1)
+            //     ROS_WARN("Time consume in A star path finding is %f", (time_2 - time_1).toSec() );
+            gridPath_ = retrievePath(current);
+            return true;
+        }
+        current->state = GridNode::CLOSEDSET; //move current node from open set to closed set.
+
+        for (int dx = -1; dx <= 1; dx++)
+            for (int dy = -1; dy <= 1; dy++)
+                for (int dz = -1; dz <= 1; dz++)
+                {
+                    if (dx == 0 && dy == 0 && dz == 0)
+                        continue;
+
+                    Vector3i neighborIdx;
+                    neighborIdx(0) = (current->index)(0) + dx;
+                    neighborIdx(1) = (current->index)(1) + dy;
+                    neighborIdx(2) = (current->index)(2) + dz;
+
+                    if (neighborIdx(0) < 1 || neighborIdx(0) >= POOL_SIZE_(0) - 1 || neighborIdx(1) < 1 || neighborIdx(1) >= POOL_SIZE_(1) - 1 || neighborIdx(2) < 1 || neighborIdx(2) >= POOL_SIZE_(2) - 1)
+                    {
+                        continue;
+                    }
+
+                    neighborPtr = GridNodeMap_[neighborIdx(0)][neighborIdx(1)][neighborIdx(2)];
+                    neighborPtr->index = neighborIdx;
+
+                    bool flag_explored = neighborPtr->rounds == rounds_;
+
+                    if (flag_explored && neighborPtr->state == GridNode::CLOSEDSET)
+                    {
+                        continue; //in closed set.
+                    }
+
+                    neighborPtr->rounds = rounds_;
+
+                    if (checkOccupancy(Index2Coord(neighborPtr->index)))
+                    {
+                        continue;
+                    }
+
+                    double static_cost = sqrt(dx * dx + dy * dy + dz * dz);
+                    tentative_gScore = current->gScore + static_cost;
+
+                    if (!flag_explored)
+                    {
+                        //discover a new node
+                        neighborPtr->state = GridNode::OPENSET;
+                        neighborPtr->cameFrom = current;
+                        neighborPtr->gScore = tentative_gScore;
+                        neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);
+                        openSet_.push(neighborPtr); //put neighbor in open set and record it.
+                    }
+                    else if (tentative_gScore < neighborPtr->gScore)
+                    { //in open set and need update
+                        neighborPtr->cameFrom = current;
+                        neighborPtr->gScore = tentative_gScore;
+                        neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);
+                    }
+                }
+        ros::Time time_2 = ros::Time::now();
+        if ((time_2 - time_1).toSec() > 0.2)
+        {
+            ROS_WARN("Failed in A star path searching !!! 0.2 seconds time limit exceeded.");
+            return false;
+        }
+    }
+
+    ros::Time time_2 = ros::Time::now();
+
+    if ((time_2 - time_1).toSec() > 0.1)
+        ROS_WARN("Time consume in A star path finding is %.3fs, iter=%d", (time_2 - time_1).toSec(), num_iter);
+
+    return false;
+}
+
+vector<Vector3d> AStar::getPath()
+{
+    vector<Vector3d> path;
+
+    for (auto ptr : gridPath_)
+        path.push_back(Index2Coord(ptr->index));
+
+    reverse(path.begin(), path.end());
+    return path;
+}

motion_planning/3d/ego_planner/planner/plan_env/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(plan_env)
+
+set(CMAKE_BUILD_TYPE "Release")
+ADD_COMPILE_OPTIONS(-std=c++11 )
+ADD_COMPILE_OPTIONS(-std=c++14 )
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+find_package(OpenCV REQUIRED)
+
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  visualization_msgs
+  cv_bridge
+  message_filters
+)
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES plan_env
+ CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+include_directories( 
+    SYSTEM 
+    include 
+    ${catkin_INCLUDE_DIRS}
+    ${Eigen3_INCLUDE_DIRS} 
+    ${PCL_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+)
+
+link_directories(${PCL_LIBRARY_DIRS})
+
+add_library( plan_env 
+    src/grid_map.cpp 
+    src/raycast.cpp
+    src/obj_predictor.cpp 
+    )
+target_link_libraries( plan_env
+    ${catkin_LIBRARIES} 
+    ${PCL_LIBRARIES}
+    )  
+
+add_executable(obj_generator
+    src/obj_generator.cpp 
+)
+target_link_libraries(obj_generator 
+    ${catkin_LIBRARIES}
+    )

motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/grid_map.h

@@ -0,0 +1,804 @@
+#ifndef _GRID_MAP_H
+#define _GRID_MAP_H
+
+#include <Eigen/Eigen>
+#include <Eigen/StdVector>
+#include <cv_bridge/cv_bridge.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <iostream>
+#include <random>
+#include <nav_msgs/Odometry.h>
+#include <queue>
+#include <ros/ros.h>
+#include <tuple>
+#include <visualization_msgs/Marker.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/time_synchronizer.h>
+
+#include <plan_env/raycast.h>
+
+#define logit(x) (log((x) / (1 - (x))))
+
+using namespace std;
+
+// voxel hashing
+template <typename T>
+struct matrix_hash : std::unary_function<T, size_t> {
+  std::size_t operator()(T const& matrix) const {
+    size_t seed = 0;
+    for (size_t i = 0; i < matrix.size(); ++i) {
+      auto elem = *(matrix.data() + i);
+      seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
+    }
+    return seed;
+  }
+};
+
+// constant parameters
+
+struct MappingParameters {
+
+  /* map properties */
+  Eigen::Vector3d map_origin_, map_size_;
+  Eigen::Vector3d map_min_boundary_, map_max_boundary_;  // map range in pos
+  Eigen::Vector3i map_voxel_num_;                        // map range in index
+  Eigen::Vector3d local_update_range_;
+  double resolution_, resolution_inv_;
+  double obstacles_inflation_;
+  string frame_id_;
+  int pose_type_;
+
+  /* camera parameters */
+  double cx_, cy_, fx_, fy_;
+
+  /* time out */
+  double odom_depth_timeout_;
+
+  /* depth image projection filtering */
+  double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_;
+  int depth_filter_margin_;
+  bool use_depth_filter_;
+  double k_depth_scaling_factor_;
+  int skip_pixel_;
+
+  /* raycasting */
+  double p_hit_, p_miss_, p_min_, p_max_, p_occ_;  // occupancy probability
+  double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_,
+      min_occupancy_log_;                   // logit of occupancy probability
+  double min_ray_length_, max_ray_length_;  // range of doing raycasting
+
+  /* local map update and clear */
+  int local_map_margin_;
+
+  /* visualization and computation time display */
+  double visualization_truncate_height_, virtual_ceil_height_, ground_height_, virtual_ceil_yp_, virtual_ceil_yn_;
+  bool show_occ_time_;
+
+  /* active mapping */
+  double unknown_flag_;
+};
+
+// intermediate mapping data for fusion
+
+struct MappingData {
+  // main map data, occupancy of each voxel and Euclidean distance
+
+  std::vector<double> occupancy_buffer_;
+  std::vector<char> occupancy_buffer_inflate_;
+
+  // camera position and pose data
+
+  Eigen::Vector3d camera_pos_, last_camera_pos_;
+  Eigen::Matrix3d camera_r_m_, last_camera_r_m_;
+  Eigen::Matrix4d cam2body_;
+
+  // depth image data
+
+  cv::Mat depth_image_, last_depth_image_;
+  int image_cnt_;
+
+  // flags of map state
+
+  bool occ_need_update_, local_updated_;
+  bool has_first_depth_;
+  bool has_odom_, has_cloud_;
+
+  // odom_depth_timeout_
+  ros::Time last_occ_update_time_;
+  bool flag_depth_odom_timeout_;
+  bool flag_use_depth_fusion;
+
+  // depth image projected point cloud
+
+  vector<Eigen::Vector3d> proj_points_;
+  int proj_points_cnt;
+
+  // flag buffers for speeding up raycasting
+
+  vector<short> count_hit_, count_hit_and_miss_;
+  vector<char> flag_traverse_, flag_rayend_;
+  char raycast_num_;
+  queue<Eigen::Vector3i> cache_voxel_;
+
+  // range of updating grid
+
+  Eigen::Vector3i local_bound_min_, local_bound_max_;
+
+  // computation time
+
+  double fuse_time_, max_fuse_time_;
+  int update_num_;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+class GridMap {
+public:
+  GridMap() {}
+  ~GridMap() {}
+
+  enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 };
+
+  // occupancy map management
+  void resetBuffer();
+  void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max);
+
+  inline void posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id);
+  inline void indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos);
+  inline int toAddress(const Eigen::Vector3i& id);
+  inline int toAddress(int& x, int& y, int& z);
+  inline bool isInMap(const Eigen::Vector3d& pos);
+  inline bool isInMap(const Eigen::Vector3i& idx);
+
+  inline void setOccupancy(Eigen::Vector3d pos, double occ = 1);
+  inline void setOccupied(Eigen::Vector3d pos);
+  inline int getOccupancy(Eigen::Vector3d pos);
+  inline int getOccupancy(Eigen::Vector3i id);
+  inline int getInflateOccupancy(Eigen::Vector3d pos);
+
+  inline void boundIndex(Eigen::Vector3i& id);
+  inline bool isUnknown(const Eigen::Vector3i& id);
+  inline bool isUnknown(const Eigen::Vector3d& pos);
+  inline bool isKnownFree(const Eigen::Vector3i& id);
+  inline bool isKnownOccupied(const Eigen::Vector3i& id);
+
+  void initMap(ros::NodeHandle& nh);
+
+  void publishMap();
+  void publishMapInflate(bool all_info = false);
+
+  void publishDepth();
+
+  bool hasDepthObservation();
+  bool odomValid();
+  void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size);
+  inline double getResolution();
+  Eigen::Vector3d getOrigin();
+  int getVoxelNum();
+  bool getOdomDepthTimeout() { return md_.flag_depth_odom_timeout_; }
+
+  typedef std::shared_ptr<GridMap> Ptr;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+private:
+  MappingParameters mp_;
+  MappingData md_;
+
+  // get depth image and camera pose
+  void depthPoseCallback(const sensor_msgs::ImageConstPtr& img,
+                         const geometry_msgs::PoseStampedConstPtr& pose);
+  void extrinsicCallback(const nav_msgs::OdometryConstPtr& odom);
+  void depthOdomCallback(const sensor_msgs::ImageConstPtr& img, const nav_msgs::OdometryConstPtr& odom);
+  void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img);
+  void odomCallback(const nav_msgs::OdometryConstPtr& odom);
+
+  // update occupancy by raycasting
+  void updateOccupancyCallback(const ros::TimerEvent& /*event*/);
+  void visCallback(const ros::TimerEvent& /*event*/);
+
+  // main update process
+  void projectDepthImage();
+  void raycastProcess();
+  void clearAndInflateLocalMap();
+
+  inline void inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts);
+  int setCacheOccupancy(Eigen::Vector3d pos, int occ);
+  Eigen::Vector3d closetPointInMap(const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt);
+
+  // typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image,
+  // nav_msgs::Odometry> SyncPolicyImageOdom; typedef
+  // message_filters::sync_policies::ExactTime<sensor_msgs::Image,
+  // geometry_msgs::PoseStamped> SyncPolicyImagePose;
+  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, nav_msgs::Odometry>
+      SyncPolicyImageOdom;
+  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::PoseStamped>
+      SyncPolicyImagePose;
+  typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImagePose>> SynchronizerImagePose;
+  typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImageOdom>> SynchronizerImageOdom;
+
+  ros::NodeHandle node_;
+  shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> depth_sub_;
+  shared_ptr<message_filters::Subscriber<geometry_msgs::PoseStamped>> pose_sub_;
+  shared_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub_;
+  SynchronizerImagePose sync_image_pose_;
+  SynchronizerImageOdom sync_image_odom_;
+
+  ros::Subscriber indep_cloud_sub_, indep_odom_sub_, extrinsic_sub_;
+  ros::Publisher map_pub_, map_inf_pub_;
+  ros::Timer occ_timer_, vis_timer_;
+
+  //
+  uniform_real_distribution<double> rand_noise_;
+  normal_distribution<double> rand_noise2_;
+  default_random_engine eng_;
+};
+
+/* ============================== definition of inline function
+ * ============================== */
+
+inline int GridMap::toAddress(const Eigen::Vector3i& id) {
+  return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2);
+}
+
+inline int GridMap::toAddress(int& x, int& y, int& z) {
+  return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z;
+}
+
+inline void GridMap::boundIndex(Eigen::Vector3i& id) {
+  Eigen::Vector3i id1;
+  id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0);
+  id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0);
+  id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0);
+  id = id1;
+}
+
+inline bool GridMap::isUnknown(const Eigen::Vector3i& id) {
+  Eigen::Vector3i id1 = id;
+  boundIndex(id1);
+  return md_.occupancy_buffer_[toAddress(id1)] < mp_.clamp_min_log_ - 1e-3;
+}
+
+inline bool GridMap::isUnknown(const Eigen::Vector3d& pos) {
+  Eigen::Vector3i idc;
+  posToIndex(pos, idc);
+  return isUnknown(idc);
+}
+
+inline bool GridMap::isKnownFree(const Eigen::Vector3i& id) {
+  Eigen::Vector3i id1 = id;
+  boundIndex(id1);
+  int adr = toAddress(id1);
+
+  // return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ &&
+  //     md_.occupancy_buffer_[adr] < mp_.min_occupancy_log_;
+  return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && md_.occupancy_buffer_inflate_[adr] == 0;
+}
+
+inline bool GridMap::isKnownOccupied(const Eigen::Vector3i& id) {
+  Eigen::Vector3i id1 = id;
+  boundIndex(id1);
+  int adr = toAddress(id1);
+
+  return md_.occupancy_buffer_inflate_[adr] == 1;
+}
+
+inline void GridMap::setOccupied(Eigen::Vector3d pos) {
+  if (!isInMap(pos)) return;
+
+  Eigen::Vector3i id;
+  posToIndex(pos, id);
+
+  md_.occupancy_buffer_inflate_[id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) +
+                                id(1) * mp_.map_voxel_num_(2) + id(2)] = 1;
+}
+
+inline void GridMap::setOccupancy(Eigen::Vector3d pos, double occ) {
+  if (occ != 1 && occ != 0) {
+    cout << "occ value error!" << endl;
+    return;
+  }
+
+  if (!isInMap(pos)) return;
+
+  Eigen::Vector3i id;
+  posToIndex(pos, id);
+
+  md_.occupancy_buffer_[toAddress(id)] = occ;
+}
+
+inline int GridMap::getOccupancy(Eigen::Vector3d pos) {
+  if (!isInMap(pos)) return -1;
+
+  Eigen::Vector3i id;
+  posToIndex(pos, id);
+
+  return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
+}
+
+inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) {
+  if (!isInMap(pos)) return -1;
+
+  Eigen::Vector3i id;
+  posToIndex(pos, id);
+
+  return int(md_.occupancy_buffer_inflate_[toAddress(id)]);
+}
+
+inline int GridMap::getOccupancy(Eigen::Vector3i id) {
+  if (id(0) < 0 || id(0) >= mp_.map_voxel_num_(0) || id(1) < 0 || id(1) >= mp_.map_voxel_num_(1) ||
+      id(2) < 0 || id(2) >= mp_.map_voxel_num_(2))
+    return -1;
+
+  return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
+}
+
+inline bool GridMap::isInMap(const Eigen::Vector3d& pos) {
+  if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 ||
+      pos(2) < mp_.map_min_boundary_(2) + 1e-4) {
+    // cout << "less than min range!" << endl;
+    return false;
+  }
+  if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 ||
+      pos(2) > mp_.map_max_boundary_(2) - 1e-4) {
+    return false;
+  }
+  return true;
+}
+
+inline bool GridMap::isInMap(const Eigen::Vector3i& idx) {
+  if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0) {
+    return false;
+  }
+  if (idx(0) > mp_.map_voxel_num_(0) - 1 || idx(1) > mp_.map_voxel_num_(1) - 1 ||
+      idx(2) > mp_.map_voxel_num_(2) - 1) {
+    return false;
+  }
+  return true;
+}
+
+inline void GridMap::posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id) {
+  for (int i = 0; i < 3; ++i) id(i) = floor((pos(i) - mp_.map_origin_(i)) * mp_.resolution_inv_);
+}
+
+inline void GridMap::indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos) {
+  for (int i = 0; i < 3; ++i) pos(i) = (id(i) + 0.5) * mp_.resolution_ + mp_.map_origin_(i);
+}
+
+inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts) {
+  int num = 0;
+
+  /* ---------- + shape inflate ---------- */
+  // for (int x = -step; x <= step; ++x)
+  // {
+  //   if (x == 0)
+  //     continue;
+  //   pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1), pt(2));
+  // }
+  // for (int y = -step; y <= step; ++y)
+  // {
+  //   if (y == 0)
+  //     continue;
+  //   pts[num++] = Eigen::Vector3i(pt(0), pt(1) + y, pt(2));
+  // }
+  // for (int z = -1; z <= 1; ++z)
+  // {
+  //   pts[num++] = Eigen::Vector3i(pt(0), pt(1), pt(2) + z);
+  // }
+
+  /* ---------- all inflate ---------- */
+  for (int x = -step; x <= step; ++x)
+    for (int y = -step; y <= step; ++y)
+      for (int z = -step; z <= step; ++z) {
+        pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z);
+      }
+}
+
+inline double GridMap::getResolution() { return mp_.resolution_; }
+
+#endif
+
+// #ifndef _GRID_MAP_H
+// #define _GRID_MAP_H
+
+// #include <Eigen/Eigen>
+// #include <Eigen/StdVector>
+// #include <cv_bridge/cv_bridge.h>
+// #include <geometry_msgs/PoseStamped.h>
+// #include <iostream>
+// #include <random>
+// #include <nav_msgs/Odometry.h>
+// #include <queue>
+// #include <ros/ros.h>
+// #include <tuple>
+// #include <visualization_msgs/Marker.h>
+
+// #include <pcl/point_cloud.h>
+// #include <pcl/point_types.h>
+// #include <pcl_conversions/pcl_conversions.h>
+
+// #include <message_filters/subscriber.h>
+// #include <message_filters/sync_policies/approximate_time.h>
+// #include <message_filters/sync_policies/exact_time.h>
+// #include <message_filters/time_synchronizer.h>
+
+// #include <plan_env/raycast.h>
+
+// #define logit(x) (log((x) / (1 - (x))))
+
+// using namespace std;
+
+// // voxel hashing
+// template <typename T>
+// struct matrix_hash : std::unary_function<T, size_t> {
+//   std::size_t operator()(T const& matrix) const {
+//     size_t seed = 0;
+//     for (size_t i = 0; i < matrix.size(); ++i) {
+//       auto elem = *(matrix.data() + i);
+//       seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
+//     }
+//     return seed;
+//   }
+// };
+
+// // constant parameters
+
+// struct MappingParameters {
+
+//   /* map properties */
+//   Eigen::Vector3d map_origin_, map_size_;
+//   Eigen::Vector3d map_min_boundary_, map_max_boundary_;  // map range in pos
+//   Eigen::Vector3i map_voxel_num_;                        // map range in index
+//   Eigen::Vector3d local_update_range_;
+//   double resolution_, resolution_inv_;
+//   double obstacles_inflation_;
+//   string frame_id_;
+//   int pose_type_;
+
+//   /* camera parameters */
+//   double cx_, cy_, fx_, fy_;
+
+//   /* depth image projection filtering */
+//   double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_;
+//   int depth_filter_margin_;
+//   bool use_depth_filter_;
+//   double k_depth_scaling_factor_;
+//   int skip_pixel_;
+
+//   /* raycasting */
+//   double p_hit_, p_miss_, p_min_, p_max_, p_occ_;  // occupancy probability
+//   double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_,
+//       min_occupancy_log_;                   // logit of occupancy probability
+//   double min_ray_length_, max_ray_length_;  // range of doing raycasting
+
+//   /* local map update and clear */
+//   int local_map_margin_;
+
+//   /* visualization and computation time display */
+//   double visualization_truncate_height_, virtual_ceil_height_, ground_height_;
+//   bool show_occ_time_;
+
+//   /* active mapping */
+//   double unknown_flag_;
+// };
+
+// // intermediate mapping data for fusion
+
+// struct MappingData {
+//   // main map data, occupancy of each voxel and Euclidean distance
+
+//   std::vector<double> occupancy_buffer_;
+//   std::vector<char> occupancy_buffer_inflate_;
+
+//   // camera position and pose data
+
+//   Eigen::Vector3d camera_pos_, last_camera_pos_;
+//   Eigen::Quaterniond camera_q_, last_camera_q_;
+
+//   // depth image data
+
+//   cv::Mat depth_image_, last_depth_image_;
+//   int image_cnt_;
+
+//   // flags of map state
+
+//   bool occ_need_update_, local_updated_;
+//   bool has_first_depth_;
+//   bool has_odom_, has_cloud_;
+
+//   // depth image projected point cloud
+
+//   vector<Eigen::Vector3d> proj_points_;
+//   int proj_points_cnt;
+
+//   // flag buffers for speeding up raycasting
+
+//   vector<short> count_hit_, count_hit_and_miss_;
+//   vector<char> flag_traverse_, flag_rayend_;
+//   char raycast_num_;
+//   queue<Eigen::Vector3i> cache_voxel_;
+
+//   // range of updating grid
+
+//   Eigen::Vector3i local_bound_min_, local_bound_max_;
+
+//   // computation time
+
+//   double fuse_time_, max_fuse_time_;
+//   int update_num_;
+
+//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+// };
+
+// class GridMap {
+// public:
+//   GridMap() {}
+//   ~GridMap() {}
+
+//   enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 };
+
+//   // occupancy map management
+//   void resetBuffer();
+//   void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max);
+
+//   inline void posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id);
+//   inline void indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos);
+//   inline int toAddress(const Eigen::Vector3i& id);
+//   inline int toAddress(int& x, int& y, int& z);
+//   inline bool isInMap(const Eigen::Vector3d& pos);
+//   inline bool isInMap(const Eigen::Vector3i& idx);
+
+//   inline void setOccupancy(Eigen::Vector3d pos, double occ = 1);
+//   inline void setOccupied(Eigen::Vector3d pos);
+//   inline int getOccupancy(Eigen::Vector3d pos);
+//   inline int getOccupancy(Eigen::Vector3i id);
+//   inline int getInflateOccupancy(Eigen::Vector3d pos);
+
+//   inline void boundIndex(Eigen::Vector3i& id);
+//   inline bool isUnknown(const Eigen::Vector3i& id);
+//   inline bool isUnknown(const Eigen::Vector3d& pos);
+//   inline bool isKnownFree(const Eigen::Vector3i& id);
+//   inline bool isKnownOccupied(const Eigen::Vector3i& id);
+
+//   void initMap(ros::NodeHandle& nh);
+
+//   void publishMap();
+//   void publishMapInflate(bool all_info = false);
+
+//   void publishUnknown();
+//   void publishDepth();
+
+//   bool hasDepthObservation();
+//   bool odomValid();
+//   void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size);
+//   inline double getResolution();
+//   Eigen::Vector3d getOrigin();
+//   int getVoxelNum();
+
+//   typedef std::shared_ptr<GridMap> Ptr;
+
+//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+// private:
+//   MappingParameters mp_;
+//   MappingData md_;
+
+//   // get depth image and camera pose
+//   void depthPoseCallback(const sensor_msgs::ImageConstPtr& img,
+//                          const geometry_msgs::PoseStampedConstPtr& pose);
+//   void depthOdomCallback(const sensor_msgs::ImageConstPtr& img, const nav_msgs::OdometryConstPtr& odom);
+//   void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img);
+//   void odomCallback(const nav_msgs::OdometryConstPtr& odom);
+
+//   // update occupancy by raycasting
+//   void updateOccupancyCallback(const ros::TimerEvent& /*event*/);
+//   void visCallback(const ros::TimerEvent& /*event*/);
+
+//   // main update process
+//   void projectDepthImage();
+//   void raycastProcess();
+//   void clearAndInflateLocalMap();
+
+//   inline void inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts);
+//   int setCacheOccupancy(Eigen::Vector3d pos, int occ);
+//   Eigen::Vector3d closetPointInMap(const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt);
+
+//   // typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image,
+//   // nav_msgs::Odometry> SyncPolicyImageOdom; typedef
+//   // message_filters::sync_policies::ExactTime<sensor_msgs::Image,
+//   // geometry_msgs::PoseStamped> SyncPolicyImagePose;
+//   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, nav_msgs::Odometry>
+//       SyncPolicyImageOdom;
+//   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::PoseStamped>
+//       SyncPolicyImagePose;
+//   typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImagePose>> SynchronizerImagePose;
+//   typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImageOdom>> SynchronizerImageOdom;
+
+//   ros::NodeHandle node_;
+//   shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> depth_sub_;
+//   shared_ptr<message_filters::Subscriber<geometry_msgs::PoseStamped>> pose_sub_;
+//   shared_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub_;
+//   SynchronizerImagePose sync_image_pose_;
+//   SynchronizerImageOdom sync_image_odom_;
+
+//   ros::Subscriber indep_cloud_sub_, indep_odom_sub_;
+//   ros::Publisher map_pub_, map_inf_pub_;
+//   ros::Publisher unknown_pub_;
+//   ros::Timer occ_timer_, vis_timer_;
+
+//   //
+//   uniform_real_distribution<double> rand_noise_;
+//   normal_distribution<double> rand_noise2_;
+//   default_random_engine eng_;
+// };
+
+// /* ============================== definition of inline function
+//  * ============================== */
+
+// inline int GridMap::toAddress(const Eigen::Vector3i& id) {
+//   return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2);
+// }
+
+// inline int GridMap::toAddress(int& x, int& y, int& z) {
+//   return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z;
+// }
+
+// inline void GridMap::boundIndex(Eigen::Vector3i& id) {
+//   Eigen::Vector3i id1;
+//   id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0);
+//   id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0);
+//   id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0);
+//   id = id1;
+// }
+
+// inline bool GridMap::isUnknown(const Eigen::Vector3i& id) {
+//   Eigen::Vector3i id1 = id;
+//   boundIndex(id1);
+//   return md_.occupancy_buffer_[toAddress(id1)] < mp_.clamp_min_log_ - 1e-3;
+// }
+
+// inline bool GridMap::isUnknown(const Eigen::Vector3d& pos) {
+//   Eigen::Vector3i idc;
+//   posToIndex(pos, idc);
+//   return isUnknown(idc);
+// }
+
+// inline bool GridMap::isKnownFree(const Eigen::Vector3i& id) {
+//   Eigen::Vector3i id1 = id;
+//   boundIndex(id1);
+//   int adr = toAddress(id1);
+
+//   // return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ &&
+//   //     md_.occupancy_buffer_[adr] < mp_.min_occupancy_log_;
+//   return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && md_.occupancy_buffer_inflate_[adr] == 0;
+// }
+
+// inline bool GridMap::isKnownOccupied(const Eigen::Vector3i& id) {
+//   Eigen::Vector3i id1 = id;
+//   boundIndex(id1);
+//   int adr = toAddress(id1);
+
+//   return md_.occupancy_buffer_inflate_[adr] == 1;
+// }
+
+// inline void GridMap::setOccupied(Eigen::Vector3d pos) {
+//   if (!isInMap(pos)) return;
+
+//   Eigen::Vector3i id;
+//   posToIndex(pos, id);
+
+//   md_.occupancy_buffer_inflate_[id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) +
+//                                 id(1) * mp_.map_voxel_num_(2) + id(2)] = 1;
+// }
+
+// inline void GridMap::setOccupancy(Eigen::Vector3d pos, double occ) {
+//   if (occ != 1 && occ != 0) {
+//     cout << "occ value error!" << endl;
+//     return;
+//   }
+
+//   if (!isInMap(pos)) return;
+
+//   Eigen::Vector3i id;
+//   posToIndex(pos, id);
+
+//   md_.occupancy_buffer_[toAddress(id)] = occ;
+// }
+
+// inline int GridMap::getOccupancy(Eigen::Vector3d pos) {
+//   if (!isInMap(pos)) return -1;
+
+//   Eigen::Vector3i id;
+//   posToIndex(pos, id);
+
+//   return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
+// }
+
+// inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) {
+//   if (!isInMap(pos)) return -1;
+
+//   Eigen::Vector3i id;
+//   posToIndex(pos, id);
+
+//   return int(md_.occupancy_buffer_inflate_[toAddress(id)]);
+// }
+
+// inline int GridMap::getOccupancy(Eigen::Vector3i id) {
+//   if (id(0) < 0 || id(0) >= mp_.map_voxel_num_(0) || id(1) < 0 || id(1) >= mp_.map_voxel_num_(1) ||
+//       id(2) < 0 || id(2) >= mp_.map_voxel_num_(2))
+//     return -1;
+
+//   return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
+// }
+
+// inline bool GridMap::isInMap(const Eigen::Vector3d& pos) {
+//   if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 ||
+//       pos(2) < mp_.map_min_boundary_(2) + 1e-4) {
+//     // cout << "less than min range!" << endl;
+//     return false;
+//   }
+//   if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 ||
+//       pos(2) > mp_.map_max_boundary_(2) - 1e-4) {
+//     return false;
+//   }
+//   return true;
+// }
+
+// inline bool GridMap::isInMap(const Eigen::Vector3i& idx) {
+//   if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0) {
+//     return false;
+//   }
+//   if (idx(0) > mp_.map_voxel_num_(0) - 1 || idx(1) > mp_.map_voxel_num_(1) - 1 ||
+//       idx(2) > mp_.map_voxel_num_(2) - 1) {
+//     return false;
+//   }
+//   return true;
+// }
+
+// inline void GridMap::posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id) {
+//   for (int i = 0; i < 3; ++i) id(i) = floor((pos(i) - mp_.map_origin_(i)) * mp_.resolution_inv_);
+// }
+
+// inline void GridMap::indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos) {
+//   for (int i = 0; i < 3; ++i) pos(i) = (id(i) + 0.5) * mp_.resolution_ + mp_.map_origin_(i);
+// }
+
+// inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts) {
+//   int num = 0;
+
+//   /* ---------- + shape inflate ---------- */
+//   // for (int x = -step; x <= step; ++x)
+//   // {
+//   //   if (x == 0)
+//   //     continue;
+//   //   pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1), pt(2));
+//   // }
+//   // for (int y = -step; y <= step; ++y)
+//   // {
+//   //   if (y == 0)
+//   //     continue;
+//   //   pts[num++] = Eigen::Vector3i(pt(0), pt(1) + y, pt(2));
+//   // }
+//   // for (int z = -1; z <= 1; ++z)
+//   // {
+//   //   pts[num++] = Eigen::Vector3i(pt(0), pt(1), pt(2) + z);
+//   // }
+
+//   /* ---------- all inflate ---------- */
+//   for (int x = -step; x <= step; ++x)
+//     for (int y = -step; y <= step; ++y)
+//       for (int z = -step; z <= step; ++z) {
+//         pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z);
+//       }
+// }
+
+// inline double GridMap::getResolution() { return mp_.resolution_; }
+
+// #endif

motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/linear_obj_model.hpp

@@ -0,0 +1,267 @@
+/**
+* This file is part of Fast-Planner.
+*
+* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
+* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
+* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
+* If you use this code, please cite the respective publications as
+* listed on the above website.
+*
+* Fast-Planner is free software: you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* Fast-Planner is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+
+#ifndef _LINEAR_OBJ_MODEL_H_
+#define _LINEAR_OBJ_MODEL_H_
+
+#include <Eigen/Eigen>
+#include <algorithm>
+#include <iostream>
+
+class LinearObjModel {
+private:
+  bool last_out_bound_{false};
+  int input_type_;
+  /* data */
+public:
+  LinearObjModel(/* args */);
+  ~LinearObjModel();
+
+  void initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw, double yaw_dot,
+                  Eigen::Vector3d color, Eigen::Vector3d scale, int input_type);
+
+  void setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc);
+
+  void update(double dt);  // linear trippler integrator model
+
+  static bool collide(LinearObjModel& obj1, LinearObjModel& obj2);
+
+  // void setInput(Eigen::Vector3d acc) { 
+  //   acc_ = acc; 
+  // }
+  void setInput(Eigen::Vector3d vel) {
+    vel_ = vel;
+  }
+
+  void setYawDot(double yaw_dot) {
+    yaw_dot_ = yaw_dot;
+  }
+
+  Eigen::Vector3d getPosition() {
+    return pos_;
+  }
+  void setPosition(Eigen::Vector3d pos) {
+    pos_ = pos;
+  }
+
+  Eigen::Vector3d getVelocity() {
+    return vel_;
+  }
+
+  void setVelocity(double x, double y, double z) {
+    vel_ = Eigen::Vector3d(x, y, z);
+  }
+
+  Eigen::Vector3d getColor() {
+    return color_;
+  }
+  Eigen::Vector3d getScale() {
+    return scale_;
+  }
+
+  double getYaw() {
+    return yaw_;
+  }
+
+private:
+  Eigen::Vector3d pos_, vel_, acc_;
+  Eigen::Vector3d color_, scale_;
+  double yaw_, yaw_dot_;
+
+  Eigen::Vector3d bound_;
+  Eigen::Vector2d limit_v_, limit_a_;
+};
+
+LinearObjModel::LinearObjModel(/* args */) {
+}
+
+LinearObjModel::~LinearObjModel() {
+}
+
+void LinearObjModel::initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw,
+                                double yaw_dot, Eigen::Vector3d color, Eigen::Vector3d scale, int input_type) {
+  pos_ = p;
+  vel_ = v;
+  acc_ = a;
+  color_ = color;
+  scale_ = scale;
+  input_type_ = input_type;
+
+  yaw_ = yaw;
+  yaw_dot_ = yaw_dot;
+}
+
+void LinearObjModel::setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc) {
+  bound_ = bound;
+  limit_v_ = vel;
+  limit_a_ = acc;
+}
+
+void LinearObjModel::update(double dt) {
+  Eigen::Vector3d p0, v0, a0;
+  p0 = pos_, v0 = vel_, a0 = acc_;
+  //std::cout << v0.transpose() << std::endl;
+
+  /* ---------- use acc as input ---------- */
+  if ( input_type_ == 2 )
+  {
+    vel_ = v0 + acc_ * dt;
+    for (int i = 0; i < 3; ++i)
+    {
+      if (vel_(i) > 0) vel_(i) = std::max(limit_v_(0), std::min(vel_(i),
+      limit_v_(1)));
+      if (vel_(i) <= 0) vel_(i) = std::max(-limit_v_(1), std::min(vel_(i),
+      -limit_v_(0)));
+    }
+
+    pos_ = p0 + v0 * dt + 0.5 * acc_ * pow(dt, 2);
+
+    /* ---------- reflect acc when collide with bound ---------- */
+    if ( pos_(0) <= bound_(0) && pos_(0) >= -bound_(0) &&
+        pos_(1) <= bound_(1) && pos_(1) >= -bound_(1) &&
+        pos_(2) <= bound_(2) && pos_(2) >= 0
+      )
+    {
+      last_out_bound_ = false;
+    }
+    else if ( !last_out_bound_ )
+    {
+      last_out_bound_ = true;
+
+      // if ( pos_(0) > bound_(0) || pos_(0) < -bound_(0) ) acc_(0) = -acc_(0);
+      // if ( pos_(1) > bound_(1) || pos_(1) < -bound_(1) ) acc_(1) = -acc_(1);
+      // if ( pos_(2) > bound_(2) || pos_(2) < -bound_(2) ) acc_(2) = -acc_(2);
+      acc_ = -acc_;
+      //ROS_ERROR("AAAAAAAAAAAAAAAAAAa");
+    }
+  }
+  // for (int i = 0; i < 2; ++i)
+  // {
+  //   pos_(i) = std::min(pos_(i), bound_(i));
+  //   pos_(i) = std::max(pos_(i), -bound_(i));
+  // }
+  // pos_(2) = std::min(pos_(2), bound_(2));
+  // pos_(2) = std::max(pos_(2), 0.0);
+
+  /* ---------- use vel as input ---------- */
+  else if ( input_type_ == 1 )
+  {
+    pos_ = p0 + v0 * dt;
+    for (int i = 0; i < 2; ++i) {
+      pos_(i) = std::min(pos_(i), bound_(i));
+      pos_(i) = std::max(pos_(i), -bound_(i));
+    }
+    pos_(2) = std::min(pos_(2), bound_(2));
+    pos_(2) = std::max(pos_(2), 0.0);
+
+    yaw_ += yaw_dot_ * dt;
+
+    const double PI = 3.1415926;
+    if (yaw_ > 2 * PI) yaw_ -= 2 * PI;
+
+    const double tol = 0.1;
+    if (pos_(0) > bound_(0) - tol) {
+      pos_(0) = bound_(0) - tol;
+      vel_(0) = -vel_(0);
+    }
+    if (pos_(0) < -bound_(0) + tol) {
+      pos_(0) = -bound_(0) + tol;
+      vel_(0) = -vel_(0);
+    }
+
+    if (pos_(1) > bound_(1) - tol) {
+      pos_(1) = bound_(1) - tol;
+      vel_(1) = -vel_(1);
+    }
+    if (pos_(1) < -bound_(1) + tol) {
+      pos_(1) = -bound_(1) + tol;
+      vel_(1) = -vel_(1);
+    }
+
+    if (pos_(2) > bound_(2) - tol) {
+      pos_(2) = bound_(2) - tol;
+      vel_(2) = -vel_(2);
+    }
+    if (pos_(2) < tol) {
+      pos_(2) = tol;
+      vel_(2) = -vel_(2);
+    }
+  }
+
+  // /* ---------- reflect when collide with bound ---------- */
+
+
+  //std::cout << pos_.transpose() << "  " << bound_.transpose() << std::endl;
+}
+
+bool LinearObjModel::collide(LinearObjModel& obj1, LinearObjModel& obj2) {
+  Eigen::Vector3d pos1, pos2, vel1, vel2, scale1, scale2;
+  pos1 = obj1.getPosition();
+  vel1 = obj1.getVelocity();
+  scale1 = obj1.getScale();
+
+  pos2 = obj2.getPosition();
+  vel2 = obj2.getVelocity();
+  scale2 = obj2.getScale();
+
+  /* ---------- collide ---------- */
+  bool collide = fabs(pos1(0) - pos2(0)) < 0.5 * (scale1(0) + scale2(0)) &&
+      fabs(pos1(1) - pos2(1)) < 0.5 * (scale1(1) + scale2(1)) &&
+      fabs(pos1(2) - pos2(2)) < 0.5 * (scale1(2) + scale2(2));
+
+  if (collide) {
+    double tol[3];
+    tol[0] = 0.5 * (scale1(0) + scale2(0)) - fabs(pos1(0) - pos2(0));
+    tol[1] = 0.5 * (scale1(1) + scale2(1)) - fabs(pos1(1) - pos2(1));
+    tol[2] = 0.5 * (scale1(2) + scale2(2)) - fabs(pos1(2) - pos2(2));
+
+    for (int i = 0; i < 3; ++i) {
+      if (tol[i] < tol[(i + 1) % 3] && tol[i] < tol[(i + 2) % 3]) {
+        vel1(i) = -vel1(i);
+        vel2(i) = -vel2(i);
+        obj1.setVelocity(vel1(0), vel1(1), vel1(2));
+        obj2.setVelocity(vel2(0), vel2(1), vel2(2));
+
+        if (pos1(i) >= pos2(i)) {
+          pos1(i) += tol[i];
+          pos2(i) -= tol[i];
+        } else {
+          pos1(i) -= tol[i];
+          pos2(i) += tol[i];
+        }
+        obj1.setPosition(pos1);
+        obj2.setPosition(pos2);
+
+        break;
+      }
+    }
+
+    return true;
+  } else {
+    return false;
+  }
+}
+
+#endif

motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/obj_predictor.h

@@ -0,0 +1,176 @@
+/**
+* This file is part of Fast-Planner.
+*
+* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
+* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
+* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
+* If you use this code, please cite the respective publications as
+* listed on the above website.
+*
+* Fast-Planner is free software: you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* Fast-Planner is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+
+#ifndef _OBJ_PREDICTOR_H_
+#define _OBJ_PREDICTOR_H_
+
+#include <Eigen/Eigen>
+#include <algorithm>
+#include <geometry_msgs/PoseStamped.h>
+#include <iostream>
+#include <list>
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+
+using std::cout;
+using std::endl;
+using std::list;
+using std::shared_ptr;
+using std::unique_ptr;
+using std::vector;
+
+namespace fast_planner {
+class PolynomialPrediction;
+typedef shared_ptr<vector<PolynomialPrediction>> ObjPrediction;
+typedef shared_ptr<vector<Eigen::Vector3d>> ObjScale;
+
+/* ========== prediction polynomial ========== */
+class PolynomialPrediction {
+private:
+  vector<Eigen::Matrix<double, 6, 1>> polys;
+  double t1, t2;  // start / end
+  ros::Time global_start_time_;
+
+public:
+  PolynomialPrediction(/* args */) {
+  }
+  ~PolynomialPrediction() {
+  }
+
+  void setPolynomial(vector<Eigen::Matrix<double, 6, 1>>& pls) {
+    polys = pls;
+  }
+  void setTime(double t1, double t2) {
+    this->t1 = t1;
+    this->t2 = t2;
+  }
+  void setGlobalStartTime(ros::Time global_start_time) {
+    global_start_time_ = global_start_time;
+  }
+
+  bool valid() {
+    return polys.size() == 3;
+  }
+
+  /* note that t should be in [t1, t2] */
+  Eigen::Vector3d evaluate(double t) {
+    Eigen::Matrix<double, 6, 1> tv;
+    tv << 1.0, pow(t, 1), pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5);
+
+    Eigen::Vector3d pt;
+    pt(0) = tv.dot(polys[0]), pt(1) = tv.dot(polys[1]), pt(2) = tv.dot(polys[2]);
+
+    return pt;
+  }
+
+  Eigen::Vector3d evaluateConstVel(double t) {
+    Eigen::Matrix<double, 2, 1> tv;
+    tv << 1.0, pow(t-global_start_time_.toSec(), 1);
+
+    // cout << t-global_start_time_.toSec() << endl;
+
+    Eigen::Vector3d pt;
+    pt(0) = tv.dot(polys[0].head(2)), pt(1) = tv.dot(polys[1].head(2)), pt(2) = tv.dot(polys[2].head(2));
+
+    return pt;
+  }
+};
+
+/* ========== subscribe and record object history ========== */
+class ObjHistory {
+public:
+  int skip_num_;
+  int queue_size_;
+  ros::Time global_start_time_;
+
+  ObjHistory() {
+  }
+  ~ObjHistory() {
+  }
+
+  void init(int id, int skip_num, int queue_size, ros::Time global_start_time);
+
+  void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
+
+  void clear() {
+    history_.clear();
+  }
+
+  void getHistory(list<Eigen::Vector4d>& his) {
+    his = history_;
+  }
+
+private:
+  list<Eigen::Vector4d> history_;  // x,y,z;t
+  int skip_;
+  int obj_idx_;
+  Eigen::Vector3d scale_;
+};
+
+/* ========== predict future trajectory using history ========== */
+class ObjPredictor {
+private:
+  ros::NodeHandle node_handle_;
+
+  int obj_num_;
+  double lambda_;
+  double predict_rate_;
+
+  vector<ros::Subscriber> pose_subs_;
+  ros::Subscriber marker_sub_;
+  ros::Timer predict_timer_;
+  vector<shared_ptr<ObjHistory>> obj_histories_;
+
+  /* share data with planner */
+  ObjPrediction predict_trajs_;
+  ObjScale obj_scale_;
+  vector<bool> scale_init_;
+
+  void markerCallback(const visualization_msgs::MarkerConstPtr& msg);
+
+  void predictCallback(const ros::TimerEvent& e);
+  void predictPolyFit();
+  void predictConstVel();
+
+public:
+  ObjPredictor(/* args */);
+  ObjPredictor(ros::NodeHandle& node);
+  ~ObjPredictor();
+
+  void init();
+
+  ObjPrediction getPredictionTraj();
+  ObjScale getObjScale();
+  int getObjNums() {return obj_num_;}
+
+  Eigen::Vector3d evaluatePoly(int obs_id, double time);
+  Eigen::Vector3d evaluateConstVel(int obs_id, double time);
+
+  typedef shared_ptr<ObjPredictor> Ptr;
+};
+
+}  // namespace fast_planner
+
+#endif

motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/raycast.h

@@ -0,0 +1,63 @@
+#ifndef RAYCAST_H_
+#define RAYCAST_H_
+
+#include <Eigen/Eigen>
+#include <vector>
+
+double signum(double x);
+
+double mod(double value, double modulus);
+
+double intbound(double s, double ds);
+
+void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
+             const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output);
+
+void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
+             const Eigen::Vector3d& max, std::vector<Eigen::Vector3d>* output);
+
+class RayCaster {
+private:
+  /* data */
+  Eigen::Vector3d start_;
+  Eigen::Vector3d end_;
+  Eigen::Vector3d direction_;
+  Eigen::Vector3d min_;
+  Eigen::Vector3d max_;
+  int x_;
+  int y_;
+  int z_;
+  int endX_;
+  int endY_;
+  int endZ_;
+  double maxDist_;
+  double dx_;
+  double dy_;
+  double dz_;
+  int stepX_;
+  int stepY_;
+  int stepZ_;
+  double tMaxX_;
+  double tMaxY_;
+  double tMaxZ_;
+  double tDeltaX_;
+  double tDeltaY_;
+  double tDeltaZ_;
+  double dist_;
+
+  int step_num_;
+
+public:
+  RayCaster(/* args */) {
+  }
+  ~RayCaster() {
+  }
+
+  bool setInput(const Eigen::Vector3d& start,
+                const Eigen::Vector3d& end /* , const Eigen::Vector3d& min,
+                const Eigen::Vector3d& max */);
+
+  bool step(Eigen::Vector3d& ray_pt);
+};
+
+#endif  // RAYCAST_H_

motion_planning/3d/ego_planner/planner/plan_env/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>plan_env</name>
+  <version>0.0.0</version>
+  <description>The plan_env package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/plan_env</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

motion_planning/3d/ego_planner/planner/plan_env/src/obj_generator.cpp

@@ -0,0 +1,238 @@
+/**
+* This file is part of Fast-Planner.
+*
+* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
+* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
+* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
+* If you use this code, please cite the respective publications as
+* listed on the above website.
+*
+* Fast-Planner is free software: you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* Fast-Planner is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+
+#include "visualization_msgs/Marker.h"
+#include <ros/ros.h>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <random>
+#include <sensor_msgs/PointCloud2.h>
+#include <string>
+
+#include <plan_env/linear_obj_model.hpp>
+using namespace std;
+
+int obj_num, _input_type;
+double _x_size, _y_size, _h_size, _vel, _yaw_dot, _acc_r1, _acc_r2, _acc_z, _scale1, _scale2, _interval;
+
+
+ros::Publisher obj_pub;            // visualize marker
+vector<ros::Publisher> pose_pubs;  // obj pose (from optitrack)
+vector<LinearObjModel> obj_models;
+
+random_device rd;
+default_random_engine eng(rd());
+uniform_real_distribution<double> rand_pos_x;
+uniform_real_distribution<double> rand_pos_y;
+uniform_real_distribution<double> rand_h;
+uniform_real_distribution<double> rand_vel;
+uniform_real_distribution<double> rand_acc_r;
+uniform_real_distribution<double> rand_acc_t;
+uniform_real_distribution<double> rand_acc_z;
+uniform_real_distribution<double> rand_color;
+uniform_real_distribution<double> rand_scale;
+uniform_real_distribution<double> rand_yaw_dot;
+uniform_real_distribution<double> rand_yaw;
+
+ros::Time time_update, time_change;
+
+void updateCallback(const ros::TimerEvent& e);
+void visualizeObj(int id);
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "dynamic_obj");
+  ros::NodeHandle node("~");
+
+  /* ---------- initialize ---------- */
+  node.param("obj_generator/obj_num", obj_num, 20);
+  node.param("obj_generator/x_size", _x_size, 10.0);
+  node.param("obj_generator/y_size", _y_size, 10.0);
+  node.param("obj_generator/h_size", _h_size, 2.0);
+  node.param("obj_generator/vel", _vel, 2.0);
+  node.param("obj_generator/yaw_dot", _yaw_dot, 2.0);
+  node.param("obj_generator/acc_r1", _acc_r1, 2.0);
+  node.param("obj_generator/acc_r2", _acc_r2, 2.0);
+  node.param("obj_generator/acc_z", _acc_z, 0.0);
+  node.param("obj_generator/scale1", _scale1, 0.5);
+  node.param("obj_generator/scale2", _scale2, 1.0);
+  node.param("obj_generator/interval", _interval, 100.0);
+  node.param("obj_generator/input_type", _input_type, 1);
+
+  obj_pub = node.advertise<visualization_msgs::Marker>("/dynamic/obj", 10);
+  for (int i = 0; i < obj_num; ++i) {
+    ros::Publisher pose_pub =
+        node.advertise<geometry_msgs::PoseStamped>("/dynamic/pose_" + to_string(i), 10);
+    pose_pubs.push_back(pose_pub);
+  }
+
+  ros::Timer update_timer = node.createTimer(ros::Duration(1 / 30.0), updateCallback);
+  cout << "[dynamic]: initialize with " + to_string(obj_num) << " moving obj." << endl;
+  ros::Duration(1.0).sleep();
+
+  rand_color = uniform_real_distribution<double>(0.0, 1.0);
+  rand_pos_x = uniform_real_distribution<double>(-_x_size/2, _x_size/2);
+  rand_pos_y = uniform_real_distribution<double>(-_y_size/2, _y_size/2);
+  rand_h = uniform_real_distribution<double>(0.0, _h_size);
+  rand_vel = uniform_real_distribution<double>(-_vel, _vel);
+  rand_acc_t = uniform_real_distribution<double>(0.0, 6.28);
+  rand_acc_r = uniform_real_distribution<double>(_acc_r1, _acc_r2);
+  rand_acc_z = uniform_real_distribution<double>(-_acc_z, _acc_z);
+  rand_scale = uniform_real_distribution<double>(_scale1, _scale2);
+  rand_yaw = uniform_real_distribution<double>(0.0, 2 * 3.141592);
+  rand_yaw_dot = uniform_real_distribution<double>(-_yaw_dot, _yaw_dot);
+
+  /* ---------- give initial value of each obj ---------- */
+  for (int i = 0; i < obj_num; ++i) {
+    LinearObjModel model;
+    Eigen::Vector3d pos(rand_pos_x(eng), rand_pos_y(eng), rand_h(eng));
+    Eigen::Vector3d vel(rand_vel(eng), rand_vel(eng), 0.0);
+    Eigen::Vector3d color(rand_color(eng), rand_color(eng), rand_color(eng));
+    Eigen::Vector3d scale(rand_scale(eng), 1.5 * rand_scale(eng), 5.0*rand_scale(eng));
+    double yaw = rand_yaw(eng);
+    double yaw_dot = rand_yaw_dot(eng);
+
+    double r, t, z;
+    r = rand_acc_r(eng);
+    t = rand_acc_t(eng);
+    z = rand_acc_z(eng);
+    Eigen::Vector3d acc(r * cos(t), r * sin(t), z);
+
+    if ( _input_type == 1 )
+    {
+      model.initialize(pos, vel, acc, yaw, yaw_dot, color, scale, _input_type); // Vel input
+    }
+    else
+    {
+      model.initialize(pos, Eigen::Vector3d(0,0,0), acc, yaw, yaw_dot, color, scale, _input_type); // Acc input
+    }
+    model.setLimits(Eigen::Vector3d(_x_size/2, _y_size/2, _h_size), Eigen::Vector2d(0.0, _vel),
+                    Eigen::Vector2d(0, 0));
+    obj_models.push_back(model);
+  }
+
+  time_update = ros::Time::now();
+  time_change = ros::Time::now();
+
+  /* ---------- start loop ---------- */
+  ros::spin();
+
+  return 0;
+}
+
+void updateCallback(const ros::TimerEvent& e) {
+  ros::Time time_now = ros::Time::now();
+
+  /* ---------- change input ---------- */
+  // double dtc = (time_now - time_change).toSec();
+  // if (dtc > _interval) {
+  //   for (int i = 0; i < obj_num; ++i) {
+  //     /* ---------- use acc input ---------- */
+  //     // double r, t, z;
+  //     // r = rand_acc_r(eng);
+  //     // t = rand_acc_t(eng);
+  //     // z = rand_acc_z(eng);
+  //     // Eigen::Vector3d acc(r * cos(t), r * sin(t), z);
+  //     // obj_models[i].setInput(acc);
+
+  //     /* ---------- use vel input ---------- */
+  //     double vx, vy, vz, yd;
+  //     vx = rand_vel(eng);
+  //     vy = rand_vel(eng);
+  //     vz = 0.0;
+  //     yd = rand_yaw_dot(eng);
+
+  //     obj_models[i].setInput(Eigen::Vector3d(vx, vy, vz));
+  //     obj_models[i].setYawDot(yd);
+  //   }
+  //   time_change = time_now;
+  // }
+
+  /* ---------- update obj state ---------- */
+  double dt = (time_now - time_update).toSec();
+  time_update = time_now;
+  for (int i = 0; i < obj_num; ++i) {
+    obj_models[i].update(dt);
+    visualizeObj(i);
+    ros::Duration(0.000001).sleep();
+  }
+
+  /* ---------- collision ---------- */
+  // for (int i = 0; i < obj_num; ++i)
+  //   for (int j = i + 1; j < obj_num; ++j) {
+  //     bool collision = LinearObjModel::collide(obj_models[i], obj_models[j]);
+  //     if (collision) {
+  //       double yd1 = rand_yaw_dot(eng);
+  //       double yd2 = rand_yaw_dot(eng);
+  //       obj_models[i].setYawDot(yd1);
+  //       obj_models[j].setYawDot(yd2);
+  //     }
+  //   }
+}
+
+void visualizeObj(int id) {
+  Eigen::Vector3d pos, color, scale;
+  pos = obj_models[id].getPosition();
+  color = obj_models[id].getColor();
+  scale = obj_models[id].getScale();
+  double yaw = obj_models[id].getYaw();
+
+  Eigen::Matrix3d rot;
+  rot << cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0;
+
+  Eigen::Quaterniond qua;
+  qua = rot;
+
+  /* ---------- rviz ---------- */
+  visualization_msgs::Marker mk;
+  mk.header.frame_id = "world";
+  mk.header.stamp = ros::Time::now();
+  mk.type = visualization_msgs::Marker::CUBE;
+  mk.action = visualization_msgs::Marker::ADD;
+  mk.id = id;
+
+  mk.scale.x = scale(0), mk.scale.y = scale(1), mk.scale.z = scale(2);
+  mk.color.a = 1.0, mk.color.r = color(0), mk.color.g = color(1), mk.color.b = color(2);
+
+  mk.pose.orientation.w = qua.w();
+  mk.pose.orientation.x = qua.x();
+  mk.pose.orientation.y = qua.y();
+  mk.pose.orientation.z = qua.z();
+
+  mk.pose.position.x = pos(0), mk.pose.position.y = pos(1), mk.pose.position.z = pos(2);
+
+  obj_pub.publish(mk);
+
+  /* ---------- pose ---------- */
+  geometry_msgs::PoseStamped pose;
+  pose.header.frame_id = "world";
+  pose.header.seq = id;
+  pose.pose.position.x = pos(0), pose.pose.position.y = pos(1), pose.pose.position.z = pos(2);
+  pose.pose.orientation.w = 1.0;
+  pose_pubs[id].publish(pose);
+}

motion_planning/3d/ego_planner/planner/plan_env/src/obj_predictor.cpp

@@ -0,0 +1,289 @@
+/**
+* This file is part of Fast-Planner.
+*
+* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
+* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
+* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
+* If you use this code, please cite the respective publications as
+* listed on the above website.
+*
+* Fast-Planner is free software: you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* Fast-Planner is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+
+#include <plan_env/obj_predictor.h>
+#include <string>
+
+namespace fast_planner {
+/* ============================== obj history_ ============================== */
+
+// int ObjHistory::queue_size_;
+// int ObjHistory::skip_num_;
+// ros::Time ObjHistory::global_start_time_;
+
+void ObjHistory::init(int id, int skip_num, int queue_size, ros::Time global_start_time) {
+  clear();
+  skip_ = 0;
+  obj_idx_ = id;
+  skip_num_ = skip_num;
+  queue_size_ = queue_size;
+  global_start_time_ = global_start_time;
+}
+
+void ObjHistory::poseCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
+  ++skip_;
+  if (skip_ < skip_num_) return;
+
+  Eigen::Vector4d pos_t;
+  pos_t(0) = msg->pose.position.x, pos_t(1) = msg->pose.position.y, pos_t(2) = msg->pose.position.z;
+  pos_t(3) = (ros::Time::now() - global_start_time_).toSec();
+
+  history_.push_back(pos_t);
+  // cout << "idx: " << obj_idx_ << "pos_t: " << pos_t.transpose() << endl;
+
+  if (history_.size() > queue_size_) history_.pop_front();
+
+  skip_ = 0;
+}
+
+// ObjHistory::
+/* ============================== obj predictor ==============================
+ */
+ObjPredictor::ObjPredictor(/* args */) {
+}
+
+ObjPredictor::ObjPredictor(ros::NodeHandle& node) {
+  this->node_handle_ = node;
+}
+
+ObjPredictor::~ObjPredictor() {
+}
+
+void ObjPredictor::init() {
+  /* get param */
+  int queue_size, skip_nums;
+
+  node_handle_.param("prediction/obj_num", obj_num_, 0);
+  node_handle_.param("prediction/lambda", lambda_, 1.0);
+  node_handle_.param("prediction/predict_rate", predict_rate_, 1.0);
+  node_handle_.param("prediction/queue_size", queue_size, 10);
+  node_handle_.param("prediction/skip_nums", skip_nums, 1);
+
+  predict_trajs_.reset(new vector<PolynomialPrediction>);
+  predict_trajs_->resize(obj_num_);
+
+  obj_scale_.reset(new vector<Eigen::Vector3d>);
+  obj_scale_->resize(obj_num_);
+  scale_init_.resize(obj_num_);
+  for (int i = 0; i < obj_num_; i++)
+    scale_init_[i] = false;
+
+  /* subscribe to pose */
+  ros::Time t_now = ros::Time::now();
+  for (int i = 0; i < obj_num_; i++) {
+    shared_ptr<ObjHistory> obj_his(new ObjHistory);
+
+    obj_his->init(i, skip_nums, queue_size, t_now);
+    obj_histories_.push_back(obj_his);
+
+    ros::Subscriber pose_sub = node_handle_.subscribe<geometry_msgs::PoseStamped>(
+        "/dynamic/pose_" + std::to_string(i), 10, &ObjHistory::poseCallback, obj_his.get());
+
+    pose_subs_.push_back(pose_sub);
+
+    predict_trajs_->at(i).setGlobalStartTime(t_now);
+  }
+
+  marker_sub_ = node_handle_.subscribe<visualization_msgs::Marker>("/dynamic/obj", 10,
+                                                                   &ObjPredictor::markerCallback, this);
+
+  /* update prediction */
+  predict_timer_ =
+      node_handle_.createTimer(ros::Duration(1 / predict_rate_), &ObjPredictor::predictCallback, this);
+}
+
+ObjPrediction ObjPredictor::getPredictionTraj() {
+  return this->predict_trajs_;
+}
+
+ObjScale ObjPredictor::getObjScale() {
+  return this->obj_scale_;
+}
+
+void ObjPredictor::predictPolyFit() {
+  /* iterate all obj */
+  for (int i = 0; i < obj_num_; i++) {
+    /* ---------- write A and b ---------- */
+    Eigen::Matrix<double, 6, 6> A;
+    Eigen::Matrix<double, 6, 1> temp;
+    Eigen::Matrix<double, 6, 1> bm[3];  // poly coefficent
+    vector<Eigen::Matrix<double, 6, 1>> pm(3);
+
+    A.setZero();
+    for (int i = 0; i < 3; ++i)
+      bm[i].setZero();
+
+    /* ---------- estimation error ---------- */
+    list<Eigen::Vector4d> his;
+    obj_histories_[i]->getHistory(his);
+    for (list<Eigen::Vector4d>::iterator it = his.begin(); it != his.end(); ++it) {
+      Eigen::Vector3d qi = (*it).head(3);
+      double ti = (*it)(3);
+
+      /* A */
+      temp << 1.0, ti, pow(ti, 2), pow(ti, 3), pow(ti, 4), pow(ti, 5);
+      for (int j = 0; j < 6; ++j)
+        A.row(j) += 2.0 * pow(ti, j) * temp.transpose();
+
+      /* b */
+      for (int dim = 0; dim < 3; ++dim)
+        bm[dim] += 2.0 * qi(dim) * temp;
+    }
+
+    /* ---------- acceleration regulator ---------- */
+    double t1 = his.front()(3);
+    double t2 = his.back()(3);
+
+    temp << 0.0, 0.0, 2 * t1 - 2 * t2, 3 * pow(t1, 2) - 3 * pow(t2, 2), 4 * pow(t1, 3) - 4 * pow(t2, 3),
+        5 * pow(t1, 4) - 5 * pow(t2, 4);
+    A.row(2) += -4 * lambda_ * temp.transpose();
+
+    temp << 0.0, 0.0, pow(t1, 2) - pow(t2, 2), 2 * pow(t1, 3) - 2 * pow(t2, 3),
+        3 * pow(t1, 4) - 3 * pow(t2, 4), 4 * pow(t1, 5) - 4 * pow(t2, 5);
+    A.row(3) += -12 * lambda_ * temp.transpose();
+
+    temp << 0.0, 0.0, 20 * pow(t1, 3) - 20 * pow(t2, 3), 45 * pow(t1, 4) - 45 * pow(t2, 4),
+        72 * pow(t1, 5) - 72 * pow(t2, 5), 100 * pow(t1, 6) - 100 * pow(t2, 6);
+    A.row(4) += -4.0 / 5.0 * lambda_ * temp.transpose();
+
+    temp << 0.0, 0.0, 35 * pow(t1, 4) - 35 * pow(t2, 4), 84 * pow(t1, 5) - 84 * pow(t2, 5),
+        140 * pow(t1, 6) - 140 * pow(t2, 6), 200 * pow(t1, 7) - 200 * pow(t2, 7);
+    A.row(5) += -4.0 / 7.0 * lambda_ * temp.transpose();
+
+    /* ---------- solve ---------- */
+    for (int j = 0; j < 3; j++) {
+      pm[j] = A.colPivHouseholderQr().solve(bm[j]);
+    }
+
+    /* ---------- update prediction container ---------- */
+    predict_trajs_->at(i).setPolynomial(pm);
+    predict_trajs_->at(i).setTime(t1, t2);
+  }
+}
+
+void ObjPredictor::predictCallback(const ros::TimerEvent& e) {
+  // predictPolyFit();
+  predictConstVel();
+}
+
+void ObjPredictor::markerCallback(const visualization_msgs::MarkerConstPtr& msg) {
+  int idx = msg->id;
+  (*obj_scale_)[idx](0) = msg->scale.x;
+  (*obj_scale_)[idx](1) = msg->scale.y;
+  (*obj_scale_)[idx](2) = msg->scale.z;
+
+  scale_init_[idx] = true;
+
+  int finish_num = 0;
+  for (int i = 0; i < obj_num_; i++) {
+    if (scale_init_[i]) finish_num++;
+  }
+
+  if (finish_num == obj_num_) {
+    marker_sub_.shutdown();
+  }
+}
+
+void ObjPredictor::predictConstVel() {
+  for (int i = 0; i < obj_num_; i++) {
+    /* ---------- get the last two point ---------- */
+    list<Eigen::Vector4d> his;
+    obj_histories_[i]->getHistory(his);
+    // if ( i==0 )
+    // {
+    //   cout << "his.size()=" << his.size() << endl;
+    //   for ( auto hi:his )
+    //   {
+    //     cout << hi.transpose() << endl;
+    //   }
+    // }
+    list<Eigen::Vector4d>::iterator list_it = his.end();
+
+    /* ---------- test iteration ---------- */
+    // cout << "----------------------------" << endl;
+    // for (auto v4d : his)
+    //   cout << "v4d: " << v4d.transpose() << endl;
+
+    Eigen::Vector3d q1, q2;
+    double t1, t2;
+
+    --list_it;
+    q2 = (*list_it).head(3);
+    t2 = (*list_it)(3);
+
+    --list_it;
+    q1 = (*list_it).head(3);
+    t1 = (*list_it)(3);
+
+    Eigen::Matrix<double, 2, 3> p01, q12;
+    q12.row(0) = q1.transpose();
+    q12.row(1) = q2.transpose();
+
+    Eigen::Matrix<double, 2, 2> At12;
+    At12 << 1, t1, 1, t2;
+
+    p01 = At12.inverse() * q12;
+
+    vector<Eigen::Matrix<double, 6, 1>> polys(3);
+    for (int j = 0; j < 3; ++j) {
+      polys[j].setZero();
+      polys[j].head(2) = p01.col(j);
+    }
+    
+    // if ( i==0 )
+    // {
+    //   cout << "q1=" << q1.transpose() << " t1=" << t1 << " q2=" << q2.transpose() << " t2=" << t2 << endl;
+    //   cout << "polys=" << polys[0].transpose() << endl;
+    // }
+
+    predict_trajs_->at(i).setPolynomial(polys);
+    predict_trajs_->at(i).setTime(t1, t2);
+  }
+}
+
+Eigen::Vector3d ObjPredictor::evaluatePoly(int obj_id, double time)
+{
+  if ( obj_id < obj_num_ )
+  {
+    return predict_trajs_->at(obj_id).evaluate(time);
+  }
+
+  double MAX = std::numeric_limits<double>::max();
+  return Eigen::Vector3d(MAX, MAX, MAX);
+}
+
+Eigen::Vector3d ObjPredictor::evaluateConstVel(int obj_id, double time)
+{
+  if ( obj_id < obj_num_ )
+  {
+    return predict_trajs_->at(obj_id).evaluateConstVel(time);
+  }
+
+  double MAX = std::numeric_limits<double>::max();
+  return Eigen::Vector3d(MAX, MAX, MAX);
+}
+
+// ObjPredictor::
+}  // namespace fast_planner

motion_planning/3d/ego_planner/planner/plan_env/src/raycast.cpp

@@ -0,0 +1,321 @@
+#include <Eigen/Eigen>
+#include <cmath>
+#include <iostream>
+#include <plan_env/raycast.h>
+
+int signum(int x) {
+  return x == 0 ? 0 : x < 0 ? -1 : 1;
+}
+
+double mod(double value, double modulus) {
+  return fmod(fmod(value, modulus) + modulus, modulus);
+}
+
+double intbound(double s, double ds) {
+  // Find the smallest positive t such that s+t*ds is an integer.
+  if (ds < 0) {
+    return intbound(-s, -ds);
+  } else {
+    s = mod(s, 1);
+    // problem is now s+t*ds = 1
+    return (1 - s) / ds;
+  }
+}
+
+void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
+             const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output) {
+  //    std::cout << start << ' ' << end << std::endl;
+  // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
+  // by John Amanatides and Andrew Woo, 1987
+  // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
+  // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
+  // Extensions to the described algorithm:
+  //   • Imposed a distance limit.
+  //   • The face passed through to reach the current cube is provided to
+  //     the callback.
+
+  // The foundation of this algorithm is a parameterized representation of
+  // the provided ray,
+  //                    origin + t * direction,
+  // except that t is not actually stored; rather, at any given point in the
+  // traversal, we keep track of the *greater* t values which we would have
+  // if we took a step sufficient to cross a cube boundary along that axis
+  // (i.e. change the integer part of the coordinate) in the variables
+  // tMaxX, tMaxY, and tMaxZ.
+
+  // Cube containing origin point.
+  int x = (int)std::floor(start.x());
+  int y = (int)std::floor(start.y());
+  int z = (int)std::floor(start.z());
+  int endX = (int)std::floor(end.x());
+  int endY = (int)std::floor(end.y());
+  int endZ = (int)std::floor(end.z());
+  Eigen::Vector3d direction = (end - start);
+  double maxDist = direction.squaredNorm();
+
+  // Break out direction vector.
+  double dx = endX - x;
+  double dy = endY - y;
+  double dz = endZ - z;
+
+  // Direction to increment x,y,z when stepping.
+  int stepX = (int)signum((int)dx);
+  int stepY = (int)signum((int)dy);
+  int stepZ = (int)signum((int)dz);
+
+  // See description above. The initial values depend on the fractional
+  // part of the origin.
+  double tMaxX = intbound(start.x(), dx);
+  double tMaxY = intbound(start.y(), dy);
+  double tMaxZ = intbound(start.z(), dz);
+
+  // The change in t when taking a step (always positive).
+  double tDeltaX = ((double)stepX) / dx;
+  double tDeltaY = ((double)stepY) / dy;
+  double tDeltaZ = ((double)stepZ) / dz;
+
+  // Avoids an infinite loop.
+  if (stepX == 0 && stepY == 0 && stepZ == 0) return;
+
+  double dist = 0;
+  while (true) {
+    if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) {
+      output[output_points_cnt](0) = x;
+      output[output_points_cnt](1) = y;
+      output[output_points_cnt](2) = z;
+
+      output_points_cnt++;
+      dist = sqrt((x - start(0)) * (x - start(0)) + (y - start(1)) * (y - start(1)) +
+                  (z - start(2)) * (z - start(2)));
+
+      if (dist > maxDist) return;
+
+      /*            if (output_points_cnt > 1500) {
+                      std::cerr << "Error, too many racyast voxels." <<
+         std::endl;
+                      throw std::out_of_range("Too many raycast voxels");
+                  }*/
+    }
+
+    if (x == endX && y == endY && z == endZ) break;
+
+    // tMaxX stores the t-value at which we cross a cube boundary along the
+    // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
+    // chooses the closest cube boundary. Only the first case of the four
+    // has been commented in detail.
+    if (tMaxX < tMaxY) {
+      if (tMaxX < tMaxZ) {
+        // Update which cube we are now in.
+        x += stepX;
+        // Adjust tMaxX to the next X-oriented boundary crossing.
+        tMaxX += tDeltaX;
+      } else {
+        z += stepZ;
+        tMaxZ += tDeltaZ;
+      }
+    } else {
+      if (tMaxY < tMaxZ) {
+        y += stepY;
+        tMaxY += tDeltaY;
+      } else {
+        z += stepZ;
+        tMaxZ += tDeltaZ;
+      }
+    }
+  }
+}
+
+void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
+             const Eigen::Vector3d& max, std::vector<Eigen::Vector3d>* output) {
+  //    std::cout << start << ' ' << end << std::endl;
+  // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
+  // by John Amanatides and Andrew Woo, 1987
+  // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
+  // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
+  // Extensions to the described algorithm:
+  //   • Imposed a distance limit.
+  //   • The face passed through to reach the current cube is provided to
+  //     the callback.
+
+  // The foundation of this algorithm is a parameterized representation of
+  // the provided ray,
+  //                    origin + t * direction,
+  // except that t is not actually stored; rather, at any given point in the
+  // traversal, we keep track of the *greater* t values which we would have
+  // if we took a step sufficient to cross a cube boundary along that axis
+  // (i.e. change the integer part of the coordinate) in the variables
+  // tMaxX, tMaxY, and tMaxZ.
+
+  // Cube containing origin point.
+  int x = (int)std::floor(start.x());
+  int y = (int)std::floor(start.y());
+  int z = (int)std::floor(start.z());
+  int endX = (int)std::floor(end.x());
+  int endY = (int)std::floor(end.y());
+  int endZ = (int)std::floor(end.z());
+  Eigen::Vector3d direction = (end - start);
+  double maxDist = direction.squaredNorm();
+
+  // Break out direction vector.
+  double dx = endX - x;
+  double dy = endY - y;
+  double dz = endZ - z;
+
+  // Direction to increment x,y,z when stepping.
+  int stepX = (int)signum((int)dx);
+  int stepY = (int)signum((int)dy);
+  int stepZ = (int)signum((int)dz);
+
+  // See description above. The initial values depend on the fractional
+  // part of the origin.
+  double tMaxX = intbound(start.x(), dx);
+  double tMaxY = intbound(start.y(), dy);
+  double tMaxZ = intbound(start.z(), dz);
+
+  // The change in t when taking a step (always positive).
+  double tDeltaX = ((double)stepX) / dx;
+  double tDeltaY = ((double)stepY) / dy;
+  double tDeltaZ = ((double)stepZ) / dz;
+
+  output->clear();
+
+  // Avoids an infinite loop.
+  if (stepX == 0 && stepY == 0 && stepZ == 0) return;
+
+  double dist = 0;
+  while (true) {
+    if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) {
+      output->push_back(Eigen::Vector3d(x, y, z));
+
+      dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm();
+
+      if (dist > maxDist) return;
+
+      if (output->size() > 1500) {
+        std::cerr << "Error, too many racyast voxels." << std::endl;
+        throw std::out_of_range("Too many raycast voxels");
+      }
+    }
+
+    if (x == endX && y == endY && z == endZ) break;
+
+    // tMaxX stores the t-value at which we cross a cube boundary along the
+    // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
+    // chooses the closest cube boundary. Only the first case of the four
+    // has been commented in detail.
+    if (tMaxX < tMaxY) {
+      if (tMaxX < tMaxZ) {
+        // Update which cube we are now in.
+        x += stepX;
+        // Adjust tMaxX to the next X-oriented boundary crossing.
+        tMaxX += tDeltaX;
+      } else {
+        z += stepZ;
+        tMaxZ += tDeltaZ;
+      }
+    } else {
+      if (tMaxY < tMaxZ) {
+        y += stepY;
+        tMaxY += tDeltaY;
+      } else {
+        z += stepZ;
+        tMaxZ += tDeltaZ;
+      }
+    }
+  }
+}
+
+bool RayCaster::setInput(const Eigen::Vector3d& start,
+                         const Eigen::Vector3d& end /* , const Eigen::Vector3d& min,
+                         const Eigen::Vector3d& max */) {
+  start_ = start;
+  end_ = end;
+  // max_ = max;
+  // min_ = min;
+
+  x_ = (int)std::floor(start_.x());
+  y_ = (int)std::floor(start_.y());
+  z_ = (int)std::floor(start_.z());
+  endX_ = (int)std::floor(end_.x());
+  endY_ = (int)std::floor(end_.y());
+  endZ_ = (int)std::floor(end_.z());
+  direction_ = (end_ - start_);
+  maxDist_ = direction_.squaredNorm();
+
+  // Break out direction vector.
+  dx_ = endX_ - x_;
+  dy_ = endY_ - y_;
+  dz_ = endZ_ - z_;
+
+  // Direction to increment x,y,z when stepping.
+  stepX_ = (int)signum((int)dx_);
+  stepY_ = (int)signum((int)dy_);
+  stepZ_ = (int)signum((int)dz_);
+
+  // See description above. The initial values depend on the fractional
+  // part of the origin.
+  tMaxX_ = intbound(start_.x(), dx_);
+  tMaxY_ = intbound(start_.y(), dy_);
+  tMaxZ_ = intbound(start_.z(), dz_);
+
+  // The change in t when taking a step (always positive).
+  tDeltaX_ = ((double)stepX_) / dx_;
+  tDeltaY_ = ((double)stepY_) / dy_;
+  tDeltaZ_ = ((double)stepZ_) / dz_;
+
+  dist_ = 0;
+
+  step_num_ = 0;
+
+  // Avoids an infinite loop.
+  if (stepX_ == 0 && stepY_ == 0 && stepZ_ == 0)
+    return false;
+  else
+    return true;
+}
+
+bool RayCaster::step(Eigen::Vector3d& ray_pt) {
+  // if (x_ >= min_.x() && x_ < max_.x() && y_ >= min_.y() && y_ < max_.y() &&
+  // z_ >= min_.z() && z_ <
+  // max_.z())
+  ray_pt = Eigen::Vector3d(x_, y_, z_);
+
+  // step_num_++;
+
+  // dist_ = (Eigen::Vector3d(x_, y_, z_) - start_).squaredNorm();
+
+  if (x_ == endX_ && y_ == endY_ && z_ == endZ_) {
+    return false;
+  }
+
+  // if (dist_ > maxDist_)
+  // {
+  //   return false;
+  // }
+
+  // tMaxX stores the t-value at which we cross a cube boundary along the
+  // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
+  // chooses the closest cube boundary. Only the first case of the four
+  // has been commented in detail.
+  if (tMaxX_ < tMaxY_) {
+    if (tMaxX_ < tMaxZ_) {
+      // Update which cube we are now in.
+      x_ += stepX_;
+      // Adjust tMaxX to the next X-oriented boundary crossing.
+      tMaxX_ += tDeltaX_;
+    } else {
+      z_ += stepZ_;
+      tMaxZ_ += tDeltaZ_;
+    }
+  } else {
+    if (tMaxY_ < tMaxZ_) {
+      y_ += stepY_;
+      tMaxY_ += tDeltaY_;
+    } else {
+      z_ += stepZ_;
+      tMaxZ_ += tDeltaZ_;
+    }
+  }
+
+  return true;
+}

motion_planning/3d/ego_planner/planner/plan_manage/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ego_planner)
+
+set(CMAKE_BUILD_TYPE "Release")
+ADD_COMPILE_OPTIONS(-std=c++11 )
+ADD_COMPILE_OPTIONS(-std=c++14 )
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  geometry_msgs
+  quadrotor_msgs
+  plan_env
+  path_searching
+  bspline_opt
+  traj_utils
+  message_generation
+  cv_bridge
+)
+
+# catkin_package(CATKIN_DEPENDS message_runtime)
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES ego_planner
+ CATKIN_DEPENDS plan_env path_searching bspline_opt traj_utils 
+#  DEPENDS system_lib
+)
+
+include_directories(
+  include
+  SYSTEM
+  ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include
+  ${EIGEN3_INCLUDE_DIR}
+  ${PCL_INCLUDE_DIRS}
+)
+
+
+add_executable(ego_planner_node
+  src/ego_planner_node.cpp 
+  src/ego_replan_fsm.cpp
+  src/planner_manager.cpp
+  )
+target_link_libraries(ego_planner_node 
+  ${catkin_LIBRARIES}
+  )
+#add_dependencies(ego_planner_node ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
+add_executable(traj_server src/traj_server.cpp)
+target_link_libraries(traj_server ${catkin_LIBRARIES})
+#add_dependencies(traj_server ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
+

motion_planning/3d/ego_planner/planner/plan_manage/include/plan_manage/ego_replan_fsm.h

@@ -0,0 +1,130 @@
+#ifndef _REBO_REPLAN_FSM_H_
+#define _REBO_REPLAN_FSM_H_
+
+#include <Eigen/Eigen>
+#include <algorithm>
+#include <iostream>
+#include <nav_msgs/Path.h>
+#include <sensor_msgs/Imu.h>
+#include <ros/ros.h>
+#include <std_msgs/Empty.h>
+#include <vector>
+#include <visualization_msgs/Marker.h>
+
+#include <bspline_opt/bspline_optimizer.h>
+#include <plan_env/grid_map.h>
+#include <traj_utils/Bspline.h>
+#include <traj_utils/MultiBsplines.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <traj_utils/DataDisp.h>
+#include <plan_manage/planner_manager.h>
+#include <traj_utils/planning_visualization.h>
+
+using std::vector;
+
+namespace ego_planner
+{
+
+  class EGOReplanFSM
+  {
+
+  private:
+    /* ---------- flag ---------- */
+    enum FSM_EXEC_STATE
+    {
+      INIT,
+      WAIT_TARGET,
+      GEN_NEW_TRAJ,
+      REPLAN_TRAJ,
+      EXEC_TRAJ,
+      EMERGENCY_STOP,
+      SEQUENTIAL_START
+    };
+    enum TARGET_TYPE
+    {
+      MANUAL_TARGET = 1,
+      PRESET_TARGET = 2,
+      REFENCE_PATH = 3
+    };
+
+    /* planning utils */
+    EGOPlannerManager::Ptr planner_manager_;
+    PlanningVisualization::Ptr visualization_;
+    traj_utils::DataDisp data_disp_;
+    traj_utils::MultiBsplines multi_bspline_msgs_buf_;
+
+    /* parameters */
+    int target_type_; // 1 mannual select, 2 hard code
+    double no_replan_thresh_, replan_thresh_;
+    double waypoints_[50][3];
+    int waypoint_num_, wp_id_;
+    double planning_horizen_, planning_horizen_time_;
+    double emergency_time_;
+    bool flag_realworld_experiment_;
+    bool enable_fail_safe_;
+
+    /* planning data */
+    bool have_trigger_, have_target_, have_odom_, have_new_target_, have_recv_pre_agent_;
+    FSM_EXEC_STATE exec_state_;
+    int continously_called_times_{0};
+
+    Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_; // odometry state
+    Eigen::Quaterniond odom_orient_;
+
+    Eigen::Vector3d init_pt_, start_pt_, start_vel_, start_acc_, start_yaw_; // start state
+    Eigen::Vector3d end_pt_, end_vel_;                                       // goal state
+    Eigen::Vector3d local_target_pt_, local_target_vel_;                     // local target state
+    std::vector<Eigen::Vector3d> wps_;
+    int current_wp_;
+
+    bool flag_escape_emergency_;
+
+    /* ROS utils */
+    ros::NodeHandle node_;
+    ros::Timer exec_timer_, safety_timer_;
+    ros::Subscriber waypoint_sub_, odom_sub_, swarm_trajs_sub_, broadcast_bspline_sub_, trigger_sub_;
+    ros::Publisher replan_pub_, new_pub_, bspline_pub_, data_disp_pub_, swarm_trajs_pub_, broadcast_bspline_pub_;
+
+    /* helper functions */
+    bool callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj); // front-end and back-end method
+    bool callEmergencyStop(Eigen::Vector3d stop_pos);                          // front-end and back-end method
+    bool planFromGlobalTraj(const int trial_times = 1);
+    bool planFromCurrentTraj(const int trial_times = 1);
+
+    /* return value: std::pair< Times of the same state be continuously called, current continuously called state > */
+    void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call);
+    std::pair<int, EGOReplanFSM::FSM_EXEC_STATE> timesOfConsecutiveStateCalls();
+    void printFSMExecState();
+
+    void readGivenWps();
+    void planNextWaypoint(const Eigen::Vector3d next_wp);
+    void getLocalTarget();
+
+    /* ROS functions */
+    void execFSMCallback(const ros::TimerEvent &e);
+    void checkCollisionCallback(const ros::TimerEvent &e);
+    void waypointCallback(const geometry_msgs::PoseStampedPtr &msg);
+    void triggerCallback(const geometry_msgs::PoseStampedPtr &msg);
+    void odometryCallback(const nav_msgs::OdometryConstPtr &msg);
+    void swarmTrajsCallback(const traj_utils::MultiBsplinesPtr &msg);
+    void BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg);
+
+    bool checkCollision();
+    void publishSwarmTrajs(bool startup_pub);
+
+  public:
+    EGOReplanFSM(/* args */)
+    {
+    }
+    ~EGOReplanFSM()
+    {
+    }
+
+    void init(ros::NodeHandle &nh);
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+} // namespace ego_planner
+
+#endif

motion_planning/3d/ego_planner/planner/plan_manage/include/plan_manage/planner_manager.h

@@ -0,0 +1,85 @@
+#ifndef _PLANNER_MANAGER_H_
+#define _PLANNER_MANAGER_H_
+
+#include <stdlib.h>
+
+#include <bspline_opt/bspline_optimizer.h>
+#include <bspline_opt/uniform_bspline.h>
+#include <traj_utils/DataDisp.h>
+#include <plan_env/grid_map.h>
+#include <plan_env/obj_predictor.h>
+#include <traj_utils/plan_container.hpp>
+#include <ros/ros.h>
+#include <traj_utils/planning_visualization.h>
+
+namespace ego_planner
+{
+
+  // Fast Planner Manager
+  // Key algorithms of mapping and planning are called
+
+  class EGOPlannerManager
+  {
+    // SECTION stable
+  public:
+    EGOPlannerManager();
+    ~EGOPlannerManager();
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    /* main planning interface */
+    bool reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc,
+                       Eigen::Vector3d end_pt, Eigen::Vector3d end_vel, bool flag_polyInit, bool flag_randomPolyTraj);
+    bool EmergencyStop(Eigen::Vector3d stop_pos);
+    bool planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
+                        const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc);
+    bool planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
+                                 const std::vector<Eigen::Vector3d> &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc);
+
+    void initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis = NULL);
+
+    void deliverTrajToOptimizer(void) { bspline_optimizer_->setSwarmTrajs(&swarm_trajs_buf_); };
+
+    void setDroneIdtoOpt(void) { bspline_optimizer_->setDroneId(pp_.drone_id); }
+
+    double getSwarmClearance(void) { return bspline_optimizer_->getSwarmClearance(); }
+
+    bool checkCollision(int drone_id);
+    
+
+    PlanParameters pp_;
+    LocalTrajData local_data_;
+    GlobalTrajData global_data_;
+    GridMap::Ptr grid_map_;
+    fast_planner::ObjPredictor::Ptr obj_predictor_;    
+    SwarmTrajData swarm_trajs_buf_;
+
+  private:
+    /* main planning algorithms & modules */
+    PlanningVisualization::Ptr visualization_;
+
+    // ros::Publisher obj_pub_; //zx-todo 
+
+    BsplineOptimizer::Ptr bspline_optimizer_;
+
+    int continous_failures_count_{0};
+
+    void updateTrajInfo(const UniformBspline &position_traj, const ros::Time time_now);
+
+    void reparamBspline(UniformBspline &bspline, vector<Eigen::Vector3d> &start_end_derivative, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt,
+                        double &time_inc);
+
+    bool refineTrajAlgo(UniformBspline &traj, vector<Eigen::Vector3d> &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points);
+
+    // !SECTION stable
+
+    // SECTION developing
+
+  public:
+    typedef unique_ptr<EGOPlannerManager> Ptr;
+
+    // !SECTION
+  };
+} // namespace ego_planner
+
+#endif

motion_planning/3d/ego_planner/planner/plan_manage/launch/advanced_param.xml

@@ -0,0 +1,161 @@
+<launch>
+  <arg name="map_size_x_"/>
+  <arg name="map_size_y_"/>
+  <arg name="map_size_z_"/>
+
+  <arg name="odometry_topic"/>
+  <arg name="camera_pose_topic"/>
+  <arg name="depth_topic"/>
+  <arg name="cloud_topic"/>
+
+  <arg name="cx"/>
+  <arg name="cy"/>
+  <arg name="fx"/>
+  <arg name="fy"/>
+
+  <arg name="max_vel"/>
+  <arg name="max_acc"/>
+  <arg name="planning_horizon"/>
+
+  <arg name="point_num"/>
+  <arg name="point0_x"/>
+  <arg name="point0_y"/>
+  <arg name="point0_z"/>
+  <arg name="point1_x"/>
+  <arg name="point1_y"/>
+  <arg name="point1_z"/>
+  <arg name="point2_x"/>
+  <arg name="point2_y"/>
+  <arg name="point2_z"/>
+  <arg name="point3_x"/>
+  <arg name="point3_y"/>
+  <arg name="point3_z"/>
+  <arg name="point4_x"/>
+  <arg name="point4_y"/>
+  <arg name="point4_z"/>
+
+  <arg name="flight_type"/>
+  <arg name="use_distinctive_trajs"/>
+
+  <arg name="obj_num_set"/>
+
+  <arg name="drone_id"/>
+
+
+  <!-- main node -->
+  <!-- <node pkg="ego_planner" name="ego_planner_node" type="ego_planner_node" output="screen" launch-prefix="valgrind"> -->
+  <node pkg="ego_planner" name="drone_$(arg drone_id)_ego_planner_node" type="ego_planner_node" output="screen">
+  
+    <!-- <remap from="~odom_world" to="/drone_$(arg drone_id)_$(arg odometry_topic)"/> -->
+    <remap from="~odom_world" to="/drone_$(arg drone_id)_$(arg odometry_topic)"/>
+    <remap from="~planning/bspline" to = "/drone_$(arg drone_id)_planning/bspline"/>
+    <remap from="~planning/data_display" to = "/drone_$(arg drone_id)_planning/data_display"/>
+    <remap from="~planning/broadcast_bspline_from_planner" to = "/broadcast_bspline"/>
+    <remap from="~planning/broadcast_bspline_to_planner" to = "/broadcast_bspline"/>
+
+    <remap from="~grid_map/odom" to="/drone_$(arg drone_id)_$(arg odometry_topic)"/>
+    <remap from="~grid_map/cloud" to="/drone_$(arg drone_id)_$(arg cloud_topic)"/>
+    <remap from="~grid_map/pose"   to = "/drone_$(arg drone_id)_$(arg camera_pose_topic)"/> 
+    <remap from="~grid_map/depth" to = "/drone_$(arg drone_id)_$(arg depth_topic)"/>
+    
+
+    <!-- planning fsm -->
+    <param name="fsm/flight_type" value="$(arg flight_type)" type="int"/>
+    <param name="fsm/thresh_replan_time" value="1.0" type="double"/>
+    <param name="fsm/thresh_no_replan_meter" value="1.0" type="double"/>
+    <param name="fsm/planning_horizon" value="$(arg planning_horizon)" type="double"/> <!--always set to 1.5 times grater than sensing horizen-->
+    <param name="fsm/planning_horizen_time" value="3" type="double"/>
+    <param name="fsm/emergency_time" value="1.0" type="double"/>
+    <param name="fsm/realworld_experiment" value="false"/>
+    <param name="fsm/fail_safe" value="true"/>
+
+    <param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/>
+    <param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
+    <param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/>
+    <param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/>
+    <param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/>
+    <param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/>
+    <param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/>
+    <param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/>
+    <param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/>
+    <param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/>
+    <param name="fsm/waypoint3_x" value="$(arg point3_x)" type="double"/>
+    <param name="fsm/waypoint3_y" value="$(arg point3_y)" type="double"/>
+    <param name="fsm/waypoint3_z" value="$(arg point3_z)" type="double"/>
+    <param name="fsm/waypoint4_x" value="$(arg point4_x)" type="double"/>
+    <param name="fsm/waypoint4_y" value="$(arg point4_y)" type="double"/>
+    <param name="fsm/waypoint4_z" value="$(arg point4_z)" type="double"/>
+
+    <param name="grid_map/resolution"      value="0.1" /> 
+    <param name="grid_map/map_size_x"   value="$(arg map_size_x_)" /> 
+    <param name="grid_map/map_size_y"   value="$(arg map_size_y_)" /> 
+    <param name="grid_map/map_size_z"   value="$(arg map_size_z_)" /> 
+    <param name="grid_map/local_update_range_x"  value="5.5" /> 
+    <param name="grid_map/local_update_range_y"  value="5.5" /> 
+    <param name="grid_map/local_update_range_z"  value="4.5" /> 
+    <param name="grid_map/obstacles_inflation"     value="0.099" /> 
+    <param name="grid_map/local_map_margin" value="10"/>
+    <param name="grid_map/ground_height"        value="-0.01"/>
+    <!-- camera parameter -->
+    <param name="grid_map/cx" value="$(arg cx)"/>
+    <param name="grid_map/cy" value="$(arg cy)"/>
+    <param name="grid_map/fx" value="$(arg fx)"/>
+    <param name="grid_map/fy" value="$(arg fy)"/>
+    <!-- depth filter -->
+    <param name="grid_map/use_depth_filter" value="true"/>
+    <param name="grid_map/depth_filter_tolerance" value="0.15"/>
+    <param name="grid_map/depth_filter_maxdist"   value="5.0"/>
+    <param name="grid_map/depth_filter_mindist"   value="0.2"/>
+    <param name="grid_map/depth_filter_margin"    value="2"/>
+    <param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
+    <param name="grid_map/skip_pixel" value="2"/>
+    <!-- local fusion -->
+    <param name="grid_map/p_hit"  value="0.65"/>
+    <param name="grid_map/p_miss" value="0.35"/>
+    <param name="grid_map/p_min"  value="0.12"/>
+    <param name="grid_map/p_max"  value="0.90"/>
+    <param name="grid_map/p_occ"  value="0.80"/>
+    <param name="grid_map/min_ray_length" value="0.1"/>
+    <param name="grid_map/max_ray_length" value="4.5"/>
+
+    <param name="grid_map/virtual_ceil_height"   value="3.0"/>
+    <param name="grid_map/visualization_truncate_height"   value="2.9"/>
+    <param name="grid_map/show_occ_time"  value="false"/>
+    <param name="grid_map/pose_type"     value="1"/>  
+    <param name="grid_map/frame_id"      value="world"/>
+
+  <!-- planner manager -->
+    <param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
+    <param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
+    <param name="manager/max_jerk" value="4" type="double"/>
+    <param name="manager/control_points_distance" value="0.4" type="double"/>
+    <param name="manager/feasibility_tolerance" value="0.05" type="double"/>
+    <param name="manager/planning_horizon" value="$(arg planning_horizon)" type="double"/>
+    <param name="manager/use_distinctive_trajs" value="$(arg use_distinctive_trajs)" type="bool"/>
+    <param name="manager/drone_id" value="$(arg drone_id)"/>
+
+  <!-- trajectory optimization -->
+    <param name="optimization/lambda_smooth" value="1.0" type="double"/>
+    <param name="optimization/lambda_collision" value="0.5" type="double"/>
+    <param name="optimization/lambda_feasibility" value="0.1" type="double"/>
+    <param name="optimization/lambda_fitness" value="1.0" type="double"/>
+    <param name="optimization/dist0" value="0.5" type="double"/>
+    <param name="optimization/swarm_clearance" value="0.5" type="double"/>
+    <param name="optimization/max_vel" value="$(arg max_vel)" type="double"/>
+    <param name="optimization/max_acc" value="$(arg max_acc)" type="double"/>
+
+    <param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
+    <param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
+    <param name="bspline/limit_ratio" value="1.1" type="double"/>
+
+  <!-- objects prediction -->
+    <param name="prediction/obj_num" value="$(arg obj_num_set)" type="int"/>
+    <param name="prediction/lambda" value="1.0" type="double"/>
+    <param name="prediction/predict_rate" value="1.0" type="double"/>
+  
+  
+
+
+  </node>
+
+</launch>

motion_planning/3d/ego_planner/planner/plan_manage/launch/advanced_param_xtdrone.xml

@@ -0,0 +1,160 @@
+<launch>
+  <arg name="map_size_x_"/>
+  <arg name="map_size_y_"/>
+  <arg name="map_size_z_"/>
+
+  <arg name="odometry_topic"/>
+  <arg name="camera_pose_topic"/>
+  <arg name="depth_topic"/>
+  <arg name="cloud_topic"/>
+
+  <arg name="cx"/>
+  <arg name="cy"/>
+  <arg name="fx"/>
+  <arg name="fy"/>
+
+  <arg name="max_vel"/>
+  <arg name="max_acc"/>
+  <arg name="planning_horizon"/>
+
+  <arg name="point_num"/>
+  <arg name="point0_x"/>
+  <arg name="point0_y"/>
+  <arg name="point0_z"/>
+  <arg name="point1_x"/>
+  <arg name="point1_y"/>
+  <arg name="point1_z"/>
+  <arg name="point2_x"/>
+  <arg name="point2_y"/>
+  <arg name="point2_z"/>
+  <arg name="point3_x"/>
+  <arg name="point3_y"/>
+  <arg name="point3_z"/>
+  <arg name="point4_x"/>
+  <arg name="point4_y"/>
+  <arg name="point4_z"/>
+
+  <arg name="flight_type"/>
+  <arg name="use_distinctive_trajs"/>
+
+  <arg name="obj_num_set"/>
+
+  <arg name="drone_id"/>
+
+
+  <!-- main node -->
+  <!-- <node pkg="ego_planner" name="ego_planner_node" type="ego_planner_node" output="screen" launch-prefix="valgrind"> -->
+  <node pkg="ego_planner" name="iris_$(arg drone_id)_ego_planner_node" type="ego_planner_node" output="screen">
+  
+    <remap from="~odom_world" to="/xtdrone/iris_$(arg drone_id)/$(arg odometry_topic)"/>
+    <remap from="~planning/bspline" to = "/xtdrone/iris_$(arg drone_id)/planning/bspline"/>
+    <remap from="~planning/data_display" to = "/xtdrone/iris_$(arg drone_id)/planning/data_display"/>
+    <remap from="~planning/broadcast_bspline_from_planner" to = "/broadcast_bspline"/>
+    <remap from="~planning/broadcast_bspline_to_planner" to = "/broadcast_bspline"/>
+
+    <remap from="~grid_map/odom" to="/xtdrone/iris_$(arg drone_id)/$(arg odometry_topic)"/>
+    <remap from="~grid_map/cloud" to="/iris_$(arg drone_id)/$(arg cloud_topic)"/>
+    <remap from="~grid_map/pose"   to = "/iris_$(arg drone_id)/$(arg camera_pose_topic)"/> 
+    <remap from="~grid_map/depth" to = "/iris_$(arg drone_id)/$(arg depth_topic)"/>
+    
+
+    <!-- planning fsm -->
+    <param name="fsm/flight_type" value="$(arg flight_type)" type="int"/>
+    <param name="fsm/thresh_replan_time" value="1.0" type="double"/>
+    <param name="fsm/thresh_no_replan_meter" value="1.0" type="double"/>
+    <param name="fsm/planning_horizon" value="$(arg planning_horizon)" type="double"/> <!--always set to 1.5 times grater than sensing horizen-->
+    <param name="fsm/planning_horizen_time" value="3" type="double"/>
+    <param name="fsm/emergency_time" value="1.0" type="double"/>
+    <param name="fsm/realworld_experiment" value="false"/>
+    <param name="fsm/fail_safe" value="true"/>
+
+    <param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/>
+    <param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
+    <param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/>
+    <param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/>
+    <param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/>
+    <param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/>
+    <param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/>
+    <param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/>
+    <param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/>
+    <param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/>
+    <param name="fsm/waypoint3_x" value="$(arg point3_x)" type="double"/>
+    <param name="fsm/waypoint3_y" value="$(arg point3_y)" type="double"/>
+    <param name="fsm/waypoint3_z" value="$(arg point3_z)" type="double"/>
+    <param name="fsm/waypoint4_x" value="$(arg point4_x)" type="double"/>
+    <param name="fsm/waypoint4_y" value="$(arg point4_y)" type="double"/>
+    <param name="fsm/waypoint4_z" value="$(arg point4_z)" type="double"/>
+
+    <param name="grid_map/resolution"      value="0.1" /> 
+    <param name="grid_map/map_size_x"   value="$(arg map_size_x_)" /> 
+    <param name="grid_map/map_size_y"   value="$(arg map_size_y_)" /> 
+    <param name="grid_map/map_size_z"   value="$(arg map_size_z_)" /> 
+    <param name="grid_map/local_update_range_x"  value="5.5" /> 
+    <param name="grid_map/local_update_range_y"  value="5.5" /> 
+    <param name="grid_map/local_update_range_z"  value="4.5" /> 
+    <param name="grid_map/obstacles_inflation"     value="0.2" /> 
+    <param name="grid_map/local_map_margin" value="10"/>
+    <param name="grid_map/ground_height"        value="0.24"/>
+    <!-- camera parameter -->
+    <param name="grid_map/cx" value="$(arg cx)"/>
+    <param name="grid_map/cy" value="$(arg cy)"/>
+    <param name="grid_map/fx" value="$(arg fx)"/>
+    <param name="grid_map/fy" value="$(arg fy)"/>
+    <!-- depth filter -->
+    <param name="grid_map/use_depth_filter" value="true"/>
+    <param name="grid_map/depth_filter_tolerance" value="0.15"/>
+    <param name="grid_map/depth_filter_maxdist"   value="5.0"/>
+    <param name="grid_map/depth_filter_mindist"   value="0.5"/>
+    <param name="grid_map/depth_filter_margin"    value="2"/>
+    <param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
+    <param name="grid_map/skip_pixel" value="2"/>
+    <!-- local fusion -->
+    <param name="grid_map/p_hit"  value="0.65"/>
+    <param name="grid_map/p_miss" value="0.35"/>
+    <param name="grid_map/p_min"  value="0.12"/>
+    <param name="grid_map/p_max"  value="0.90"/>
+    <param name="grid_map/p_occ"  value="0.80"/>
+    <param name="grid_map/min_ray_length" value="0.1"/>
+    <param name="grid_map/max_ray_length" value="4.5"/>
+
+    <param name="grid_map/virtual_ceil_height"   value="3"/>
+    <param name="grid_map/visualization_truncate_height"   value="2"/>
+    <param name="grid_map/show_occ_time"  value="false"/>
+    <param name="grid_map/pose_type"     value="1"/>  
+    <param name="grid_map/frame_id"      value="world"/>
+
+  <!-- planner manager -->
+    <param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
+    <param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
+    <param name="manager/max_jerk" value="4" type="double"/>
+    <param name="manager/control_points_distance" value="0.4" type="double"/>
+    <param name="manager/feasibility_tolerance" value="0.05" type="double"/>
+    <param name="manager/planning_horizon" value="$(arg planning_horizon)" type="double"/>
+    <param name="manager/use_distinctive_trajs" value="$(arg use_distinctive_trajs)" type="bool"/>
+    <param name="manager/drone_id" value="$(arg drone_id)"/>
+
+  <!-- trajectory optimization -->
+    <param name="optimization/lambda_smooth" value="1.0" type="double"/>
+    <param name="optimization/lambda_collision" value="0.5" type="double"/>
+    <param name="optimization/lambda_feasibility" value="0.1" type="double"/>
+    <param name="optimization/lambda_fitness" value="1.0" type="double"/>
+    <param name="optimization/dist0" value="0.5" type="double"/>
+    <param name="optimization/swarm_clearance" value="0.5" type="double"/>
+    <param name="optimization/max_vel" value="$(arg max_vel)" type="double"/>
+    <param name="optimization/max_acc" value="$(arg max_acc)" type="double"/>
+
+    <param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
+    <param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
+    <param name="bspline/limit_ratio" value="1.1" type="double"/>
+
+  <!-- objects prediction -->
+    <param name="prediction/obj_num" value="$(arg obj_num_set)" type="int"/>
+    <param name="prediction/lambda" value="1.0" type="double"/>
+    <param name="prediction/predict_rate" value="1.0" type="double"/>
+  
+  
+
+
+  </node>
+
+</launch>

motion_planning/3d/ego_planner/planner/plan_manage/launch/run_in_sim.launch

@@ -0,0 +1,135 @@
+<launch>
+  <!-- size of map, change the size inflate x, y, z according to your application -->
+  <arg name="map_size_x"/>
+  <arg name="map_size_y"/>
+  <arg name="map_size_z"/>
+  <arg name="init_x"/>
+  <arg name="init_y"/>
+  <arg name="init_z"/>
+  <arg name="target_x"/>
+  <arg name="target_y"/>
+  <arg name="target_z"/>
+
+  <arg name="drone_id"/>
+
+  <!-- topic of your odometry such as VIO or LIO -->
+  <arg name="odom_topic"/>
+
+  <!-- number of moving objects -->
+  <arg name="obj_num" value="10" />
+
+  <!-- main algorithm params -->
+  <include file="$(find ego_planner)/launch/advanced_param.xml">
+
+    <arg name="drone_id" value="$(arg drone_id)"/>
+
+    <arg name="map_size_x_" value="$(arg map_size_x)"/>
+    <arg name="map_size_y_" value="$(arg map_size_y)"/>
+    <arg name="map_size_z_" value="$(arg map_size_z)"/>
+    <arg name="odometry_topic" value="$(arg odom_topic)"/>
+
+    <arg name="obj_num_set" value="$(arg obj_num)" />
+
+    <!-- camera pose: transform of camera frame in the world frame -->
+    <!-- depth topic: depth image, 640x480 by default -->
+    <!-- don't set cloud_topic if you already set these ones! -->
+    <arg name="camera_pose_topic" value="pcl_render_node/camera_pose"/>
+    <arg name="depth_topic" value="pcl_render_node/depth"/>
+
+    <!-- topic of point cloud measurement, such as from LIDAR  -->
+    <!-- don't set camera pose and depth, if you already set this one! -->
+    <arg name="cloud_topic" value="pcl_render_node/cloud"/>
+
+    <!-- intrinsic params of the depth camera -->
+    <arg name="cx" value="321.04638671875"/>
+    <arg name="cy" value="243.44969177246094"/>
+    <arg name="fx" value="387.229248046875"/>
+    <arg name="fy" value="387.229248046875"/>
+
+    <!-- maximum velocity and acceleration the drone will reach -->
+    <arg name="max_vel" value="2.0" />
+    <arg name="max_acc" value="3.0" />
+
+    <!--always set to 1.5 times grater than sensing horizen-->
+    <arg name="planning_horizon" value="7.5" /> 
+
+    <arg name="use_distinctive_trajs" value="true" />
+
+    <!-- 1: use 2D Nav Goal to select goal  -->
+    <!-- 2: use global waypoints below  -->
+    <arg name="flight_type" value="2" />
+    
+    <!-- global waypoints -->
+    <!-- It generates a piecewise min-snap traj passing all waypoints -->
+    <arg name="point_num" value="1" />
+
+    <arg name="point0_x" value="$(arg target_x)" />
+    <arg name="point0_y" value="$(arg target_y)" />
+    <arg name="point0_z" value="$(arg target_z)" />
+
+    <arg name="point1_x" value="0.0" />
+    <arg name="point1_y" value="15.0" />
+    <arg name="point1_z" value="1.0" />
+
+    <arg name="point2_x" value="15.0" />
+    <arg name="point2_y" value="0.0" />
+    <arg name="point2_z" value="1.0" />
+
+    <arg name="point3_x" value="0.0" />
+    <arg name="point3_y" value="-15.0" />
+    <arg name="point3_z" value="1.0" />
+
+    <arg name="point4_x" value="-15.0" />
+    <arg name="point4_y" value="0.0" />
+    <arg name="point4_z" value="1.0" />
+    
+  </include>
+
+  <!-- trajectory server -->
+  <node pkg="ego_planner" name="drone_$(arg drone_id)_traj_server" type="traj_server" output="screen">
+    <remap from="position_cmd" to="drone_$(arg drone_id)_planning/pos_cmd"/>
+    <remap from="~planning/bspline" to="drone_$(arg drone_id)_planning/bspline"/>
+
+    <param name="traj_server/time_forward" value="1.0" type="double"/>
+  </node>
+
+  <!--node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
+    <remap from="~odom" to="$(arg odom_topic)"/>        
+    <remap from="~goal" to="/move_base_simple/goal"/>
+    <remap from="~traj_start_trigger" to="/traj_start_trigger" />
+    <param name="waypoint_type" value="manual-lonely-waypoint"/>    
+  </node-->
+
+  <!-- use simulator -->
+  <include file="$(find ego_planner)/launch/simulator.xml">
+
+    <arg name="drone_id" value="$(arg drone_id)"/>
+
+    <arg name="map_size_x_" value="$(arg map_size_x)"/>
+    <arg name="map_size_y_" value="$(arg map_size_y)"/>
+    <arg name="map_size_z_" value="$(arg map_size_z)"/>
+
+    <arg name="init_x_" value="$(arg init_x)"/>
+    <arg name="init_y_" value="$(arg init_y)"/>
+    <arg name="init_z_" value="$(arg init_z)"/>
+
+    <arg name="odometry_topic" value="$(arg odom_topic)" />
+  </include>
+
+  <![CDATA[node pkg="plan_env" name="obj_generator" type="obj_generator" output="screen">
+    <param name="obj_generator/obj_num" value="$(arg obj_num)"/>    
+    <param name="obj_generator/x_size" value="12.0"/>  
+    <param name="obj_generator/y_size" value="12.0"/>  
+    <param name="obj_generator/h_size" value="1.0"/>   
+    <param name="obj_generator/vel" value="1.5"/>   
+    <param name="obj_generator/yaw_dot" value="2.0"/>   
+    <param name="obj_generator/acc_r1" value="1.0"/>   
+    <param name="obj_generator/acc_r2" value="1.0"/>   
+    <param name="obj_generator/acc_z" value="0.0"/>   
+    <param name="obj_generator/scale1" value="0.5"/>   
+    <param name="obj_generator/scale2" value="1.0"/>   
+    <param name="obj_generator/interval" value="100.0"/>    
+    <param name="obj_generator/input_type" value="1"/> <!-- 1: Vel input, 2: Acc input-->>   
+  </node]]>
+
+</launch>

motion_planning/3d/ego_planner/planner/plan_manage/launch/run_in_xtdrone.launch

@@ -0,0 +1,94 @@
+<launch>
+  <!-- size of map, change the size inflate x, y, z according to your application -->
+  <arg name="map_size_x"/>
+  <arg name="map_size_y"/>
+  <arg name="map_size_z"/>
+  <arg name="target_x"/>
+  <arg name="target_y"/>
+  <arg name="target_z"/>
+
+  <arg name="drone_id"/>
+
+  <!-- topic of your odometry such as VIO or LIO -->
+  <arg name="odom_topic"/>
+
+  <!-- number of moving objects -->
+  <arg name="obj_num" value="1" />
+
+  <!-- main algorithm params -->
+  <include file="$(find ego_planner)/launch/advanced_param_xtdrone.xml">
+
+    <arg name="drone_id" value="$(arg drone_id)"/>
+
+    <arg name="map_size_x_" value="$(arg map_size_x)"/>
+    <arg name="map_size_y_" value="$(arg map_size_y)"/>
+    <arg name="map_size_z_" value="$(arg map_size_z)"/>
+    <arg name="odometry_topic" value="$(arg odom_topic)"/>
+
+    <arg name="obj_num_set" value="$(arg obj_num)" />
+
+    <!-- camera pose: transform of camera frame in the world frame -->
+    <!-- depth topic: depth image, 640x480 by default -->
+    <!-- don't set cloud_topic if you already set these ones! -->
+    <arg name="camera_pose_topic" value="camera_pose"/>
+    <arg name="depth_topic" value="realsense/depth_camera/depth/image_raw"/>
+
+    <!-- topic of point cloud measurement, such as from LIDAR  -->
+    <!-- don't set camera pose and depth, if you already set this one! -->
+    <arg name="cloud_topic" value="pcl_render_node/points"/>
+
+    <!-- intrinsic params of the depth camera -->
+    <arg name="cx" value="320.5"/>
+    <arg name="cy" value="240.5"/>
+    <arg name="fx" value="554.254691191187"/>
+    <arg name="fy" value="554.254691191187"/>
+
+    <!-- maximum velocity and acceleration the drone will reach -->
+    <arg name="max_vel" value="1" />
+    <arg name="max_acc" value="2" />
+
+    <!--always set to 1.5 times grater than sensing horizen-->
+    <arg name="planning_horizon" value="7.5" /> 
+
+    <arg name="use_distinctive_trajs" value="true" />
+
+    <!-- 1: use 2D Nav Goal to select goal  -->
+    <!-- 2: use global waypoints below  -->
+    <arg name="flight_type" value="2" />
+    
+    <!-- global waypoints -->
+    <!-- It generates a piecewise min-snap traj passing all waypoints -->
+    <arg name="point_num" value="1" />
+
+    <arg name="point0_x" value="$(arg target_x)" />
+    <arg name="point0_y" value="$(arg target_y)" />
+    <arg name="point0_z" value="$(arg target_z)" />
+
+    <arg name="point1_x" value="6.0" />
+    <arg name="point1_y" value="0.0" />
+    <arg name="point1_z" value="1.5" />
+
+    <arg name="point2_x" value="8.0" />
+    <arg name="point2_y" value="-8.0" />
+    <arg name="point2_z" value="1.5" />
+
+    <arg name="point3_x" value="0.0" />
+    <arg name="point3_y" value="-15.0" />
+    <arg name="point3_z" value="1.0" />
+
+    <arg name="point4_x" value="-15.0" />
+    <arg name="point4_y" value="0.0" />
+    <arg name="point4_z" value="1.0" />
+    
+  </include>
+
+  <!-- trajectory server -->
+  <node pkg="ego_planner" name="iris_$(arg drone_id)_traj_server" type="traj_server" output="screen">
+    <remap from="position_cmd" to="/xtdrone/iris_$(arg drone_id)/planning/pos_cmd"/>
+    <remap from="pose_cmd" to="/xtdrone/iris_$(arg drone_id)/cmd_pose_enu"/>
+    <remap from="~planning/bspline" to="/xtdrone/iris_$(arg drone_id)/planning/bspline"/>
+
+    <param name="traj_server/time_forward" value="1.0" type="double"/>
+  </node>
+  
+</launch>

motion_planning/3d/ego_planner/planner/plan_manage/launch/rviz.launch

@@ -0,0 +1,3 @@
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/default.rviz" required="true" />
+</launch>

motion_planning/3d/ego_planner/planner/plan_manage/launch/simple_run.launch

@@ -0,0 +1,4 @@
+ <launch>
+    <include file="$(find ego_planner)/launch/rviz.launch"/>
+    <include file="$(find ego_planner)/launch/swarm_large.launch"/>
+ </launch> 

motion_planning/3d/ego_planner/planner/plan_manage/launch/simulator.xml

@@ -0,0 +1,141 @@
+<launch>
+  <arg name="init_x_"/>
+  <arg name="init_y_"/>
+  <arg name="init_z_"/> 
+  <arg name="obj_num" value="1" />
+  <arg name="map_size_x_"/>
+  <arg name="map_size_y_"/>
+  <arg name="map_size_z_"/>
+  <arg name="c_num"/>
+  <arg name="p_num"/>
+  <arg name="min_dist"/>
+  <arg name="odometry_topic"/>
+  <arg name="drone_id"/>
+
+  <!-- There are two kinds of maps you can choose, just comment out the one you don’t need like the follow. Have a try. /-->
+
+  <![CDATA[node pkg ="map_generator" name ="random_forest" type ="random_forest" output = "screen">    
+    <remap from="~odometry"   to="$(arg odometry_topic)"/>    
+    <param name="map/x_size"     value="$(arg map_size_x_)" />
+    <param name="map/y_size"     value="$(arg map_size_y_)" />
+    <param name="map/z_size"     value="$(arg map_size_z_)" />
+    <param name="map/resolution" value="0.1"/>        
+
+    <param name="ObstacleShape/seed" value="1"/>
+    <param name="map/obs_num"    value="$(arg p_num)"/>
+    <param name="ObstacleShape/lower_rad" value="0.5"/>
+    <param name="ObstacleShape/upper_rad" value="0.7"/>
+    <param name="ObstacleShape/lower_hei" value="0.0"/>
+    <param name="ObstacleShape/upper_hei" value="3.0"/>        
+
+    <param name="map/circle_num" value="$(arg c_num)"/>        
+    <param name="ObstacleShape/radius_l" value="0.7"/>        
+    <param name="ObstacleShape/radius_h" value="0.5"/>        
+    <param name="ObstacleShape/z_l" value="0.7"/>        
+    <param name="ObstacleShape/z_h" value="0.8"/>        
+    <param name="ObstacleShape/theta" value="0.5"/>        
+
+    <param name="sensing/radius" value="5.0"/>        
+    <param name="sensing/rate"   value="10.0"/>  
+    
+    <param name="min_distance" value="$(arg min_dist)"/>         
+  </node]]>
+
+  <![CDATA[node pkg="mockamap" type="mockamap_node" name="mockamap_node" output="screen">  
+      <remap from="/mock_map" to="/map_generator/global_cloud"/>
+
+      <param name="seed" type="int" value="127"/>
+      <param name="update_freq" type="double" value="0.5"/>
+
+      <!--  box edge length, unit meter-->
+      <param name="resolution" type="double" value="0.1"/>
+
+      <!-- map size unit meter-->
+      <param name="x_length" value="$(arg map_size_x_)"/>
+      <param name="y_length" value="$(arg map_size_y_)"/>
+      <param name="z_length" value="$(arg map_size_z_)"/>
+
+      <param name="type" type="int" value="1"/>
+      <!-- 1 perlin noise parameters -->
+      <!-- complexity:    base noise frequency,
+                              large value will be complex
+                              typical 0.0 ~ 0.5 -->
+      <!-- fill:          infill persentage
+                              typical: 0.4 ~ 0.0 -->
+      <!-- fractal:       large value will have more detail-->
+      <!-- attenuation:   for fractal attenuation
+                              typical: 0.0 ~ 0.5 -->
+
+      <param name="complexity"    type="double" value="0.05"/>
+      <param name="fill"          type="double" value="0.12"/>
+      <param name="fractal"       type="int"    value="1"/>
+      <param name="attenuation"   type="double" value="0.1"/>
+  </node]]>
+
+  
+  <node pkg="so3_quadrotor_simulator" type="quadrotor_simulator_so3" name="drone_$(arg drone_id)_quadrotor_simulator_so3" output="screen">
+        <param name="rate/odom" value="200.0"/>
+        <param name="simulator/init_state_x" value="$(arg init_x_)"/>
+        <param name="simulator/init_state_y" value="$(arg init_y_)"/>
+        <param name="simulator/init_state_z" value="$(arg init_z_)"/>
+
+        <remap from="~odom" to="drone_$(arg drone_id)_visual_slam/odom"/>
+        <remap from="~cmd" to="drone_$(arg drone_id)_so3_cmd"/>
+        <remap from="~force_disturbance" to="drone_$(arg drone_id)_force_disturbance"/>    
+        <remap from="~moment_disturbance" to="drone_$(arg drone_id)_moment_disturbance"/>        
+  </node>
+ 
+  <node pkg="nodelet" type="nodelet" args="standalone so3_control/SO3ControlNodelet" name="drone_$(arg drone_id)_so3_control" required="true" output="screen">
+        <param name="so3_control/init_state_x" value="$(arg init_x_)"/>
+        <param name="so3_control/init_state_y" value="$(arg init_y_)"/>
+        <param name="so3_control/init_state_z" value="$(arg init_z_)"/>
+        <remap from="~odom" to="drone_$(arg drone_id)_visual_slam/odom"/>
+        <remap from="~position_cmd" to="drone_$(arg drone_id)_planning/pos_cmd"/>
+        <remap from="~motors" to="drone_$(arg drone_id)_motors"/>
+        <remap from="~corrections" to="drone_$(arg drone_id)_corrections"/>
+        <remap from="~so3_cmd" to="drone_$(arg drone_id)_so3_cmd"/>
+        <rosparam file="$(find so3_control)/config/gains_hummingbird.yaml"/>
+        <rosparam file="$(find so3_control)/config/corrections_hummingbird.yaml"/>
+        <param name="mass" value="0.98"/>
+        <param name="use_angle_corrections " value="false"/>
+        <param name="use_external_yaw "      value="false"/>
+        <param name="gains/rot/z" value="1.0"/>    
+        <param name="gains/ang/z" value="0.1"/>        
+  </node>
+  
+  <!--<node pkg="poscmd_2_odom" name="drone_$(arg drone_id)_poscmd_2_odom" type="poscmd_2_odom" output="screen">
+        <param name="init_x" value="$(arg init_x_)"/>
+        <param name="init_y" value="$(arg init_y_)"/>
+        <param name="init_z" value="$(arg init_z_)"/>
+        <remap from="~command" to="drone_$(arg drone_id)_planning/pos_cmd"/>
+        <remap from="~odometry" to="drone_$(arg drone_id)_$(arg odometry_topic)"/>
+  </node>-->
+ 
+  <node pkg="odom_visualization" name="drone_$(arg drone_id)_odom_visualization" type="odom_visualization" output="screen">
+        <remap from="~odom" to="drone_$(arg drone_id)_visual_slam/odom"/>
+        <param name="color/a" value="1.0"/>    
+        <param name="color/r" value="0.0"/>        
+        <param name="color/g" value="0.0"/>        
+        <param name="color/b" value="0.0"/>       
+        <param name="covariance_scale" value="100.0"/>       
+        <param name="robot_scale" value="1.0"/>
+        <param name="tf45" value="false"/>
+        <param name="drone_id" value="drone_id"/>
+  </node>
+
+  <node pkg="local_sensing_node" type="pcl_render_node" name="drone_$(arg drone_id)_pcl_render_node" output="screen">
+        <rosparam command="load" file="$(find local_sensing_node)/params/camera.yaml" />
+        <param name="sensing_horizon"  value="5.0" />
+        <param name="sensing_rate"     value="30.0"/>
+        <param name="estimation_rate"  value="30.0"/>
+
+        <param name="map/x_size"     value="$(arg map_size_x_)"/>
+        <param name="map/y_size"     value="$(arg map_size_y_)"/>
+        <param name="map/z_size"     value="$(arg map_size_z_)"/>
+
+        <remap from="~global_map" to="/map_generator/global_cloud"/>
+        <remap from="~odometry"   to="/drone_$(arg drone_id)_$(arg odometry_topic)"/>
+        <remap from="~pcl_render_node/cloud"   to="/drone_$(arg drone_id)_pcl_render_node/cloud"/>
+  </node>
+
+</launch>

motion_planning/3d/ego_planner/planner/plan_manage/launch/single_uav.launch

@@ -0,0 +1,28 @@
+  <launch>
+    <arg name="map_size_x" value="20.0"/>
+    <arg name="map_size_y" value="20.0"/>
+    <arg name="map_size_z" value=" 3.0"/>
+
+    <arg name="odom_topic" value="ground_truth/odom" />
+
+    <node pkg="tf" type="static_transform_publisher" name="iris_0_map_to_world"
+    args="0.0 0.0 0 0.0 0.0 0.0 /map /world 40" />
+    <node pkg="tf" type="static_transform_publisher" name="iris_0_world_to_ground_plane"
+    args="0.0 0.0 0 0.0 0.0 0.0 /world /ground_plane 40" />
+
+    <include file="$(find ego_planner)/launch/run_in_xtdrone.launch">
+        <arg name="drone_id" value="0"/>
+
+        <arg name="target_x" value="3.2"/>
+        <arg name="target_y" value="-5.5"/>
+        <arg name="target_z" value="1.0"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+
+
+  </launch>

motion_planning/3d/ego_planner/planner/plan_manage/launch/swarm.launch

@@ -0,0 +1,205 @@
+  <launch>
+    <arg name="map_size_x" value="42.0"/>
+    <arg name="map_size_y" value="30.0"/>
+    <arg name="map_size_z" value=" 5.0"/>
+
+    <arg name="odom_topic" value="visual_slam/odom" />
+
+    <node pkg ="map_generator" name ="random_forest" type ="random_forest" output = "screen">    
+        <param name="map/x_size"     value="36" />
+        <param name="map/y_size"     value="20" />
+        <param name="map/z_size"     value="3" />
+        <param name="map/resolution" value="0.1"/>        
+
+        <param name="ObstacleShape/seed" value="1"/>
+        
+        <param name="map/obs_num"    value="200"/>
+        <param name="ObstacleShape/lower_rad" value="0.5"/>
+        <param name="ObstacleShape/upper_rad" value="0.7"/>
+        <param name="ObstacleShape/lower_hei" value="0.0"/>
+        <param name="ObstacleShape/upper_hei" value="3.0"/>        
+
+        <param name="map/circle_num" value="200"/>        
+        <param name="ObstacleShape/radius_l" value="0.7"/>        
+        <param name="ObstacleShape/radius_h" value="0.5"/>        
+        <param name="ObstacleShape/z_l" value="0.7"/>        
+        <param name="ObstacleShape/z_h" value="0.8"/>        
+        <param name="ObstacleShape/theta" value="0.5"/>        
+
+        <param name="sensing/radius" value="5.0"/>        
+        <param name="sensing/rate"   value="1.0"/>  
+        
+        <param name="min_distance" value="1.2"/>         
+    </node>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="0"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="-9.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="9.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="1"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="-7.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="7.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="2"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="-5.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="5.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="3"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="-3.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="3.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="4"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="-1.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="1.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="5"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="1.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="-1.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="6"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="3.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="-3.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="7"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="5.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="-5.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="8"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="7.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="-7.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="9"/>
+
+        <arg name="init_x" value="-20.0"/>
+        <arg name="init_y" value="9.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="20.0"/>
+        <arg name="target_y" value="-9.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+    
+  </launch>

motion_planning/3d/ego_planner/planner/plan_manage/launch/swarm_large.launch

@@ -0,0 +1,412 @@
+  <launch>
+    <arg name="map_size_x" value="72.0"/>
+    <arg name="map_size_y" value="30.0"/>
+    <arg name="map_size_z" value=" 5.0"/>
+
+    <arg name="odom_topic" value="visual_slam/odom" />
+
+    <node pkg ="map_generator" name ="random_forest" type ="random_forest" output = "screen">    
+        <param name="map/x_size"     value="66" />
+        <param name="map/y_size"     value="30" />
+        <param name="map/z_size"     value="3" />
+        <param name="map/resolution" value="0.1"/>        
+
+        <param name="ObstacleShape/seed" value="1"/>
+        
+        <param name="map/obs_num"    value="300"/>
+        <param name="ObstacleShape/lower_rad" value="0.5"/>
+        <param name="ObstacleShape/upper_rad" value="0.7"/>
+        <param name="ObstacleShape/lower_hei" value="0.0"/>
+        <param name="ObstacleShape/upper_hei" value="3.0"/>        
+
+        <param name="map/circle_num" value="200"/>        
+        <param name="ObstacleShape/radius_l" value="0.7"/>        
+        <param name="ObstacleShape/radius_h" value="0.5"/>        
+        <param name="ObstacleShape/z_l" value="0.7"/>        
+        <param name="ObstacleShape/z_h" value="0.8"/>        
+        <param name="ObstacleShape/theta" value="0.5"/>        
+
+        <param name="sensing/radius" value="5.0"/>        
+        <param name="sensing/rate"   value="1.0"/>  
+        
+        <param name="min_distance" value="1.2"/>         
+    </node>
+
+    <!-- <node pkg="mockamap" type="mockamap_node" name="mockamap_node" output="screen">  
+      <remap from="/mock_map" to="/map_generator/global_cloud"/>
+
+      <param name="seed" type="int" value="127"/>
+      <param name="update_freq" type="double" value="0.5"/>
+
+      <param name="resolution" type="double" value="0.1"/>
+
+      <param name="x_length" value="66"/>
+      <param name="y_length" value="30"/>
+      <param name="z_length" value="3"/>
+
+      <param name="type" type="int" value="1"/>
+
+      <param name="complexity"    type="double" value="0.05"/>
+      <param name="fill"          type="double" value="0.12"/>
+      <param name="fractal"       type="int"    value="1"/>
+      <param name="attenuation"   type="double" value="0.1"/>
+    </node> -->
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="0"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-10.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-10.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="1"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-9.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-9.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="2"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-8.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-8.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="3"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-7.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-7.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="4"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-6.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-6.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="5"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-5.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-5.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="6"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-4.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-4.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="7"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-3.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-3.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="8"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-2.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-2.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="9"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="-1.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="-1.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="10"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="0.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="0.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="11"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="1.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="1.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="12"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="2.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="2.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="13"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="3.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="3.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="14"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="4.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="4.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="15"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="5.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="5.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="16"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="6.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="6.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="17"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="7.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="7.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="18"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="8.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="8.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="19"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="9.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="9.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+
+    <include file="$(find ego_planner)/launch/run_in_sim.launch">
+        <arg name="drone_id" value="20"/>
+
+        <arg name="init_x" value="-35.0"/>
+        <arg name="init_y" value="10.0"/>
+         <arg name="init_z" value="0.1"/>
+
+        <arg name="target_x" value="35.0"/>
+        <arg name="target_y" value="10.0"/>
+        <arg name="target_z" value="1"/>
+
+        <arg name="map_size_x" value="$(arg map_size_x)"/>
+        <arg name="map_size_y" value="$(arg map_size_y)"/>
+        <arg name="map_size_z" value="$(arg map_size_z)"/>
+        <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+    
+  </launch>

motion_planning/3d/ego_planner/planner/plan_manage/package.xml

@@ -0,0 +1,82 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ego_planner</name>
+  <version>0.0.0</version>
+  <description>The ego_planner package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ego_planner</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  
+  <build_depend>plan_env</build_depend>
+  <build_depend>path_searching</build_depend>
+  <build_depend>bspline_opt</build_depend>
+  <build_depend>traj_utils</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <exec_depend>plan_env</exec_depend>
+  <exec_depend>path_searching</exec_depend>
+  <exec_depend>bspline_opt</exec_depend>
+  <exec_depend>traj_utils</exec_depend>
+  
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
+
+
+<!-- catkin_make -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=Yes -->

motion_planning/3d/ego_planner/planner/plan_manage/src/ego_planner_node.cpp

@@ -0,0 +1,56 @@
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+
+#include <plan_manage/ego_replan_fsm.h>
+
+using namespace ego_planner;
+
+int main(int argc, char **argv)
+{
+
+  ros::init(argc, argv, "ego_planner_node");
+  ros::NodeHandle nh("~");
+
+  EGOReplanFSM rebo_replan;
+
+  rebo_replan.init(nh);
+
+  // ros::Duration(1.0).sleep();
+  ros::spin();
+
+  return 0;
+}
+
+// #include <ros/ros.h>
+// #include <csignal>
+// #include <visualization_msgs/Marker.h>
+
+// #include <plan_manage/ego_replan_fsm.h>
+
+// using namespace ego_planner;
+
+// void SignalHandler(int signal) {
+//   if(ros::isInitialized() && ros::isStarted() && ros::ok() && !ros::isShuttingDown()){
+//     ros::shutdown();
+//   }
+// }
+
+// int main(int argc, char **argv) {
+
+//   signal(SIGINT, SignalHandler);
+//   signal(SIGTERM,SignalHandler);
+
+//   ros::init(argc, argv, "ego_planner_node", ros::init_options::NoSigintHandler);
+//   ros::NodeHandle nh("~");
+
+//   EGOReplanFSM rebo_replan;
+
+//   rebo_replan.init(nh);
+
+//   // ros::Duration(1.0).sleep();
+//   ros::AsyncSpinner async_spinner(4);
+//   async_spinner.start();
+//   ros::waitForShutdown();
+
+//   return 0;
+// }

motion_planning/3d/ego_planner/planner/plan_manage/src/ego_replan_fsm.cpp

@@ -0,0 +1,948 @@
+
+#include <plan_manage/ego_replan_fsm.h>
+
+namespace ego_planner
+{
+
+  void EGOReplanFSM::init(ros::NodeHandle &nh)
+  {
+    current_wp_ = 0;
+    exec_state_ = FSM_EXEC_STATE::INIT;
+    have_target_ = false;
+    have_odom_ = false;
+    have_recv_pre_agent_ = false;
+
+    /*  fsm param  */
+    nh.param("fsm/flight_type", target_type_, -1);
+    nh.param("fsm/thresh_replan_time", replan_thresh_, -1.0);
+    nh.param("fsm/thresh_no_replan_meter", no_replan_thresh_, -1.0);
+    nh.param("fsm/planning_horizon", planning_horizen_, -1.0);
+    nh.param("fsm/planning_horizen_time", planning_horizen_time_, -1.0);
+    nh.param("fsm/emergency_time", emergency_time_, 1.0);
+    nh.param("fsm/realworld_experiment", flag_realworld_experiment_, false);
+    nh.param("fsm/fail_safe", enable_fail_safe_, true);
+
+    have_trigger_ = !flag_realworld_experiment_;
+
+    nh.param("fsm/waypoint_num", waypoint_num_, -1);
+    for (int i = 0; i < waypoint_num_; i++)
+    {
+      nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0);
+      nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0);
+      nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0);
+    }
+
+    /* initialize main modules */
+    visualization_.reset(new PlanningVisualization(nh));
+    planner_manager_.reset(new EGOPlannerManager);
+    planner_manager_->initPlanModules(nh, visualization_);
+    planner_manager_->deliverTrajToOptimizer(); // store trajectories
+    planner_manager_->setDroneIdtoOpt();
+
+    /* callback */
+    exec_timer_ = nh.createTimer(ros::Duration(0.01), &EGOReplanFSM::execFSMCallback, this);
+    safety_timer_ = nh.createTimer(ros::Duration(0.05), &EGOReplanFSM::checkCollisionCallback, this);
+
+    odom_sub_ = nh.subscribe("odom_world", 1, &EGOReplanFSM::odometryCallback, this);
+
+    if (planner_manager_->pp_.drone_id >= 1)
+    {
+      string sub_topic_name = string("/drone_") + std::to_string(planner_manager_->pp_.drone_id - 1) + string("_planning/swarm_trajs");
+      swarm_trajs_sub_ = nh.subscribe(sub_topic_name.c_str(), 10, &EGOReplanFSM::swarmTrajsCallback, this, ros::TransportHints().tcpNoDelay());
+    }
+    string pub_topic_name = string("/drone_") + std::to_string(planner_manager_->pp_.drone_id) + string("_planning/swarm_trajs");
+    swarm_trajs_pub_ = nh.advertise<traj_utils::MultiBsplines>(pub_topic_name.c_str(), 10);
+
+    broadcast_bspline_pub_ = nh.advertise<traj_utils::Bspline>("planning/broadcast_bspline_from_planner", 10);
+    broadcast_bspline_sub_ = nh.subscribe("planning/broadcast_bspline_to_planner", 100, &EGOReplanFSM::BroadcastBsplineCallback, this, ros::TransportHints().tcpNoDelay());
+
+    bspline_pub_ = nh.advertise<traj_utils::Bspline>("planning/bspline", 10);
+    data_disp_pub_ = nh.advertise<traj_utils::DataDisp>("planning/data_display", 100);
+
+    if (target_type_ == TARGET_TYPE::MANUAL_TARGET)
+    {
+      waypoint_sub_ = nh.subscribe("/move_base_simple/goal", 1, &EGOReplanFSM::waypointCallback, this);
+    }
+    else if (target_type_ == TARGET_TYPE::PRESET_TARGET)
+    {
+      trigger_sub_ = nh.subscribe("/traj_start_trigger", 1, &EGOReplanFSM::triggerCallback, this);
+
+      ROS_INFO("Wait for 1 second.");
+      int count = 0;
+      while (ros::ok() && count++ < 1000)
+      {
+        ros::spinOnce();
+        ros::Duration(0.001).sleep();
+      }
+
+      ROS_WARN("Waiting for trigger from [n3ctrl] from RC");
+
+      while (ros::ok() && (!have_odom_ || !have_trigger_))
+      {
+        ros::spinOnce();
+        ros::Duration(0.001).sleep();
+      }
+
+      readGivenWps();
+    }
+    else
+      cout << "Wrong target_type_ value! target_type_=" << target_type_ << endl;
+  }
+
+  void EGOReplanFSM::readGivenWps()
+  {
+    if (waypoint_num_ <= 0)
+    {
+      ROS_ERROR("Wrong waypoint_num_ = %d", waypoint_num_);
+      return;
+    }
+
+    wps_.resize(waypoint_num_);
+    for (int i = 0; i < waypoint_num_; i++)
+    {
+      wps_[i](0) = waypoints_[i][0];
+      wps_[i](1) = waypoints_[i][1];
+      wps_[i](2) = waypoints_[i][2];
+
+      // end_pt_ = wps_.back();
+    }
+
+    // bool success = planner_manager_->planGlobalTrajWaypoints(
+    //   odom_pos_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
+    //   wps_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
+
+    for (size_t i = 0; i < (size_t)waypoint_num_; i++)
+    {
+      visualization_->displayGoalPoint(wps_[i], Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, i);
+      ros::Duration(0.001).sleep();
+    }
+
+    // plan first global waypoint
+    wp_id_ = 0;
+    planNextWaypoint(wps_[wp_id_]);
+
+    // if (success)
+    // {
+
+    //   /*** display ***/
+    //   constexpr double step_size_t = 0.1;
+    //   int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
+    //   std::vector<Eigen::Vector3d> gloabl_traj(i_end);
+    //   for (int i = 0; i < i_end; i++)
+    //   {
+    //     gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t);
+    //   }
+
+    //   end_vel_.setZero();
+    //   have_target_ = true;
+    //   have_new_target_ = true;
+
+    //   /*** FSM ***/
+    //   // if (exec_state_ == WAIT_TARGET)
+    //   //changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
+    //   // trigger_ = true;
+    //   // else if (exec_state_ == EXEC_TRAJ)
+    //   //   changeFSMExecState(REPLAN_TRAJ, "TRIG");
+
+    //   // visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(1, 0, 0, 1), 0.3, 0);
+    //   ros::Duration(0.001).sleep();
+    //   visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
+    //   ros::Duration(0.001).sleep();
+    // }
+    // else
+    // {
+    //   ROS_ERROR("Unable to generate global trajectory!");
+    // }
+  }
+
+  void EGOReplanFSM::planNextWaypoint(const Eigen::Vector3d next_wp)
+  {
+    bool success = false;
+    success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), next_wp, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
+
+    // visualization_->displayGoalPoint(next_wp, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0);
+
+    if (success)
+    {
+      end_pt_ = next_wp;
+
+      /*** display ***/
+      constexpr double step_size_t = 0.1;
+      int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
+      vector<Eigen::Vector3d> gloabl_traj(i_end);
+      for (int i = 0; i < i_end; i++)
+      {
+        gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t);
+      }
+
+      end_vel_.setZero();
+      have_target_ = true;
+      have_new_target_ = true;
+
+      /*** FSM ***/
+      if (exec_state_ == WAIT_TARGET)
+        changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
+      else
+      {
+        while (exec_state_ != EXEC_TRAJ)
+        {
+          ros::spinOnce();
+          ros::Duration(0.001).sleep();
+        }
+        changeFSMExecState(REPLAN_TRAJ, "TRIG");
+      }
+
+      // visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(1, 0, 0, 1), 0.3, 0);
+      visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
+    }
+    else
+    {
+      ROS_ERROR("Unable to generate global trajectory!");
+    }
+  }
+
+  void EGOReplanFSM::triggerCallback(const geometry_msgs::PoseStampedPtr &msg)
+  {
+    have_trigger_ = true;
+    cout << "Triggered!" << endl;
+    init_pt_ = odom_pos_;
+  }
+
+  void EGOReplanFSM::waypointCallback(const geometry_msgs::PoseStampedPtr &msg)
+  {
+    if (msg->pose.position.z < -0.1)
+      return;
+
+    cout << "Triggered!" << endl;
+    // trigger_ = true;
+    init_pt_ = odom_pos_;
+
+    Eigen::Vector3d end_wp(msg->pose.position.x, msg->pose.position.y, 1.0);
+
+    planNextWaypoint(end_wp);
+  }
+
+  void EGOReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr &msg)
+  {
+    odom_pos_(0) = msg->pose.pose.position.x;
+    odom_pos_(1) = msg->pose.pose.position.y;
+    odom_pos_(2) = msg->pose.pose.position.z;
+
+    odom_vel_(0) = msg->twist.twist.linear.x;
+    odom_vel_(1) = msg->twist.twist.linear.y;
+    odom_vel_(2) = msg->twist.twist.linear.z;
+
+    //odom_acc_ = estimateAcc( msg );
+
+    odom_orient_.w() = msg->pose.pose.orientation.w;
+    odom_orient_.x() = msg->pose.pose.orientation.x;
+    odom_orient_.y() = msg->pose.pose.orientation.y;
+    odom_orient_.z() = msg->pose.pose.orientation.z;
+
+    have_odom_ = true;
+  }
+
+  void EGOReplanFSM::BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg)
+  {
+    size_t id = msg->drone_id;
+    if ((int)id == planner_manager_->pp_.drone_id)
+      return;
+
+    if (abs((ros::Time::now() - msg->start_time).toSec()) > 0.25)
+    {
+      ROS_ERROR("Time difference is too large! Local - Remote Agent %d = %fs",
+                msg->drone_id, (ros::Time::now() - msg->start_time).toSec());
+      return;
+    }
+
+    /* Fill up the buffer */
+    if (planner_manager_->swarm_trajs_buf_.size() <= id)
+    {
+      for (size_t i = planner_manager_->swarm_trajs_buf_.size(); i <= id; i++)
+      {
+        OneTrajDataOfSwarm blank;
+        blank.drone_id = -1;
+        planner_manager_->swarm_trajs_buf_.push_back(blank);
+      }
+    }
+
+    /* Test distance to the agent */
+    Eigen::Vector3d cp0(msg->pos_pts[0].x, msg->pos_pts[0].y, msg->pos_pts[0].z);
+    Eigen::Vector3d cp1(msg->pos_pts[1].x, msg->pos_pts[1].y, msg->pos_pts[1].z);
+    Eigen::Vector3d cp2(msg->pos_pts[2].x, msg->pos_pts[2].y, msg->pos_pts[2].z);
+    Eigen::Vector3d swarm_start_pt = (cp0 + 4 * cp1 + cp2) / 6;
+    if ((swarm_start_pt - odom_pos_).norm() > planning_horizen_ * 4.0f / 3.0f)
+    {
+      planner_manager_->swarm_trajs_buf_[id].drone_id = -1;
+      return; // if the current drone is too far to the received agent.
+    }
+
+    /* Store data */
+    Eigen::MatrixXd pos_pts(3, msg->pos_pts.size());
+    Eigen::VectorXd knots(msg->knots.size());
+    for (size_t j = 0; j < msg->knots.size(); ++j)
+    {
+      knots(j) = msg->knots[j];
+    }
+    for (size_t j = 0; j < msg->pos_pts.size(); ++j)
+    {
+      pos_pts(0, j) = msg->pos_pts[j].x;
+      pos_pts(1, j) = msg->pos_pts[j].y;
+      pos_pts(2, j) = msg->pos_pts[j].z;
+    }
+
+    planner_manager_->swarm_trajs_buf_[id].drone_id = id;
+
+    if (msg->order % 2)
+    {
+      double cutback = (double)msg->order / 2 + 1.5;
+      planner_manager_->swarm_trajs_buf_[id].duration_ = msg->knots[msg->knots.size() - ceil(cutback)];
+    }
+    else
+    {
+      double cutback = (double)msg->order / 2 + 1.5;
+      planner_manager_->swarm_trajs_buf_[id].duration_ = (msg->knots[msg->knots.size() - floor(cutback)] + msg->knots[msg->knots.size() - ceil(cutback)]) / 2;
+    }
+
+    UniformBspline pos_traj(pos_pts, msg->order, msg->knots[1] - msg->knots[0]);
+    pos_traj.setKnot(knots);
+    planner_manager_->swarm_trajs_buf_[id].position_traj_ = pos_traj;
+
+    planner_manager_->swarm_trajs_buf_[id].start_pos_ = planner_manager_->swarm_trajs_buf_[id].position_traj_.evaluateDeBoorT(0);
+
+    planner_manager_->swarm_trajs_buf_[id].start_time_ = msg->start_time;
+    // planner_manager_->swarm_trajs_buf_[id].start_time_ = ros::Time::now(); // Un-reliable time sync
+
+    /* Check Collision */
+    if (planner_manager_->checkCollision(id))
+    {
+      changeFSMExecState(REPLAN_TRAJ, "TRAJ_CHECK");
+    }
+  }
+
+  void EGOReplanFSM::swarmTrajsCallback(const traj_utils::MultiBsplinesPtr &msg)
+  {
+
+    multi_bspline_msgs_buf_.traj.clear();
+    multi_bspline_msgs_buf_ = *msg;
+
+    // cout << "\033[45;33mmulti_bspline_msgs_buf.drone_id_from=" << multi_bspline_msgs_buf_.drone_id_from << " multi_bspline_msgs_buf_.traj.size()=" << multi_bspline_msgs_buf_.traj.size() << "\033[0m" << endl;
+
+    if (!have_odom_)
+    {
+      ROS_ERROR("swarmTrajsCallback(): no odom!, return.");
+      return;
+    }
+
+    if ((int)msg->traj.size() != msg->drone_id_from + 1) // drone_id must start from 0
+    {
+      ROS_ERROR("Wrong trajectory size! msg->traj.size()=%d, msg->drone_id_from+1=%d", msg->traj.size(), msg->drone_id_from + 1);
+      return;
+    }
+
+    if (msg->traj[0].order != 3) // only support B-spline order equals 3.
+    {
+      ROS_ERROR("Only support B-spline order equals 3.");
+      return;
+    }
+
+    // Step 1. receive the trajectories
+    planner_manager_->swarm_trajs_buf_.clear();
+    planner_manager_->swarm_trajs_buf_.resize(msg->traj.size());
+
+    for (size_t i = 0; i < msg->traj.size(); i++)
+    {
+
+      Eigen::Vector3d cp0(msg->traj[i].pos_pts[0].x, msg->traj[i].pos_pts[0].y, msg->traj[i].pos_pts[0].z);
+      Eigen::Vector3d cp1(msg->traj[i].pos_pts[1].x, msg->traj[i].pos_pts[1].y, msg->traj[i].pos_pts[1].z);
+      Eigen::Vector3d cp2(msg->traj[i].pos_pts[2].x, msg->traj[i].pos_pts[2].y, msg->traj[i].pos_pts[2].z);
+      Eigen::Vector3d swarm_start_pt = (cp0 + 4 * cp1 + cp2) / 6;
+      if ((swarm_start_pt - odom_pos_).norm() > planning_horizen_ * 4.0f / 3.0f)
+      {
+        planner_manager_->swarm_trajs_buf_[i].drone_id = -1;
+        continue;
+      }
+
+      Eigen::MatrixXd pos_pts(3, msg->traj[i].pos_pts.size());
+      Eigen::VectorXd knots(msg->traj[i].knots.size());
+      for (size_t j = 0; j < msg->traj[i].knots.size(); ++j)
+      {
+        knots(j) = msg->traj[i].knots[j];
+      }
+      for (size_t j = 0; j < msg->traj[i].pos_pts.size(); ++j)
+      {
+        pos_pts(0, j) = msg->traj[i].pos_pts[j].x;
+        pos_pts(1, j) = msg->traj[i].pos_pts[j].y;
+        pos_pts(2, j) = msg->traj[i].pos_pts[j].z;
+      }
+
+      planner_manager_->swarm_trajs_buf_[i].drone_id = i;
+
+      if (msg->traj[i].order % 2)
+      {
+        double cutback = (double)msg->traj[i].order / 2 + 1.5;
+        planner_manager_->swarm_trajs_buf_[i].duration_ = msg->traj[i].knots[msg->traj[i].knots.size() - ceil(cutback)];
+      }
+      else
+      {
+        double cutback = (double)msg->traj[i].order / 2 + 1.5;
+        planner_manager_->swarm_trajs_buf_[i].duration_ = (msg->traj[i].knots[msg->traj[i].knots.size() - floor(cutback)] + msg->traj[i].knots[msg->traj[i].knots.size() - ceil(cutback)]) / 2;
+      }
+
+      // planner_manager_->swarm_trajs_buf_[i].position_traj_ =
+      UniformBspline pos_traj(pos_pts, msg->traj[i].order, msg->traj[i].knots[1] - msg->traj[i].knots[0]);
+      pos_traj.setKnot(knots);
+      planner_manager_->swarm_trajs_buf_[i].position_traj_ = pos_traj;
+
+      planner_manager_->swarm_trajs_buf_[i].start_pos_ = planner_manager_->swarm_trajs_buf_[i].position_traj_.evaluateDeBoorT(0);
+
+      planner_manager_->swarm_trajs_buf_[i].start_time_ = msg->traj[i].start_time;
+    }
+
+    have_recv_pre_agent_ = true;
+  }
+
+  void EGOReplanFSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call)
+  {
+
+    if (new_state == exec_state_)
+      continously_called_times_++;
+    else
+      continously_called_times_ = 1;
+
+    static string state_str[8] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP", "SEQUENTIAL_START"};
+    int pre_s = int(exec_state_);
+    exec_state_ = new_state;
+    cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl;
+  }
+
+  std::pair<int, EGOReplanFSM::FSM_EXEC_STATE> EGOReplanFSM::timesOfConsecutiveStateCalls()
+  {
+    return std::pair<int, FSM_EXEC_STATE>(continously_called_times_, exec_state_);
+  }
+
+  void EGOReplanFSM::printFSMExecState()
+  {
+    static string state_str[8] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP", "SEQUENTIAL_START"};
+
+    cout << "[FSM]: state: " + state_str[int(exec_state_)] << endl;
+  }
+
+  void EGOReplanFSM::execFSMCallback(const ros::TimerEvent &e)
+  {
+    exec_timer_.stop(); // To avoid blockage
+
+    static int fsm_num = 0;
+    fsm_num++;
+    if (fsm_num == 100)
+    {
+      printFSMExecState();
+      if (!have_odom_)
+        cout << "no odom." << endl;
+      if (!have_target_)
+        cout << "wait for goal or trigger." << endl;
+      fsm_num = 0;
+    }
+
+    switch (exec_state_)
+    {
+    case INIT:
+    {
+      if (!have_odom_)
+      {
+        goto force_return;
+        // return;
+      }
+      changeFSMExecState(WAIT_TARGET, "FSM");
+      break;
+    }
+
+    case WAIT_TARGET:
+    {
+      if (!have_target_ || !have_trigger_)
+        goto force_return;
+      // return;
+      else
+      {
+        // if ( planner_manager_->pp_.drone_id <= 0 )
+        // {
+        //   changeFSMExecState(GEN_NEW_TRAJ, "FSM");
+        // }
+        // else
+        // {
+        changeFSMExecState(SEQUENTIAL_START, "FSM");
+        // }
+      }
+      break;
+    }
+
+    case SEQUENTIAL_START: // for swarm
+    {
+      // cout << "id=" << planner_manager_->pp_.drone_id << " have_recv_pre_agent_=" << have_recv_pre_agent_ << endl;
+      if (planner_manager_->pp_.drone_id <= 0 || (planner_manager_->pp_.drone_id >= 1 && have_recv_pre_agent_))
+      {
+        if (have_odom_ && have_target_ && have_trigger_)
+        {
+          bool success = planFromGlobalTraj(10); // zx-todo
+          if (success)
+          {
+            changeFSMExecState(EXEC_TRAJ, "FSM");
+
+            publishSwarmTrajs(true);
+          }
+          else
+          {
+            ROS_ERROR("Failed to generate the first trajectory!!!");
+            changeFSMExecState(SEQUENTIAL_START, "FSM");
+          }
+        }
+        else
+        {
+          ROS_ERROR("No odom or no target! have_odom_=%d, have_target_=%d", have_odom_, have_target_);
+        }
+      }
+
+      break;
+    }
+
+    case GEN_NEW_TRAJ:
+    {
+
+      // Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
+      // start_yaw_(0)         = atan2(rot_x(1), rot_x(0));
+      // start_yaw_(1) = start_yaw_(2) = 0.0;
+
+      bool success = planFromGlobalTraj(10); // zx-todo
+      if (success)
+      {
+        changeFSMExecState(EXEC_TRAJ, "FSM");
+        flag_escape_emergency_ = true;
+        publishSwarmTrajs(false);
+      }
+      else
+      {
+        changeFSMExecState(GEN_NEW_TRAJ, "FSM");
+      }
+      break;
+    }
+
+    case REPLAN_TRAJ:
+    {
+
+      if (planFromCurrentTraj(1))
+      {
+        changeFSMExecState(EXEC_TRAJ, "FSM");
+        publishSwarmTrajs(false);
+      }
+      else
+      {
+        changeFSMExecState(REPLAN_TRAJ, "FSM");
+      }
+
+      break;
+    }
+
+    case EXEC_TRAJ:
+    {
+      /* determine if need to replan */
+      LocalTrajData *info = &planner_manager_->local_data_;
+      ros::Time time_now = ros::Time::now();
+      double t_cur = (time_now - info->start_time_).toSec();
+      t_cur = min(info->duration_, t_cur);
+
+      Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);
+
+      /* && (end_pt_ - pos).norm() < 0.5 */
+      if ((target_type_ == TARGET_TYPE::PRESET_TARGET) &&
+          (wp_id_ < waypoint_num_-1) &&
+          (end_pt_ - pos).norm() < no_replan_thresh_)
+      {
+        wp_id_++;
+        planNextWaypoint(wps_[wp_id_]);
+      }
+      else if ((local_target_pt_ - end_pt_).norm() < 1e-3) // close to the global target
+      {
+        if (t_cur > info->duration_ - 1e-2)
+        {
+          have_target_ = false;
+          have_trigger_ = false;
+
+          if ( target_type_ == TARGET_TYPE::PRESET_TARGET )
+          {
+            wp_id_ = 0;
+            planNextWaypoint(wps_[wp_id_]);
+          }
+
+          changeFSMExecState(WAIT_TARGET, "FSM");
+          goto force_return;
+          // return;
+        }
+        else if ((end_pt_ - pos).norm() > no_replan_thresh_ && t_cur > replan_thresh_)
+        {
+          changeFSMExecState(REPLAN_TRAJ, "FSM");
+        }
+      }
+      else if (t_cur > replan_thresh_)
+      {
+        changeFSMExecState(REPLAN_TRAJ, "FSM");
+      }
+
+      break;
+    }
+
+    case EMERGENCY_STOP:
+    {
+
+      if (flag_escape_emergency_) // Avoiding repeated calls
+      {
+        callEmergencyStop(odom_pos_);
+      }
+      else
+      {
+        if (enable_fail_safe_ && odom_vel_.norm() < 0.1)
+          changeFSMExecState(GEN_NEW_TRAJ, "FSM");
+      }
+
+      flag_escape_emergency_ = false;
+      break;
+    }
+    }
+
+    data_disp_.header.stamp = ros::Time::now();
+    data_disp_pub_.publish(data_disp_);
+
+  force_return:;
+    exec_timer_.start();
+  }
+
+  bool EGOReplanFSM::planFromGlobalTraj(const int trial_times /*=1*/) //zx-todo
+  {
+    start_pt_ = odom_pos_;
+    start_vel_ = odom_vel_;
+    start_acc_.setZero();
+
+    bool flag_random_poly_init;
+    if (timesOfConsecutiveStateCalls().first == 1)
+      flag_random_poly_init = false;
+    else
+      flag_random_poly_init = true;
+
+    for (int i = 0; i < trial_times; i++)
+    {
+      if (callReboundReplan(true, flag_random_poly_init))
+      {
+        return true;
+      }
+    }
+    return false;
+  }
+
+  bool EGOReplanFSM::planFromCurrentTraj(const int trial_times /*=1*/)
+  {
+
+    LocalTrajData *info = &planner_manager_->local_data_;
+    ros::Time time_now = ros::Time::now();
+    double t_cur = (time_now - info->start_time_).toSec();
+
+    //cout << "info->velocity_traj_=" << info->velocity_traj_.get_control_points() << endl;
+
+    start_pt_ = info->position_traj_.evaluateDeBoorT(t_cur);
+    start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur);
+    start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur);
+
+    bool success = callReboundReplan(false, false);
+
+    if (!success)
+    {
+      success = callReboundReplan(true, false);
+      //changeFSMExecState(EXEC_TRAJ, "FSM");
+      if (!success)
+      {
+        for (int i = 0; i < trial_times; i++)
+        {
+          success = callReboundReplan(true, true);
+          if (success)
+            break;
+        }
+        if (!success)
+        {
+          return false;
+        }
+      }
+    }
+
+    return true;
+  }
+
+  void EGOReplanFSM::checkCollisionCallback(const ros::TimerEvent &e)
+  {
+
+    LocalTrajData *info = &planner_manager_->local_data_;
+    auto map = planner_manager_->grid_map_;
+
+    if (exec_state_ == WAIT_TARGET || info->start_time_.toSec() < 1e-5)
+      return;
+
+    /* ---------- check lost of depth ---------- */
+    if (map->getOdomDepthTimeout())
+    {
+      ROS_ERROR("Depth Lost! EMERGENCY_STOP");
+      enable_fail_safe_ = false;
+      changeFSMExecState(EMERGENCY_STOP, "SAFETY");
+    }
+
+    /* ---------- check trajectory ---------- */
+    constexpr double time_step = 0.01;
+    double t_cur = (ros::Time::now() - info->start_time_).toSec();
+    Eigen::Vector3d p_cur = info->position_traj_.evaluateDeBoorT(t_cur);
+    const double CLEARANCE = 1.0 * planner_manager_->getSwarmClearance();
+    double t_cur_global = ros::Time::now().toSec();
+    double t_2_3 = info->duration_ * 2 / 3;
+    for (double t = t_cur; t < info->duration_; t += time_step)
+    {
+      if (t_cur < t_2_3 && t >= t_2_3) // If t_cur < t_2_3, only the first 2/3 partition of the trajectory is considered valid and will get checked.
+        break;
+
+      bool occ = false;
+      occ |= map->getInflateOccupancy(info->position_traj_.evaluateDeBoorT(t));
+
+      for (size_t id = 0; id < planner_manager_->swarm_trajs_buf_.size(); id++)
+      {
+        if ((planner_manager_->swarm_trajs_buf_.at(id).drone_id != (int)id) || (planner_manager_->swarm_trajs_buf_.at(id).drone_id == planner_manager_->pp_.drone_id))
+        {
+          continue;
+        }
+
+        double t_X = t_cur_global - planner_manager_->swarm_trajs_buf_.at(id).start_time_.toSec();
+        Eigen::Vector3d swarm_pridicted = planner_manager_->swarm_trajs_buf_.at(id).position_traj_.evaluateDeBoorT(t_X);
+        double dist = (p_cur - swarm_pridicted).norm();
+
+        if (dist < CLEARANCE)
+        {
+          occ = true;
+          break;
+        }
+      }
+
+      if (occ)
+      {
+
+        if (planFromCurrentTraj()) // Make a chance
+        {
+          changeFSMExecState(EXEC_TRAJ, "SAFETY");
+          publishSwarmTrajs(false);
+          return;
+        }
+        else
+        {
+          if (t - t_cur < emergency_time_) // 0.8s of emergency time
+          {
+            ROS_WARN("Suddenly discovered obstacles. emergency stop! time=%f", t - t_cur);
+            changeFSMExecState(EMERGENCY_STOP, "SAFETY");
+          }
+          else
+          {
+            //ROS_WARN("current traj in collision, replan.");
+            changeFSMExecState(REPLAN_TRAJ, "SAFETY");
+          }
+          return;
+        }
+        break;
+      }
+    }
+  }
+
+  bool EGOReplanFSM::callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj)
+  {
+
+    getLocalTarget();
+
+    bool plan_and_refine_success =
+        planner_manager_->reboundReplan(start_pt_, start_vel_, start_acc_, local_target_pt_, local_target_vel_, (have_new_target_ || flag_use_poly_init), flag_randomPolyTraj);
+    have_new_target_ = false;
+
+    cout << "refine_success=" << plan_and_refine_success << endl;
+
+    if (plan_and_refine_success)
+    {
+
+      auto info = &planner_manager_->local_data_;
+
+      traj_utils::Bspline bspline;
+      bspline.order = 3;
+      bspline.start_time = info->start_time_;
+      bspline.traj_id = info->traj_id_;
+
+      Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
+      bspline.pos_pts.reserve(pos_pts.cols());
+      for (int i = 0; i < pos_pts.cols(); ++i)
+      {
+        geometry_msgs::Point pt;
+        pt.x = pos_pts(0, i);
+        pt.y = pos_pts(1, i);
+        pt.z = pos_pts(2, i);
+        bspline.pos_pts.push_back(pt);
+      }
+
+      Eigen::VectorXd knots = info->position_traj_.getKnot();
+      // cout << knots.transpose() << endl;
+      bspline.knots.reserve(knots.rows());
+      for (int i = 0; i < knots.rows(); ++i)
+      {
+        bspline.knots.push_back(knots(i));
+      }
+
+      /* 1. publish traj to traj_server */
+      bspline_pub_.publish(bspline);
+
+      /* 2. publish traj to the next drone of swarm */
+
+      /* 3. publish traj for visualization */
+      visualization_->displayOptimalList(info->position_traj_.get_control_points(), 0);
+    }
+
+    return plan_and_refine_success;
+  }
+
+  void EGOReplanFSM::publishSwarmTrajs(bool startup_pub)
+  {
+    auto info = &planner_manager_->local_data_;
+
+    traj_utils::Bspline bspline;
+    bspline.order = 3;
+    bspline.start_time = info->start_time_;
+    bspline.drone_id = planner_manager_->pp_.drone_id;
+    bspline.traj_id = info->traj_id_;
+
+    Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
+    bspline.pos_pts.reserve(pos_pts.cols());
+    for (int i = 0; i < pos_pts.cols(); ++i)
+    {
+      geometry_msgs::Point pt;
+      pt.x = pos_pts(0, i);
+      pt.y = pos_pts(1, i);
+      pt.z = pos_pts(2, i);
+      bspline.pos_pts.push_back(pt);
+    }
+
+    Eigen::VectorXd knots = info->position_traj_.getKnot();
+    // cout << knots.transpose() << endl;
+    bspline.knots.reserve(knots.rows());
+    for (int i = 0; i < knots.rows(); ++i)
+    {
+      bspline.knots.push_back(knots(i));
+    }
+
+    if (startup_pub)
+    {
+      multi_bspline_msgs_buf_.drone_id_from = planner_manager_->pp_.drone_id; // zx-todo
+      if ((int)multi_bspline_msgs_buf_.traj.size() == planner_manager_->pp_.drone_id + 1)
+      {
+        multi_bspline_msgs_buf_.traj.back() = bspline;
+      }
+      else if ((int)multi_bspline_msgs_buf_.traj.size() == planner_manager_->pp_.drone_id)
+      {
+        multi_bspline_msgs_buf_.traj.push_back(bspline);
+      }
+      else
+      {
+        ROS_ERROR("Wrong traj nums and drone_id pair!!! traj.size()=%d, drone_id=%d", multi_bspline_msgs_buf_.traj.size(), planner_manager_->pp_.drone_id);
+        // return plan_and_refine_success;
+      }
+      swarm_trajs_pub_.publish(multi_bspline_msgs_buf_);
+    }
+
+    broadcast_bspline_pub_.publish(bspline);
+  }
+
+  bool EGOReplanFSM::callEmergencyStop(Eigen::Vector3d stop_pos)
+  {
+
+    planner_manager_->EmergencyStop(stop_pos);
+
+    auto info = &planner_manager_->local_data_;
+
+    /* publish traj */
+    traj_utils::Bspline bspline;
+    bspline.order = 3;
+    bspline.start_time = info->start_time_;
+    bspline.traj_id = info->traj_id_;
+
+    Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
+    bspline.pos_pts.reserve(pos_pts.cols());
+    for (int i = 0; i < pos_pts.cols(); ++i)
+    {
+      geometry_msgs::Point pt;
+      pt.x = pos_pts(0, i);
+      pt.y = pos_pts(1, i);
+      pt.z = pos_pts(2, i);
+      bspline.pos_pts.push_back(pt);
+    }
+
+    Eigen::VectorXd knots = info->position_traj_.getKnot();
+    bspline.knots.reserve(knots.rows());
+    for (int i = 0; i < knots.rows(); ++i)
+    {
+      bspline.knots.push_back(knots(i));
+    }
+
+    bspline_pub_.publish(bspline);
+
+    return true;
+  }
+
+  void EGOReplanFSM::getLocalTarget()
+  {
+    double t;
+
+    double t_step = planning_horizen_ / 20 / planner_manager_->pp_.max_vel_;
+    double dist_min = 9999, dist_min_t = 0.0;
+    for (t = planner_manager_->global_data_.last_progress_time_; t < planner_manager_->global_data_.global_duration_; t += t_step)
+    {
+      Eigen::Vector3d pos_t = planner_manager_->global_data_.getPosition(t);
+      double dist = (pos_t - start_pt_).norm();
+
+      // if (t < planner_manager_->global_data_.last_progress_time_ + 1e-5 && dist > planning_horizen_)
+      // {
+      //   // todo
+      //   ROS_ERROR("last_progress_time_ ERROR, TODO!");
+      //   ROS_ERROR("last_progress_time_ ERROR, TODO!");
+      //   ROS_ERROR("last_progress_time_ ERROR, TODO!");
+      //   ROS_ERROR("last_progress_time_ ERROR, TODO!");
+      //   ROS_ERROR("last_progress_time_ ERROR, TODO!");
+      //   cout << "dist=" << dist << endl;
+      //   cout << "planner_manager_->global_data_.last_progress_time_=" << planner_manager_->global_data_.last_progress_time_ << endl;
+      //   return;
+      // }
+      if (dist < dist_min)
+      {
+        dist_min = dist;
+        dist_min_t = t;
+      }
+      if (dist >= planning_horizen_)
+      {
+        local_target_pt_ = pos_t;
+        planner_manager_->global_data_.last_progress_time_ = dist_min_t;
+        break;
+      }
+    }
+    if (t > planner_manager_->global_data_.global_duration_) // Last global point
+    {
+      // planner_manager_->grid_map_;
+      local_target_pt_ = end_pt_;
+    }
+
+    if ((end_pt_ - local_target_pt_).norm() < (planner_manager_->pp_.max_vel_ * planner_manager_->pp_.max_vel_) / (2 * planner_manager_->pp_.max_acc_))
+    {
+      // local_target_vel_ = (end_pt_ - init_pt_).normalized() * planner_manager_->pp_.max_vel_ * (( end_pt_ - local_target_pt_ ).norm() / ((planner_manager_->pp_.max_vel_*planner_manager_->pp_.max_vel_)/(2*planner_manager_->pp_.max_acc_)));
+      // cout << "A" << endl;
+      local_target_vel_ = Eigen::Vector3d::Zero();
+    }
+    else
+    {
+      local_target_vel_ = planner_manager_->global_data_.getVelocity(t);
+      // cout << "AA" << endl;
+    }
+  }
+
+} // namespace ego_planner

motion_planning/3d/ego_planner/planner/plan_manage/src/planner_manager.cpp

@@ -0,0 +1,567 @@
+// #include <fstream>
+#include <plan_manage/planner_manager.h>
+#include <thread>
+#include "visualization_msgs/Marker.h" // zx-todo
+
+namespace ego_planner
+{
+
+  // SECTION interfaces for setup and query
+
+  EGOPlannerManager::EGOPlannerManager() {}
+
+  EGOPlannerManager::~EGOPlannerManager() { }
+
+  void EGOPlannerManager::initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis)
+  {
+    /* read algorithm parameters */
+
+    nh.param("manager/max_vel", pp_.max_vel_, -1.0);
+    nh.param("manager/max_acc", pp_.max_acc_, -1.0);
+    nh.param("manager/max_jerk", pp_.max_jerk_, -1.0);
+    nh.param("manager/feasibility_tolerance", pp_.feasibility_tolerance_, 0.0);
+    nh.param("manager/control_points_distance", pp_.ctrl_pt_dist, -1.0);
+    nh.param("manager/planning_horizon", pp_.planning_horizen_, 5.0);
+    nh.param("manager/use_distinctive_trajs", pp_.use_distinctive_trajs, false);
+    nh.param("manager/drone_id", pp_.drone_id, -1);
+
+    local_data_.traj_id_ = 0;
+    grid_map_.reset(new GridMap);
+    grid_map_->initMap(nh);
+
+    // obj_predictor_.reset(new fast_planner::ObjPredictor(nh));
+    // obj_predictor_->init();
+    // obj_pub_ = nh.advertise<visualization_msgs::Marker>("/dynamic/obj_prdi", 10); // zx-todo
+
+    bspline_optimizer_.reset(new BsplineOptimizer);
+    bspline_optimizer_->setParam(nh);
+    bspline_optimizer_->setEnvironment(grid_map_, obj_predictor_);
+    bspline_optimizer_->a_star_.reset(new AStar);
+    bspline_optimizer_->a_star_->initGridMap(grid_map_, Eigen::Vector3i(100, 100, 100));
+
+    visualization_ = vis;
+  }
+
+  // !SECTION
+
+  // SECTION rebond replanning
+
+  bool EGOPlannerManager::reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,
+                                        Eigen::Vector3d start_acc, Eigen::Vector3d local_target_pt,
+                                        Eigen::Vector3d local_target_vel, bool flag_polyInit, bool flag_randomPolyTraj)
+  {
+    static int count = 0;
+    printf("\033[47;30m\n[drone %d replan %d]==============================================\033[0m\n", pp_.drone_id, count++);
+    // cout.precision(3);
+    // cout << "start: " << start_pt.transpose() << ", " << start_vel.transpose() << "\ngoal:" << local_target_pt.transpose() << ", " << local_target_vel.transpose()
+    //      << endl;
+
+    if ((start_pt - local_target_pt).norm() < 0.2)
+    {
+      cout << "Close to goal" << endl;
+      continous_failures_count_++;
+      return false;
+    }
+
+    bspline_optimizer_->setLocalTargetPt( local_target_pt );
+
+    ros::Time t_start = ros::Time::now();
+    ros::Duration t_init, t_opt, t_refine;
+
+    /*** STEP 1: INIT ***/
+    double ts = (start_pt - local_target_pt).norm() > 0.1 ? pp_.ctrl_pt_dist / pp_.max_vel_ * 1.5 : pp_.ctrl_pt_dist / pp_.max_vel_ * 5; // pp_.ctrl_pt_dist / pp_.max_vel_ is too tense, and will surely exceed the acc/vel limits
+    vector<Eigen::Vector3d> point_set, start_end_derivatives;
+    static bool flag_first_call = true, flag_force_polynomial = false;
+    bool flag_regenerate = false;
+    do
+    {
+      point_set.clear();
+      start_end_derivatives.clear();
+      flag_regenerate = false;
+
+      if (flag_first_call || flag_polyInit || flag_force_polynomial /*|| ( start_pt - local_target_pt ).norm() < 1.0*/) // Initial path generated from a min-snap traj by order.
+      {
+        flag_first_call = false;
+        flag_force_polynomial = false;
+
+        PolynomialTraj gl_traj;
+
+        double dist = (start_pt - local_target_pt).norm();
+        double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_) : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_;
+
+        if (!flag_randomPolyTraj)
+        {
+          gl_traj = PolynomialTraj::one_segment_traj_gen(start_pt, start_vel, start_acc, local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), time);
+        }
+        else
+        {
+          Eigen::Vector3d horizen_dir = ((start_pt - local_target_pt).cross(Eigen::Vector3d(0, 0, 1))).normalized();
+          Eigen::Vector3d vertical_dir = ((start_pt - local_target_pt).cross(horizen_dir)).normalized();
+          Eigen::Vector3d random_inserted_pt = (start_pt + local_target_pt) / 2 +
+                                               (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * horizen_dir * 0.8 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989) +
+                                               (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * vertical_dir * 0.4 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989);
+          Eigen::MatrixXd pos(3, 3);
+          pos.col(0) = start_pt;
+          pos.col(1) = random_inserted_pt;
+          pos.col(2) = local_target_pt;
+          Eigen::VectorXd t(2);
+          t(0) = t(1) = time / 2;
+          gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, local_target_vel, start_acc, Eigen::Vector3d::Zero(), t);
+        }
+
+        double t;
+        bool flag_too_far;
+        ts *= 1.5; // ts will be divided by 1.5 in the next
+        do
+        {
+          ts /= 1.5;
+          point_set.clear();
+          flag_too_far = false;
+          Eigen::Vector3d last_pt = gl_traj.evaluate(0);
+          for (t = 0; t < time; t += ts)
+          {
+            Eigen::Vector3d pt = gl_traj.evaluate(t);
+            if ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5)
+            {
+              flag_too_far = true;
+              break;
+            }
+            last_pt = pt;
+            point_set.push_back(pt);
+          }
+        } while (flag_too_far || point_set.size() < 7); // To make sure the initial path has enough points.
+        t -= ts;
+        start_end_derivatives.push_back(gl_traj.evaluateVel(0));
+        start_end_derivatives.push_back(local_target_vel);
+        start_end_derivatives.push_back(gl_traj.evaluateAcc(0));
+        start_end_derivatives.push_back(gl_traj.evaluateAcc(t));
+      }
+      else // Initial path generated from previous trajectory.
+      {
+
+        double t;
+        double t_cur = (ros::Time::now() - local_data_.start_time_).toSec();
+
+        vector<double> pseudo_arc_length;
+        vector<Eigen::Vector3d> segment_point;
+        pseudo_arc_length.push_back(0.0);
+        for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts)
+        {
+          segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t));
+          if (t > t_cur)
+          {
+            pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());
+          }
+        }
+        t -= ts;
+
+        double poly_time = (local_data_.position_traj_.evaluateDeBoorT(t) - local_target_pt).norm() / pp_.max_vel_ * 2;
+        if (poly_time > ts)
+        {
+          PolynomialTraj gl_traj = PolynomialTraj::one_segment_traj_gen(local_data_.position_traj_.evaluateDeBoorT(t),
+                                                                        local_data_.velocity_traj_.evaluateDeBoorT(t),
+                                                                        local_data_.acceleration_traj_.evaluateDeBoorT(t),
+                                                                        local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), poly_time);
+
+          for (t = ts; t < poly_time; t += ts)
+          {
+            if (!pseudo_arc_length.empty())
+            {
+              segment_point.push_back(gl_traj.evaluate(t));
+              pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());
+            }
+            else
+            {
+              ROS_ERROR("pseudo_arc_length is empty, return!");
+              continous_failures_count_++;
+              return false;
+            }
+          }
+        }
+
+        double sample_length = 0;
+        double cps_dist = pp_.ctrl_pt_dist * 1.5; // cps_dist will be divided by 1.5 in the next
+        size_t id = 0;
+        do
+        {
+          cps_dist /= 1.5;
+          point_set.clear();
+          sample_length = 0;
+          id = 0;
+          while ((id <= pseudo_arc_length.size() - 2) && sample_length <= pseudo_arc_length.back())
+          {
+            if (sample_length >= pseudo_arc_length[id] && sample_length < pseudo_arc_length[id + 1])
+            {
+              point_set.push_back((sample_length - pseudo_arc_length[id]) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id + 1] +
+                                  (pseudo_arc_length[id + 1] - sample_length) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id]);
+              sample_length += cps_dist;
+            }
+            else
+              id++;
+          }
+          point_set.push_back(local_target_pt);
+        } while (point_set.size() < 7); // If the start point is very close to end point, this will help
+
+        start_end_derivatives.push_back(local_data_.velocity_traj_.evaluateDeBoorT(t_cur));
+        start_end_derivatives.push_back(local_target_vel);
+        start_end_derivatives.push_back(local_data_.acceleration_traj_.evaluateDeBoorT(t_cur));
+        start_end_derivatives.push_back(Eigen::Vector3d::Zero());
+
+        if (point_set.size() > pp_.planning_horizen_ / pp_.ctrl_pt_dist * 3) // The initial path is unnormally too long!
+        {
+          flag_force_polynomial = true;
+          flag_regenerate = true;
+        }
+      }
+    } while (flag_regenerate);
+
+    Eigen::MatrixXd ctrl_pts, ctrl_pts_temp;
+    UniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts);
+
+    vector<std::pair<int, int>> segments;
+    segments = bspline_optimizer_->initControlPoints(ctrl_pts, true);
+
+    t_init = ros::Time::now() - t_start;
+    t_start = ros::Time::now();
+
+    /*** STEP 2: OPTIMIZE ***/
+    bool flag_step_1_success = false;
+    vector<vector<Eigen::Vector3d>> vis_trajs;
+
+    if (pp_.use_distinctive_trajs)
+    {
+      // cout << "enter" << endl;
+      std::vector<ControlPoints> trajs = bspline_optimizer_->distinctiveTrajs(segments);
+      cout << "\033[1;33m"
+           << "multi-trajs=" << trajs.size() << "\033[1;0m" << endl;
+
+      double final_cost, min_cost = 999999.0;
+      for (int i = trajs.size() - 1; i >= 0; i--)
+      {
+        if (bspline_optimizer_->BsplineOptimizeTrajRebound(ctrl_pts_temp, final_cost, trajs[i], ts))
+        {
+
+          cout << "traj " << trajs.size() - i << " success." << endl;
+
+          flag_step_1_success = true;
+          if (final_cost < min_cost)
+          {
+            min_cost = final_cost;
+            ctrl_pts = ctrl_pts_temp;
+          }
+
+          // visualization
+          point_set.clear();
+          for (int j = 0; j < ctrl_pts_temp.cols(); j++)
+          {
+            point_set.push_back(ctrl_pts_temp.col(j));
+          }
+          vis_trajs.push_back(point_set);
+        }
+        else
+        {
+          cout << "traj " << trajs.size() - i << " failed." << endl;
+        }
+      }
+
+      t_opt = ros::Time::now() - t_start;
+
+      visualization_->displayMultiInitPathList(vis_trajs, 0.2); // This visuallization will take up several milliseconds.
+    }
+    else
+    {
+      flag_step_1_success = bspline_optimizer_->BsplineOptimizeTrajRebound(ctrl_pts, ts);
+      t_opt = ros::Time::now() - t_start;
+      //static int vis_id = 0;
+      visualization_->displayInitPathList(point_set, 0.2, 0);
+    }
+
+    cout << "plan_success=" << flag_step_1_success << endl;
+    if (!flag_step_1_success)
+    {
+      visualization_->displayOptimalList(ctrl_pts, 0);
+      continous_failures_count_++;
+      return false;
+    }
+
+    t_start = ros::Time::now();
+
+    UniformBspline pos = UniformBspline(ctrl_pts, 3, ts);
+    pos.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_, pp_.feasibility_tolerance_);
+
+    /*** STEP 3: REFINE(RE-ALLOCATE TIME) IF NECESSARY ***/
+    static bool print_once = true;
+    if ( print_once )
+    {
+      print_once = false;
+      ROS_ERROR("IN SWARM MODE, REFINE DISABLED!");
+    }
+    // disable refine in swarm scenario
+    // double ratio;
+    // bool flag_step_2_success = true;
+    // if (!pos.checkFeasibility(ratio, false))
+    // {
+    //   cout << "Need to reallocate time." << endl;
+
+    //   Eigen::MatrixXd optimal_control_points;
+    //   flag_step_2_success = refineTrajAlgo(pos, start_end_derivatives, ratio, ts, optimal_control_points);
+    //   if (flag_step_2_success)
+    //     pos = UniformBspline(optimal_control_points, 3, ts);
+    // }
+
+    // if (!flag_step_2_success)
+    // {
+    //   printf("\033[34mThis refined trajectory hits obstacles. It doesn't matter if appeares occasionally. But if continously appearing, Increase parameter \"lambda_fitness\".\n\033[0m");
+    //   continous_failures_count_++;
+    //   return false;
+    // }
+
+    t_refine = ros::Time::now() - t_start;
+
+    // save planned results
+    updateTrajInfo(pos, ros::Time::now());
+
+    static double sum_time = 0;
+    static int count_success = 0;
+    sum_time += (t_init + t_opt + t_refine).toSec();
+    count_success++;
+    cout << "total time:\033[42m" << (t_init + t_opt + t_refine).toSec() << "\033[0m,optimize:" << (t_init + t_opt).toSec() << ",refine:" << t_refine.toSec() << ",avg_time=" << sum_time / count_success << endl;
+
+    // success. YoY
+    continous_failures_count_ = 0;
+    return true;
+  }
+
+  bool EGOPlannerManager::EmergencyStop(Eigen::Vector3d stop_pos)
+  {
+    Eigen::MatrixXd control_points(3, 6);
+    for (int i = 0; i < 6; i++)
+    {
+      control_points.col(i) = stop_pos;
+    }
+
+    updateTrajInfo(UniformBspline(control_points, 3, 1.0), ros::Time::now());
+
+    return true;
+  }
+
+  bool EGOPlannerManager::checkCollision(int drone_id)
+  {
+    if ( local_data_.start_time_.toSec() < 1e9 ) // It means my first planning has not started
+      return false;
+    
+    double my_traj_start_time = local_data_.start_time_.toSec();
+    double other_traj_start_time = swarm_trajs_buf_[drone_id].start_time_.toSec();
+
+    double t_start = max( my_traj_start_time, other_traj_start_time );
+    double t_end = min( my_traj_start_time + local_data_.duration_ * 2 / 3, other_traj_start_time + swarm_trajs_buf_[drone_id].duration_ );
+
+    for ( double t=t_start; t<t_end; t+=0.03 )
+    {
+      if ( (local_data_.position_traj_.evaluateDeBoorT(t - my_traj_start_time) - swarm_trajs_buf_[drone_id].position_traj_.evaluateDeBoorT(t - other_traj_start_time)).norm() < bspline_optimizer_->getSwarmClearance() )
+      {
+        return true;
+      }
+    }
+
+    return false;
+  }
+
+  bool EGOPlannerManager::planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
+                                                  const std::vector<Eigen::Vector3d> &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc)
+  {
+
+    // generate global reference trajectory
+
+    vector<Eigen::Vector3d> points;
+    points.push_back(start_pos);
+
+    for (size_t wp_i = 0; wp_i < waypoints.size(); wp_i++)
+    {
+      points.push_back(waypoints[wp_i]);
+    }
+
+    double total_len = 0;
+    total_len += (start_pos - waypoints[0]).norm();
+    for (size_t i = 0; i < waypoints.size() - 1; i++)
+    {
+      total_len += (waypoints[i + 1] - waypoints[i]).norm();
+    }
+
+    // insert intermediate points if too far
+    vector<Eigen::Vector3d> inter_points;
+    double dist_thresh = max(total_len / 8, 4.0);
+
+    for (size_t i = 0; i < points.size() - 1; ++i)
+    {
+      inter_points.push_back(points.at(i));
+      double dist = (points.at(i + 1) - points.at(i)).norm();
+
+      if (dist > dist_thresh)
+      {
+        int id_num = floor(dist / dist_thresh) + 1;
+
+        for (int j = 1; j < id_num; ++j)
+        {
+          Eigen::Vector3d inter_pt =
+              points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num;
+          inter_points.push_back(inter_pt);
+        }
+      }
+    }
+
+    inter_points.push_back(points.back());
+
+    // for ( int i=0; i<inter_points.size(); i++ )
+    // {
+    //   cout << inter_points[i].transpose() << endl;
+    // }
+
+    // write position matrix
+    int pt_num = inter_points.size();
+    Eigen::MatrixXd pos(3, pt_num);
+    for (int i = 0; i < pt_num; ++i)
+      pos.col(i) = inter_points[i];
+
+    Eigen::Vector3d zero(0, 0, 0);
+    Eigen::VectorXd time(pt_num - 1);
+    for (int i = 0; i < pt_num - 1; ++i)
+    {
+      time(i) = (pos.col(i + 1) - pos.col(i)).norm() / (pp_.max_vel_);
+    }
+
+    time(0) *= 2.0;
+    time(time.rows() - 1) *= 2.0;
+
+    PolynomialTraj gl_traj;
+    if (pos.cols() >= 3)
+      gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time);
+    else if (pos.cols() == 2)
+      gl_traj = PolynomialTraj::one_segment_traj_gen(start_pos, start_vel, start_acc, pos.col(1), end_vel, end_acc, time(0));
+    else
+      return false;
+
+    auto time_now = ros::Time::now();
+    global_data_.setGlobalTraj(gl_traj, time_now);
+
+    return true;
+  }
+
+  bool EGOPlannerManager::planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
+                                         const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc)
+  {
+
+    // generate global reference trajectory
+
+    vector<Eigen::Vector3d> points;
+    points.push_back(start_pos);
+    points.push_back(end_pos);
+
+    // insert intermediate points if too far
+    vector<Eigen::Vector3d> inter_points;
+    const double dist_thresh = 4.0;
+
+    for (size_t i = 0; i < points.size() - 1; ++i)
+    {
+      inter_points.push_back(points.at(i));
+      double dist = (points.at(i + 1) - points.at(i)).norm();
+
+      if (dist > dist_thresh)
+      {
+        int id_num = floor(dist / dist_thresh) + 1;
+
+        for (int j = 1; j < id_num; ++j)
+        {
+          Eigen::Vector3d inter_pt =
+              points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num;
+          inter_points.push_back(inter_pt);
+        }
+      }
+    }
+
+    inter_points.push_back(points.back());
+
+    // write position matrix
+    int pt_num = inter_points.size();
+    Eigen::MatrixXd pos(3, pt_num);
+    for (int i = 0; i < pt_num; ++i)
+      pos.col(i) = inter_points[i];
+
+    Eigen::Vector3d zero(0, 0, 0);
+    Eigen::VectorXd time(pt_num - 1);
+    for (int i = 0; i < pt_num - 1; ++i)
+    {
+      time(i) = (pos.col(i + 1) - pos.col(i)).norm() / (pp_.max_vel_);
+    }
+
+    time(0) *= 2.0;
+    time(time.rows() - 1) *= 2.0;
+
+    PolynomialTraj gl_traj;
+    if (pos.cols() >= 3)
+      gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time);
+    else if (pos.cols() == 2)
+      gl_traj = PolynomialTraj::one_segment_traj_gen(start_pos, start_vel, start_acc, end_pos, end_vel, end_acc, time(0));
+    else
+      return false;
+
+    auto time_now = ros::Time::now();
+    global_data_.setGlobalTraj(gl_traj, time_now);
+
+    return true;
+  }
+
+  bool EGOPlannerManager::refineTrajAlgo(UniformBspline &traj, vector<Eigen::Vector3d> &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points)
+  {
+    double t_inc;
+
+    Eigen::MatrixXd ctrl_pts; // = traj.getControlPoint()
+
+    // std::cout << "ratio: " << ratio << std::endl;
+    reparamBspline(traj, start_end_derivative, ratio, ctrl_pts, ts, t_inc);
+
+    traj = UniformBspline(ctrl_pts, 3, ts);
+
+    double t_step = traj.getTimeSum() / (ctrl_pts.cols() - 3);
+    bspline_optimizer_->ref_pts_.clear();
+    for (double t = 0; t < traj.getTimeSum() + 1e-4; t += t_step)
+      bspline_optimizer_->ref_pts_.push_back(traj.evaluateDeBoorT(t));
+
+    bool success = bspline_optimizer_->BsplineOptimizeTrajRefine(ctrl_pts, ts, optimal_control_points);
+
+    return success;
+  }
+
+  void EGOPlannerManager::updateTrajInfo(const UniformBspline &position_traj, const ros::Time time_now)
+  {
+    local_data_.start_time_ = time_now;
+    local_data_.position_traj_ = position_traj;
+    local_data_.velocity_traj_ = local_data_.position_traj_.getDerivative();
+    local_data_.acceleration_traj_ = local_data_.velocity_traj_.getDerivative();
+    local_data_.start_pos_ = local_data_.position_traj_.evaluateDeBoorT(0.0);
+    local_data_.duration_ = local_data_.position_traj_.getTimeSum();
+    local_data_.traj_id_ += 1;
+  }
+
+  void EGOPlannerManager::reparamBspline(UniformBspline &bspline, vector<Eigen::Vector3d> &start_end_derivative, double ratio,
+                                         Eigen::MatrixXd &ctrl_pts, double &dt, double &time_inc)
+  {
+    double time_origin = bspline.getTimeSum();
+    int seg_num = bspline.getControlPoint().cols() - 3;
+    // double length = bspline.getLength(0.1);
+    // int seg_num = ceil(length / pp_.ctrl_pt_dist);
+
+    bspline.lengthenTime(ratio);
+    double duration = bspline.getTimeSum();
+    dt = duration / double(seg_num);
+    time_inc = duration - time_origin;
+
+    vector<Eigen::Vector3d> point_set;
+    for (double time = 0.0; time <= duration + 1e-4; time += dt)
+    {
+      point_set.push_back(bspline.evaluateDeBoorT(time));
+    }
+    UniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts);
+  }
+
+} // namespace ego_planner

motion_planning/3d/ego_planner/planner/plan_manage/src/traj_server.cpp

@@ -0,0 +1,279 @@
+#include "bspline_opt/uniform_bspline.h"
+#include "nav_msgs/Odometry.h"
+#include "traj_utils/Bspline.h"
+#include "quadrotor_msgs/PositionCommand.h"
+#include "geometry_msgs/Pose.h"
+#include "std_msgs/Empty.h"
+#include "visualization_msgs/Marker.h"
+#include <ros/ros.h>
+
+ros::Publisher pos_cmd_pub;
+ros::Publisher pose_cmd_pub;
+
+quadrotor_msgs::PositionCommand cmd;
+geometry_msgs::Pose pose_cmd;
+double pos_gain[3] = {0, 0, 0};
+double vel_gain[3] = {0, 0, 0};
+
+using ego_planner::UniformBspline;
+
+bool receive_traj_ = false;
+vector<UniformBspline> traj_;
+double traj_duration_;
+ros::Time start_time_;
+int traj_id_;
+
+// yaw control
+double last_yaw_, last_yaw_dot_;
+double time_forward_;
+
+void bsplineCallback(traj_utils::BsplineConstPtr msg)
+{
+  // parse pos traj
+
+  Eigen::MatrixXd pos_pts(3, msg->pos_pts.size());
+
+  Eigen::VectorXd knots(msg->knots.size());
+  for (size_t i = 0; i < msg->knots.size(); ++i)
+  {
+    knots(i) = msg->knots[i];
+  }
+
+  for (size_t i = 0; i < msg->pos_pts.size(); ++i)
+  {
+    pos_pts(0, i) = msg->pos_pts[i].x;
+    pos_pts(1, i) = msg->pos_pts[i].y;
+    pos_pts(2, i) = msg->pos_pts[i].z;
+  }
+
+  UniformBspline pos_traj(pos_pts, msg->order, 0.1);
+  pos_traj.setKnot(knots);
+
+  // parse yaw traj
+
+  // Eigen::MatrixXd yaw_pts(msg->yaw_pts.size(), 1);
+  // for (int i = 0; i < msg->yaw_pts.size(); ++i) {
+  //   yaw_pts(i, 0) = msg->yaw_pts[i];
+  // }
+
+  //UniformBspline yaw_traj(yaw_pts, msg->order, msg->yaw_dt);
+
+  start_time_ = msg->start_time;
+  traj_id_ = msg->traj_id;
+
+  traj_.clear();
+  traj_.push_back(pos_traj);
+  traj_.push_back(traj_[0].getDerivative());
+  traj_.push_back(traj_[1].getDerivative());
+
+  traj_duration_ = traj_[0].getTimeSum();
+
+  receive_traj_ = true;
+}
+
+std::pair<double, double> calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last)
+{
+  constexpr double PI = 3.1415926;
+  constexpr double YAW_DOT_MAX_PER_SEC = PI;
+  // constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI;
+  std::pair<double, double> yaw_yawdot(0, 0);
+  double yaw = 0;
+  double yawdot = 0;
+
+  Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos;
+  double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_;
+  double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec();
+  if (yaw_temp - last_yaw_ > PI)
+  {
+    if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change)
+    {
+      yaw = last_yaw_ - max_yaw_change;
+      if (yaw < -PI)
+        yaw += 2 * PI;
+
+      yawdot = -YAW_DOT_MAX_PER_SEC;
+    }
+    else
+    {
+      yaw = yaw_temp;
+      if (yaw - last_yaw_ > PI)
+        yawdot = -YAW_DOT_MAX_PER_SEC;
+      else
+        yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
+    }
+  }
+  else if (yaw_temp - last_yaw_ < -PI)
+  {
+    if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change)
+    {
+      yaw = last_yaw_ + max_yaw_change;
+      if (yaw > PI)
+        yaw -= 2 * PI;
+
+      yawdot = YAW_DOT_MAX_PER_SEC;
+    }
+    else
+    {
+      yaw = yaw_temp;
+      if (yaw - last_yaw_ < -PI)
+        yawdot = YAW_DOT_MAX_PER_SEC;
+      else
+        yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
+    }
+  }
+  else
+  {
+    if (yaw_temp - last_yaw_ < -max_yaw_change)
+    {
+      yaw = last_yaw_ - max_yaw_change;
+      if (yaw < -PI)
+        yaw += 2 * PI;
+
+      yawdot = -YAW_DOT_MAX_PER_SEC;
+    }
+    else if (yaw_temp - last_yaw_ > max_yaw_change)
+    {
+      yaw = last_yaw_ + max_yaw_change;
+      if (yaw > PI)
+        yaw -= 2 * PI;
+
+      yawdot = YAW_DOT_MAX_PER_SEC;
+    }
+    else
+    {
+      yaw = yaw_temp;
+      if (yaw - last_yaw_ > PI)
+        yawdot = -YAW_DOT_MAX_PER_SEC;
+      else if (yaw - last_yaw_ < -PI)
+        yawdot = YAW_DOT_MAX_PER_SEC;
+      else
+        yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
+    }
+  }
+
+  if (fabs(yaw - last_yaw_) <= max_yaw_change)
+    yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPF
+  yawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot;
+  last_yaw_ = yaw;
+  last_yaw_dot_ = yawdot;
+
+  yaw_yawdot.first = yaw;
+  yaw_yawdot.second = yawdot;
+
+  return yaw_yawdot;
+}
+
+void cmdCallback(const ros::TimerEvent &e)
+{
+  /* no publishing before receive traj_ */
+  if (!receive_traj_)
+    return;
+
+  ros::Time time_now = ros::Time::now();
+  double t_cur = (time_now - start_time_).toSec();
+
+  Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()), pos_f;
+  std::pair<double, double> yaw_yawdot(0, 0);
+
+  static ros::Time time_last = ros::Time::now();
+  if (t_cur < traj_duration_ && t_cur >= 0.0)
+  {
+    pos = traj_[0].evaluateDeBoorT(t_cur);
+    vel = traj_[1].evaluateDeBoorT(t_cur);
+    acc = traj_[2].evaluateDeBoorT(t_cur);
+
+    /*** calculate yaw ***/
+    yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last);
+    /*** calculate yaw ***/
+
+    double tf = min(traj_duration_, t_cur + 2.0);
+    pos_f = traj_[0].evaluateDeBoorT(tf);
+  }
+  else if (t_cur >= traj_duration_)
+  {
+    /* hover when finish traj_ */
+    pos = traj_[0].evaluateDeBoorT(traj_duration_);
+    vel.setZero();
+    acc.setZero();
+
+    yaw_yawdot.first = last_yaw_;
+    yaw_yawdot.second = 0;
+
+    pos_f = pos;
+  }
+  else
+  {
+    cout << "[Traj server]: invalid time." << endl;
+  }
+  time_last = time_now;
+
+  cmd.header.stamp = time_now;
+  cmd.header.frame_id = "world";
+  cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY;
+  cmd.trajectory_id = traj_id_;
+
+  cmd.position.x = pos(0);
+  cmd.position.y = pos(1);
+  cmd.position.z = pos(2);
+
+  cmd.velocity.x = vel(0);
+  cmd.velocity.y = vel(1);
+  cmd.velocity.z = vel(2);
+
+  cmd.acceleration.x = acc(0);
+  cmd.acceleration.y = acc(1);
+  cmd.acceleration.z = acc(2);
+
+  cmd.yaw = yaw_yawdot.first;
+  cmd.yaw_dot = yaw_yawdot.second;
+
+  last_yaw_ = cmd.yaw;
+
+  pos_cmd_pub.publish(cmd);
+
+  pose_cmd.position.x = pos(0);
+  pose_cmd.position.y = pos(1);
+  pose_cmd.position.z = pos(2);
+
+  pose_cmd.orientation.x = 0.0; 
+  pose_cmd.orientation.y = 0.0;
+  pose_cmd.orientation.z = sin(yaw_yawdot.first/2);
+  pose_cmd.orientation.w = cos(yaw_yawdot.first/2);
+
+  pose_cmd_pub.publish(pose_cmd);
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "traj_server");
+  // ros::NodeHandle node;
+  ros::NodeHandle nh("~");
+
+  ros::Subscriber bspline_sub = nh.subscribe("planning/bspline", 10, bsplineCallback);
+
+  pos_cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);
+  pose_cmd_pub = nh.advertise<geometry_msgs::Pose>("/pose_cmd", 50);
+
+  ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback);
+
+  /* control parameter */
+  cmd.kx[0] = pos_gain[0];
+  cmd.kx[1] = pos_gain[1];
+  cmd.kx[2] = pos_gain[2];
+
+  cmd.kv[0] = vel_gain[0];
+  cmd.kv[1] = vel_gain[1];
+  cmd.kv[2] = vel_gain[2];
+
+  nh.param("traj_server/time_forward", time_forward_, -1.0);
+  last_yaw_ = 0.0;
+  last_yaw_dot_ = 0.0;
+
+  ros::Duration(1.0).sleep();
+
+  ROS_WARN("[Traj server]: ready.");
+
+  ros::spin();
+
+  return 0;
+}

motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rosmsg_tcp_bridge)
+
+set(CMAKE_BUILD_TYPE "Release")
+ADD_COMPILE_OPTIONS(-std=c++11 )
+ADD_COMPILE_OPTIONS(-std=c++14 )
+set(CMAKE_CXX_FLAGS_RELEASE "-O2 -Wall -g")
+
+find_package(Eigen3 REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  geometry_msgs
+  traj_utils
+)
+
+catkin_package(
+ CATKIN_DEPENDS traj_utils
+)
+
+include_directories(
+  SYSTEM
+  ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+add_executable(bridge_node
+  src/bridge_node.cpp 
+  )
+target_link_libraries(bridge_node 
+  ${catkin_LIBRARIES}
+  )
+
+

motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/launch/bridge.launch

@@ -0,0 +1,18 @@
+<launch>
+
+  <arg name="drone_id" value="1" />
+
+  <!-- <node pkg="rosmsg_tcp_bridge" name="drone_$(arg drone_id)_bridge_node" type="bridge_node" output="screen" launch-prefix="valgrind" > -->
+  <node pkg="rosmsg_tcp_bridge" name="drone_$(arg drone_id)_bridge_node" type="bridge_node" output="screen" >
+    <remap from="position_cmd" to="drone_$(arg drone_id)_planning/pos_cmd"/>
+    <remap from="planning/bspline" to="drone_$(arg drone_id)_planning/bspline"/>
+    <remap from="~my_odom" to="/vins_estimator/imu_propagate"/>
+
+    <param name="next_drone_ip" value="127.0.0.1" type="string"/>
+    <param name="broadcast_ip" value="127.0.0.255" type="string"/>
+    <param name="drone_id" value="$(arg drone_id)"/>
+    <param name="odom_max_freq" value="70"/>
+
+  </node>
+
+</launch>

motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rosmsg_tcp_bridge</name>
+  <version>0.0.0</version>
+  <description>The rosmsg_tcp_bridge package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ego_planner</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>traj_utils</build_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>traj_utils</exec_depend>
+  
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/src/bridge_node.cpp

@@ -0,0 +1,828 @@
+#include <ros/ros.h>
+#include <boost/thread.hpp>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <iostream>
+#include <traj_utils/MultiBsplines.h>
+#include <traj_utils/Bspline.h>
+#include <nav_msgs/Odometry.h>
+#include <std_msgs/Empty.h>
+
+#include <unistd.h>
+#include <arpa/inet.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#define PORT 8080
+#define UDP_PORT 8081
+#define BUF_LEN 1048576    // 1MB
+#define BUF_LEN_SHORT 1024 // 1KB
+
+using namespace std;
+
+int send_sock_, server_fd_, recv_sock_, udp_server_fd_, udp_send_fd_;
+ros::Subscriber swarm_trajs_sub_, other_odoms_sub_, emergency_stop_sub_, one_traj_sub_;
+ros::Publisher swarm_trajs_pub_, other_odoms_pub_, emergency_stop_pub_, one_traj_pub_;
+string tcp_ip_, udp_ip_;
+int drone_id_;
+double odom_broadcast_freq_;
+char send_buf_[BUF_LEN], recv_buf_[BUF_LEN], udp_recv_buf_[BUF_LEN], udp_send_buf_[BUF_LEN];
+struct sockaddr_in addr_udp_send_;
+traj_utils::MultiBsplinesPtr bsplines_msg_;
+nav_msgs::OdometryPtr odom_msg_;
+std_msgs::EmptyPtr stop_msg_;
+traj_utils::BsplinePtr bspline_msg_;
+
+enum MESSAGE_TYPE
+{
+  ODOM = 888,
+  MULTI_TRAJ,
+  ONE_TRAJ,
+  STOP
+} massage_type_;
+
+int connect_to_next_drone(const char *ip, const int port)
+{
+  /* Connect */
+  int sock = 0;
+  struct sockaddr_in serv_addr;
+  if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0)
+  {
+    printf("\n Socket creation error \n");
+    return -1;
+  }
+
+  serv_addr.sin_family = AF_INET;
+  serv_addr.sin_port = htons(port);
+
+  // Convert IPv4 and IPv6 addresses from text to binary form
+  if (inet_pton(AF_INET, ip, &serv_addr.sin_addr) <= 0)
+  {
+    printf("\nInvalid address/ Address not supported \n");
+    return -1;
+  }
+
+  if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
+  {
+    ROS_WARN("Tcp connection to drone_%d Failed", drone_id_+1);
+    return -1;
+  }
+
+  char str[INET_ADDRSTRLEN];
+  ROS_INFO("Connect to %s success!", inet_ntop(AF_INET, &serv_addr.sin_addr, str, sizeof(str)));
+
+  return sock;
+}
+
+int init_broadcast(const char *ip, const int port)
+{
+  int fd;
+
+  if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) <= 0)
+  {
+    ROS_ERROR("[bridge_node]Socket sender creation error!");
+    exit(EXIT_FAILURE);
+  }
+
+  int so_broadcast = 1;
+  if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &so_broadcast, sizeof(so_broadcast)) < 0)
+  {
+    cout << "Error in setting Broadcast option";
+    exit(EXIT_FAILURE);
+  }
+
+  addr_udp_send_.sin_family = AF_INET;
+  addr_udp_send_.sin_port = htons(port);
+
+  if (inet_pton(AF_INET, ip, &addr_udp_send_.sin_addr) <= 0)
+  {
+    printf("\nInvalid address/ Address not supported \n");
+    return -1;
+  }
+
+  return fd;
+}
+
+int wait_connection_from_previous_drone(const int port, int &server_fd, int &new_socket)
+{
+  struct sockaddr_in address;
+  int opt = 1;
+  int addrlen = sizeof(address);
+
+  // Creating socket file descriptor
+  if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0)
+  {
+    perror("socket failed");
+    exit(EXIT_FAILURE);
+  }
+
+  // Forcefully attaching socket to the port
+  if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT,
+                 &opt, sizeof(opt)))
+  {
+    perror("setsockopt");
+    exit(EXIT_FAILURE);
+  }
+  address.sin_family = AF_INET;
+  address.sin_addr.s_addr = INADDR_ANY;
+  address.sin_port = htons(PORT);
+
+  // Forcefully attaching socket to the port
+  if (bind(server_fd, (struct sockaddr *)&address,
+           sizeof(address)) < 0)
+  {
+    perror("bind failed");
+    exit(EXIT_FAILURE);
+  }
+  if (listen(server_fd, 3) < 0)
+  {
+    perror("listen");
+    exit(EXIT_FAILURE);
+  }
+  if ((new_socket = accept(server_fd, (struct sockaddr *)&address,
+                           (socklen_t *)&addrlen)) < 0)
+  {
+    perror("accept");
+    exit(EXIT_FAILURE);
+  }
+
+  char str[INET_ADDRSTRLEN];
+  ROS_INFO( "Receive tcp connection from %s", inet_ntop(AF_INET, &address.sin_addr, str, sizeof(str)) );
+
+  return new_socket;
+}
+
+int udp_bind_to_port(const int port, int &server_fd)
+{
+  struct sockaddr_in address;
+  int opt = 1;
+
+  // Creating socket file descriptor
+  if ((server_fd = socket(AF_INET, SOCK_DGRAM, 0)) == 0)
+  {
+    perror("socket failed");
+    exit(EXIT_FAILURE);
+  }
+
+  // Forcefully attaching socket to the port
+  if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT,
+                 &opt, sizeof(opt)))
+  {
+    perror("setsockopt");
+    exit(EXIT_FAILURE);
+  }
+  address.sin_family = AF_INET;
+  address.sin_addr.s_addr = INADDR_ANY;
+  address.sin_port = htons(port);
+
+  // Forcefully attaching socket to the port
+  if (bind(server_fd, (struct sockaddr *)&address,
+           sizeof(address)) < 0)
+  {
+    perror("bind failed");
+    exit(EXIT_FAILURE);
+  }
+
+  return server_fd;
+}
+
+int serializeMultiBsplines(const traj_utils::MultiBsplinesPtr &msg)
+{
+  char *ptr = send_buf_;
+
+  unsigned long total_len = 0;
+  total_len += sizeof(MESSAGE_TYPE) + sizeof(int32_t) + sizeof(size_t);
+  for (size_t i = 0; i < msg->traj.size(); i++)
+  {
+    total_len += sizeof(int32_t) + sizeof(int32_t) + sizeof(double) + sizeof(int64_t) + sizeof(double);
+    total_len += sizeof(size_t) + msg->traj[i].knots.size() * sizeof(double);
+    total_len += sizeof(size_t) + (3 * msg->traj[i].pos_pts.size()) * sizeof(double);
+    total_len += sizeof(size_t) + msg->traj[i].yaw_pts.size() * sizeof(double);
+  }
+  if (total_len + 1 > BUF_LEN)
+  {
+    ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN");
+    return -1;
+  }
+
+  *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::MULTI_TRAJ;
+  ptr += sizeof(MESSAGE_TYPE);
+
+  *((int32_t *)ptr) = msg->drone_id_from;
+  ptr += sizeof(int32_t);
+  if (ptr - send_buf_ > BUF_LEN)
+  {
+  }
+  *((size_t *)ptr) = msg->traj.size();
+  ptr += sizeof(size_t);
+  for (size_t i = 0; i < msg->traj.size(); i++)
+  {
+    *((int32_t *)ptr) = msg->traj[i].drone_id;
+    ptr += sizeof(int32_t);
+    *((int32_t *)ptr) = msg->traj[i].order;
+    ptr += sizeof(int32_t);
+    *((double *)ptr) = msg->traj[i].start_time.toSec();
+    ptr += sizeof(double);
+    *((int64_t *)ptr) = msg->traj[i].traj_id;
+    ptr += sizeof(int64_t);
+    *((double *)ptr) = msg->traj[i].yaw_dt;
+    ptr += sizeof(double);
+
+    *((size_t *)ptr) = msg->traj[i].knots.size();
+    ptr += sizeof(size_t);
+    for (size_t j = 0; j < msg->traj[i].knots.size(); j++)
+    {
+      *((double *)ptr) = msg->traj[i].knots[j];
+      ptr += sizeof(double);
+    }
+
+    *((size_t *)ptr) = msg->traj[i].pos_pts.size();
+    ptr += sizeof(size_t);
+    for (size_t j = 0; j < msg->traj[i].pos_pts.size(); j++)
+    {
+      *((double *)ptr) = msg->traj[i].pos_pts[j].x;
+      ptr += sizeof(double);
+      *((double *)ptr) = msg->traj[i].pos_pts[j].y;
+      ptr += sizeof(double);
+      *((double *)ptr) = msg->traj[i].pos_pts[j].z;
+      ptr += sizeof(double);
+    }
+
+    *((size_t *)ptr) = msg->traj[i].yaw_pts.size();
+    ptr += sizeof(size_t);
+    for (size_t j = 0; j < msg->traj[i].yaw_pts.size(); j++)
+    {
+      *((double *)ptr) = msg->traj[i].yaw_pts[j];
+      ptr += sizeof(double);
+    }
+  }
+
+  return ptr - send_buf_;
+}
+
+int serializeOdom(const nav_msgs::OdometryPtr &msg)
+{
+  char *ptr = udp_send_buf_;
+
+  unsigned long total_len = 0;
+  total_len = sizeof(size_t) +
+              msg->child_frame_id.length() * sizeof(char) +
+              sizeof(size_t) +
+              msg->header.frame_id.length() * sizeof(char) +
+              sizeof(uint32_t) +
+              sizeof(double) +
+              7 * sizeof(double) +
+              36 * sizeof(double) +
+              6 * sizeof(double) +
+              36 * sizeof(double);
+
+  if (total_len + 1 > BUF_LEN)
+  {
+    ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN");
+    return -1;
+  }
+
+  *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::ODOM;
+  ptr += sizeof(MESSAGE_TYPE);
+
+  // child_frame_id
+  size_t len = msg->child_frame_id.length();
+  *((size_t *)ptr) = len;
+  ptr += sizeof(size_t);
+  memcpy((void *)ptr, (void *)msg->child_frame_id.c_str(), len * sizeof(char));
+  ptr += len * sizeof(char);
+
+  // header
+  len = msg->header.frame_id.length();
+  *((size_t *)ptr) = len;
+  ptr += sizeof(size_t);
+  memcpy((void *)ptr, (void *)msg->header.frame_id.c_str(), len * sizeof(char));
+  ptr += len * sizeof(char);
+  *((uint32_t *)ptr) = msg->header.seq;
+  ptr += sizeof(uint32_t);
+  *((double *)ptr) = msg->header.stamp.toSec();
+  ptr += sizeof(double);
+
+  *((double *)ptr) = msg->pose.pose.position.x;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->pose.pose.position.y;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->pose.pose.position.z;
+  ptr += sizeof(double);
+
+  *((double *)ptr) = msg->pose.pose.orientation.w;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->pose.pose.orientation.x;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->pose.pose.orientation.y;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->pose.pose.orientation.z;
+  ptr += sizeof(double);
+
+  for (size_t j = 0; j < 36; j++)
+  {
+    *((double *)ptr) = msg->pose.covariance[j];
+    ptr += sizeof(double);
+  }
+
+  *((double *)ptr) = msg->twist.twist.linear.x;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->twist.twist.linear.y;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->twist.twist.linear.z;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->twist.twist.angular.x;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->twist.twist.angular.y;
+  ptr += sizeof(double);
+  *((double *)ptr) = msg->twist.twist.angular.z;
+  ptr += sizeof(double);
+
+  for (size_t j = 0; j < 36; j++)
+  {
+    *((double *)ptr) = msg->twist.covariance[j];
+    ptr += sizeof(double);
+  }
+
+  return ptr - udp_send_buf_;
+}
+
+int serializeStop(const std_msgs::EmptyPtr &msg)
+{
+  char *ptr = udp_send_buf_;
+
+  *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::STOP;
+  ptr += sizeof(MESSAGE_TYPE);
+
+  return ptr - udp_send_buf_;
+}
+
+int serializeOneTraj(const traj_utils::BsplinePtr &msg)
+{
+  char *ptr = udp_send_buf_;
+
+  unsigned long total_len = 0;
+  total_len += sizeof(int32_t) + sizeof(int32_t) + sizeof(double) + sizeof(int64_t) + sizeof(double);
+  total_len += sizeof(size_t) + msg->knots.size() * sizeof(double);
+  total_len += sizeof(size_t) + (3 * msg->pos_pts.size()) * sizeof(double);
+  total_len += sizeof(size_t) + msg->yaw_pts.size() * sizeof(double);
+  if (total_len + 1 > BUF_LEN)
+  {
+    ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN (2)");
+    return -1;
+  }
+
+  *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::ONE_TRAJ;
+  ptr += sizeof(MESSAGE_TYPE);
+
+  *((int32_t *)ptr) = msg->drone_id;
+  ptr += sizeof(int32_t);
+  *((int32_t *)ptr) = msg->order;
+  ptr += sizeof(int32_t);
+  *((double *)ptr) = msg->start_time.toSec();
+  ptr += sizeof(double);
+  *((int64_t *)ptr) = msg->traj_id;
+  ptr += sizeof(int64_t);
+  *((double *)ptr) = msg->yaw_dt;
+  ptr += sizeof(double);
+
+  *((size_t *)ptr) = msg->knots.size();
+  ptr += sizeof(size_t);
+  for (size_t j = 0; j < msg->knots.size(); j++)
+  {
+    *((double *)ptr) = msg->knots[j];
+    ptr += sizeof(double);
+  }
+
+  *((size_t *)ptr) = msg->pos_pts.size();
+  ptr += sizeof(size_t);
+  for (size_t j = 0; j < msg->pos_pts.size(); j++)
+  {
+    *((double *)ptr) = msg->pos_pts[j].x;
+    ptr += sizeof(double);
+    *((double *)ptr) = msg->pos_pts[j].y;
+    ptr += sizeof(double);
+    *((double *)ptr) = msg->pos_pts[j].z;
+    ptr += sizeof(double);
+  }
+
+  *((size_t *)ptr) = msg->yaw_pts.size();
+  ptr += sizeof(size_t);
+  for (size_t j = 0; j < msg->yaw_pts.size(); j++)
+  {
+    *((double *)ptr) = msg->yaw_pts[j];
+    ptr += sizeof(double);
+  }
+
+  return ptr - udp_send_buf_;
+}
+
+int deserializeOneTraj(traj_utils::BsplinePtr &msg)
+{
+  char *ptr = udp_recv_buf_;
+
+  ptr += sizeof(MESSAGE_TYPE);
+
+  msg->drone_id = *((int32_t *)ptr);
+  ptr += sizeof(int32_t);
+  msg->order = *((int32_t *)ptr);
+  ptr += sizeof(int32_t);
+  msg->start_time.fromSec(*((double *)ptr));
+  ptr += sizeof(double);
+  msg->traj_id = *((int64_t *)ptr);
+  ptr += sizeof(int64_t);
+  msg->yaw_dt = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->knots.resize(*((size_t *)ptr));
+  ptr += sizeof(size_t);
+  for (size_t j = 0; j < msg->knots.size(); j++)
+  {
+    msg->knots[j] = *((double *)ptr);
+    ptr += sizeof(double);
+  }
+
+  msg->pos_pts.resize(*((size_t *)ptr));
+  ptr += sizeof(size_t);
+  for (size_t j = 0; j < msg->pos_pts.size(); j++)
+  {
+    msg->pos_pts[j].x = *((double *)ptr);
+    ptr += sizeof(double);
+    msg->pos_pts[j].y = *((double *)ptr);
+    ptr += sizeof(double);
+    msg->pos_pts[j].z = *((double *)ptr);
+    ptr += sizeof(double);
+  }
+
+  msg->yaw_pts.resize(*((size_t *)ptr));
+  ptr += sizeof(size_t);
+  for (size_t j = 0; j < msg->yaw_pts.size(); j++)
+  {
+    msg->yaw_pts[j] = *((double *)ptr);
+    ptr += sizeof(double);
+  }
+
+  return ptr - udp_recv_buf_;
+}
+
+int deserializeStop(std_msgs::EmptyPtr &msg)
+{
+  char *ptr = udp_recv_buf_;
+
+  return ptr - udp_recv_buf_;
+}
+
+int deserializeOdom(nav_msgs::OdometryPtr &msg)
+{
+  char *ptr = udp_recv_buf_;
+
+  ptr += sizeof(MESSAGE_TYPE);
+
+  // child_frame_id
+  size_t len = *((size_t *)ptr);
+  ptr += sizeof(size_t);
+  msg->child_frame_id.assign((const char *)ptr, len);
+  ptr += len * sizeof(char);
+
+  // header
+  len = *((size_t *)ptr);
+  ptr += sizeof(size_t);
+  msg->header.frame_id.assign((const char *)ptr, len);
+  ptr += len * sizeof(char);
+  msg->header.seq = *((uint32_t *)ptr);
+  ptr += sizeof(uint32_t);
+  msg->header.stamp.fromSec(*((double *)ptr));
+  ptr += sizeof(double);
+
+  msg->pose.pose.position.x = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->pose.pose.position.y = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->pose.pose.position.z = *((double *)ptr);
+  ptr += sizeof(double);
+
+  msg->pose.pose.orientation.w = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->pose.pose.orientation.x = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->pose.pose.orientation.y = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->pose.pose.orientation.z = *((double *)ptr);
+  ptr += sizeof(double);
+
+  for (size_t j = 0; j < 36; j++)
+  {
+    msg->pose.covariance[j] = *((double *)ptr);
+    ptr += sizeof(double);
+  }
+
+  msg->twist.twist.linear.x = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->twist.twist.linear.y = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->twist.twist.linear.z = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->twist.twist.angular.x = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->twist.twist.angular.y = *((double *)ptr);
+  ptr += sizeof(double);
+  msg->twist.twist.angular.z = *((double *)ptr);
+  ptr += sizeof(double);
+
+  for (size_t j = 0; j < 36; j++)
+  {
+    msg->twist.covariance[j] = *((double *)ptr);
+    ptr += sizeof(double);
+  }
+
+  return ptr - udp_recv_buf_;
+}
+
+int deserializeMultiBsplines(traj_utils::MultiBsplinesPtr &msg)
+{
+  char *ptr = recv_buf_;
+
+  ptr += sizeof(MESSAGE_TYPE);
+
+  msg->drone_id_from = *((int32_t *)ptr);
+  ptr += sizeof(int32_t);
+  msg->traj.resize(*((size_t *)ptr));
+  ptr += sizeof(size_t);
+  for (size_t i = 0; i < msg->traj.size(); i++)
+  {
+    msg->traj[i].drone_id = *((int32_t *)ptr);
+    ptr += sizeof(int32_t);
+    msg->traj[i].order = *((int32_t *)ptr);
+    ptr += sizeof(int32_t);
+    msg->traj[i].start_time.fromSec(*((double *)ptr));
+    ptr += sizeof(double);
+    msg->traj[i].traj_id = *((int64_t *)ptr);
+    ptr += sizeof(int64_t);
+    msg->traj[i].yaw_dt = *((double *)ptr);
+    ptr += sizeof(double);
+
+    msg->traj[i].knots.resize(*((size_t *)ptr));
+    ptr += sizeof(size_t);
+    for (size_t j = 0; j < msg->traj[i].knots.size(); j++)
+    {
+      msg->traj[i].knots[j] = *((double *)ptr);
+      ptr += sizeof(double);
+    }
+
+    msg->traj[i].pos_pts.resize(*((size_t *)ptr));
+    ptr += sizeof(size_t);
+    for (size_t j = 0; j < msg->traj[i].pos_pts.size(); j++)
+    {
+      msg->traj[i].pos_pts[j].x = *((double *)ptr);
+      ptr += sizeof(double);
+      msg->traj[i].pos_pts[j].y = *((double *)ptr);
+      ptr += sizeof(double);
+      msg->traj[i].pos_pts[j].z = *((double *)ptr);
+      ptr += sizeof(double);
+    }
+
+    msg->traj[i].yaw_pts.resize(*((size_t *)ptr));
+    ptr += sizeof(size_t);
+    for (size_t j = 0; j < msg->traj[i].yaw_pts.size(); j++)
+    {
+      msg->traj[i].yaw_pts[j] = *((double *)ptr);
+      ptr += sizeof(double);
+    }
+  }
+
+  return ptr - recv_buf_;
+}
+
+void multitraj_sub_tcp_cb(const traj_utils::MultiBsplinesPtr &msg)
+{
+  int len = serializeMultiBsplines(msg);
+  if (send(send_sock_, send_buf_, len, 0) <= 0)
+  {
+    ROS_ERROR("TCP SEND ERROR!!!");
+  }
+}
+
+void odom_sub_udp_cb(const nav_msgs::OdometryPtr &msg)
+{
+
+  static ros::Time t_last;
+  ros::Time t_now = ros::Time::now();
+  if ((t_now - t_last).toSec() * odom_broadcast_freq_ < 1.0)
+  {
+    return;
+  }
+  t_last = t_now;
+
+  msg->child_frame_id = string("drone_") + std::to_string(drone_id_);
+
+  int len = serializeOdom(msg);
+
+  if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0)
+  {
+    ROS_ERROR("UDP SEND ERROR (1)!!!");
+  }
+}
+
+void emergency_stop_sub_udp_cb(const std_msgs::EmptyPtr &msg)
+{
+
+  int len = serializeStop(msg);
+
+  if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0)
+  {
+    ROS_ERROR("UDP SEND ERROR (2)!!!");
+  }
+}
+
+void one_traj_sub_udp_cb(const traj_utils::BsplinePtr &msg)
+{
+
+  int len = serializeOneTraj(msg);
+
+  if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0)
+  {
+    ROS_ERROR("UDP SEND ERROR (3)!!!");
+  }
+}
+
+void server_fun()
+{
+  int valread;
+
+  // Connect
+  if (wait_connection_from_previous_drone(PORT, server_fd_, recv_sock_) < 0)
+  {
+    ROS_ERROR("[bridge_node]Socket recever creation error!");
+    exit(EXIT_FAILURE);
+  }
+
+  while (true)
+  {
+    valread = read(recv_sock_, recv_buf_, BUF_LEN);
+
+    if ( valread <= 0 )
+    {
+      ROS_ERROR("Received message length <= 0, maybe connection has lost");
+      close(recv_sock_);
+      close(server_fd_);
+      return;
+    }
+
+    if (valread == deserializeMultiBsplines(bsplines_msg_))
+    {
+      swarm_trajs_pub_.publish(*bsplines_msg_);
+    }
+    else
+    {
+      ROS_ERROR("Received message length not matches the sent one!!!");
+      continue;
+    }
+  }
+}
+
+void udp_recv_fun()
+{
+  int valread;
+  struct sockaddr_in addr_client;
+  socklen_t addr_len;
+
+  // Connect
+  if (udp_bind_to_port(UDP_PORT, udp_server_fd_) < 0)
+  {
+    ROS_ERROR("[bridge_node]Socket recever creation error!");
+    exit(EXIT_FAILURE);
+  }
+
+  while (true)
+  {
+    if ((valread = recvfrom(udp_server_fd_, udp_recv_buf_, BUF_LEN, 0, (struct sockaddr *)&addr_client, (socklen_t *)&addr_len)) < 0)
+    {
+      perror("recvfrom error:");
+      exit(EXIT_FAILURE);
+    }
+
+    char *ptr = udp_recv_buf_;
+    switch (*((MESSAGE_TYPE *)ptr))
+    {
+    case MESSAGE_TYPE::STOP:
+    {
+
+      if (valread == sizeof(std_msgs::Empty))
+      {
+        if (valread == deserializeStop(stop_msg_))
+        {
+          emergency_stop_pub_.publish(*stop_msg_);
+        }
+        else
+        {
+          ROS_ERROR("Received message length not matches the sent one (1)!!!");
+          continue;
+        }
+      }
+
+      break;
+    }
+
+    case MESSAGE_TYPE::ODOM:
+    {
+      if (valread == deserializeOdom(odom_msg_))
+      {
+        other_odoms_pub_.publish(*odom_msg_);
+      }
+      else
+      {
+        ROS_ERROR("Received message length not matches the sent one (2)!!!");
+        continue;
+      }
+
+      break;
+    }
+
+    case MESSAGE_TYPE::ONE_TRAJ:
+    {
+
+      if ( valread == deserializeOneTraj(bspline_msg_) )
+      {
+        one_traj_pub_.publish(*bspline_msg_);
+      }
+      else
+      {
+        ROS_ERROR("Received message length not matches the sent one (3)!!!");
+        continue;
+      }
+
+      break;
+    }
+
+    default:
+
+      //ROS_ERROR("Unknown received message???");
+
+      break;
+    }
+  }
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "rosmsg_tcp_bridge");
+  ros::NodeHandle nh("~");
+
+  nh.param("next_drone_ip", tcp_ip_, string("127.0.0.1"));
+  nh.param("broadcast_ip", udp_ip_, string("127.0.0.255"));
+  nh.param("drone_id", drone_id_, -1);
+  nh.param("odom_max_freq", odom_broadcast_freq_, 1000.0);
+
+  bsplines_msg_.reset(new traj_utils::MultiBsplines);
+  odom_msg_.reset(new nav_msgs::Odometry);
+  stop_msg_.reset(new std_msgs::Empty);
+  bspline_msg_.reset(new traj_utils::Bspline);
+
+  if (drone_id_ == -1)
+  {
+    ROS_ERROR("Wrong drone_id!");
+    exit(EXIT_FAILURE);
+  }
+
+  string sub_traj_topic_name = string("/drone_") + std::to_string(drone_id_) + string("_planning/swarm_trajs");
+  swarm_trajs_sub_ = nh.subscribe(sub_traj_topic_name.c_str(), 10, multitraj_sub_tcp_cb, ros::TransportHints().tcpNoDelay());
+
+  if ( drone_id_ >= 1 )
+  {
+    string pub_traj_topic_name = string("/drone_") + std::to_string(drone_id_ - 1) + string("_planning/swarm_trajs");
+    swarm_trajs_pub_ = nh.advertise<traj_utils::MultiBsplines>(pub_traj_topic_name.c_str(), 10);
+  }
+
+  other_odoms_sub_ = nh.subscribe("my_odom", 10, odom_sub_udp_cb, ros::TransportHints().tcpNoDelay());
+  other_odoms_pub_ = nh.advertise<nav_msgs::Odometry>("/others_odom", 10);
+
+  //emergency_stop_sub_ = nh.subscribe("emergency_stop_broadcast", 10, emergency_stop_sub_udp_cb, ros::TransportHints().tcpNoDelay());
+  //emergency_stop_pub_ = nh.advertise<std_msgs::Empty>("emergency_stop_recv", 10);
+
+  one_traj_sub_ = nh.subscribe("/broadcast_bspline", 100, one_traj_sub_udp_cb, ros::TransportHints().tcpNoDelay());
+  one_traj_pub_ = nh.advertise<traj_utils::Bspline>("/broadcast_bspline2", 100);
+
+  boost::thread recv_thd(server_fun);
+  recv_thd.detach();
+  ros::Duration(0.1).sleep();
+  boost::thread udp_recv_thd(udp_recv_fun);
+  udp_recv_thd.detach();
+  ros::Duration(0.1).sleep();
+
+  // TCP connect
+  send_sock_ = connect_to_next_drone(tcp_ip_.c_str(), PORT);
+
+  // UDP connect
+  udp_send_fd_ = init_broadcast(udp_ip_.c_str(), UDP_PORT);
+
+  cout << "[rosmsg_tcp_bridge] start running" << endl;
+
+  ros::spin();
+
+  close(send_sock_);
+  close(recv_sock_);
+  close(server_fd_);
+  close(udp_server_fd_);
+  close(udp_send_fd_);
+
+  return 0;
+}

motion_planning/3d/ego_planner/planner/traj_utils/CMakeLists.txt

@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(traj_utils)
+
+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
+  #bspline_opt
+  #path_searching
+  roscpp
+  std_msgs
+  geometry_msgs
+  message_generation
+  #cv_bridge
+)
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+
+# Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  Bspline.msg
+  DataDisp.msg
+  MultiBsplines.msg
+  )
+  
+# Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES traj_utils 
+ CATKIN_DEPENDS message_runtime
+#  DEPENDS system_lib
+)
+
+include_directories( 
+    SYSTEM 
+    include 
+    ${catkin_INCLUDE_DIRS}
+    ${Eigen3_INCLUDE_DIRS} 
+    ${PCL_INCLUDE_DIRS}
+)
+
+link_directories(${PCL_LIBRARY_DIRS})
+
+add_library( traj_utils
+    src/planning_visualization.cpp 
+    src/polynomial_traj.cpp 
+    )
+target_link_libraries( traj_utils
+    ${catkin_LIBRARIES} 
+    )  

motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/plan_container.hpp

@@ -0,0 +1,233 @@
+#ifndef _PLAN_CONTAINER_H_
+#define _PLAN_CONTAINER_H_
+
+#include <Eigen/Eigen>
+#include <vector>
+#include <ros/ros.h>
+
+#include <bspline_opt/uniform_bspline.h>
+#include <traj_utils/polynomial_traj.h>
+
+using std::vector;
+
+namespace ego_planner
+{
+
+  class GlobalTrajData
+  {
+  private:
+  public:
+    PolynomialTraj global_traj_;
+    vector<UniformBspline> local_traj_;
+
+    double global_duration_;
+    ros::Time global_start_time_;
+    double local_start_time_, local_end_time_;
+    double time_increase_;
+    double last_time_inc_;
+    double last_progress_time_;
+
+    GlobalTrajData(/* args */) {}
+
+    ~GlobalTrajData() {}
+
+    bool localTrajReachTarget() { return fabs(local_end_time_ - global_duration_) < 0.1; }
+
+    void setGlobalTraj(const PolynomialTraj &traj, const ros::Time &time)
+    {
+      global_traj_ = traj;
+      global_traj_.init();
+      global_duration_ = global_traj_.getTimeSum();
+      global_start_time_ = time;
+
+      local_traj_.clear();
+      local_start_time_ = -1;
+      local_end_time_ = -1;
+      time_increase_ = 0.0;
+      last_time_inc_ = 0.0;
+      last_progress_time_ = 0.0;
+    }
+
+    void setLocalTraj(UniformBspline traj, double local_ts, double local_te, double time_inc)
+    {
+      local_traj_.resize(3);
+      local_traj_[0] = traj;
+      local_traj_[1] = local_traj_[0].getDerivative();
+      local_traj_[2] = local_traj_[1].getDerivative();
+
+      local_start_time_ = local_ts;
+      local_end_time_ = local_te;
+      global_duration_ += time_inc;
+      time_increase_ += time_inc;
+      last_time_inc_ = time_inc;
+    }
+
+    Eigen::Vector3d getPosition(double t)
+    {
+      if (t >= -1e-3 && t <= local_start_time_)
+      {
+        return global_traj_.evaluate(t - time_increase_ + last_time_inc_);
+      }
+      else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
+      {
+        return global_traj_.evaluate(t - time_increase_);
+      }
+      else
+      {
+        double tm, tmp;
+        local_traj_[0].getTimeSpan(tm, tmp);
+        return local_traj_[0].evaluateDeBoor(tm + t - local_start_time_);
+      }
+    }
+
+    Eigen::Vector3d getVelocity(double t)
+    {
+      if (t >= -1e-3 && t <= local_start_time_)
+      {
+        return global_traj_.evaluateVel(t);
+      }
+      else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
+      {
+        return global_traj_.evaluateVel(t - time_increase_);
+      }
+      else
+      {
+        double tm, tmp;
+        local_traj_[0].getTimeSpan(tm, tmp);
+        return local_traj_[1].evaluateDeBoor(tm + t - local_start_time_);
+      }
+    }
+
+    Eigen::Vector3d getAcceleration(double t)
+    {
+      if (t >= -1e-3 && t <= local_start_time_)
+      {
+        return global_traj_.evaluateAcc(t);
+      }
+      else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
+      {
+        return global_traj_.evaluateAcc(t - time_increase_);
+      }
+      else
+      {
+        double tm, tmp;
+        local_traj_[0].getTimeSpan(tm, tmp);
+        return local_traj_[2].evaluateDeBoor(tm + t - local_start_time_);
+      }
+    }
+
+    // get Bspline paramterization data of a local trajectory within a sphere
+    // start_t: start time of the trajectory
+    // dist_pt: distance between the discretized points
+    void getTrajByRadius(const double &start_t, const double &des_radius, const double &dist_pt,
+                         vector<Eigen::Vector3d> &point_set, vector<Eigen::Vector3d> &start_end_derivative,
+                         double &dt, double &seg_duration)
+    {
+      double seg_length = 0.0; // length of the truncated segment
+      double seg_time = 0.0;   // duration of the truncated segment
+      double radius = 0.0;     // distance to the first point of the segment
+
+      double delt = 0.2;
+      Eigen::Vector3d first_pt = getPosition(start_t); // first point of the segment
+      Eigen::Vector3d prev_pt = first_pt;              // previous point
+      Eigen::Vector3d cur_pt;                          // current point
+
+      // go forward until the traj exceed radius or global time
+
+      while (radius < des_radius && seg_time < global_duration_ - start_t - 1e-3)
+      {
+        seg_time += delt;
+        seg_time = min(seg_time, global_duration_ - start_t);
+
+        cur_pt = getPosition(start_t + seg_time);
+        seg_length += (cur_pt - prev_pt).norm();
+        prev_pt = cur_pt;
+        radius = (cur_pt - first_pt).norm();
+      }
+
+      // get parameterization dt by desired density of points
+      int seg_num = floor(seg_length / dist_pt);
+
+      // get outputs
+
+      seg_duration = seg_time; // duration of the truncated segment
+      dt = seg_time / seg_num; // time difference between two points
+
+      for (double tp = 0.0; tp <= seg_time + 1e-4; tp += dt)
+      {
+        cur_pt = getPosition(start_t + tp);
+        point_set.push_back(cur_pt);
+      }
+
+      start_end_derivative.push_back(getVelocity(start_t));
+      start_end_derivative.push_back(getVelocity(start_t + seg_time));
+      start_end_derivative.push_back(getAcceleration(start_t));
+      start_end_derivative.push_back(getAcceleration(start_t + seg_time));
+    }
+
+    // get Bspline paramterization data of a fixed duration local trajectory
+    // start_t: start time of the trajectory
+    // duration: time length of the segment
+    // seg_num: discretized the segment into *seg_num* parts
+    void getTrajByDuration(double start_t, double duration, int seg_num,
+                           vector<Eigen::Vector3d> &point_set,
+                           vector<Eigen::Vector3d> &start_end_derivative, double &dt)
+    {
+      dt = duration / seg_num;
+      Eigen::Vector3d cur_pt;
+      for (double tp = 0.0; tp <= duration + 1e-4; tp += dt)
+      {
+        cur_pt = getPosition(start_t + tp);
+        point_set.push_back(cur_pt);
+      }
+
+      start_end_derivative.push_back(getVelocity(start_t));
+      start_end_derivative.push_back(getVelocity(start_t + duration));
+      start_end_derivative.push_back(getAcceleration(start_t));
+      start_end_derivative.push_back(getAcceleration(start_t + duration));
+    }
+  };
+
+  struct PlanParameters
+  {
+    /* planning algorithm parameters */
+    double max_vel_, max_acc_, max_jerk_; // physical limits
+    double ctrl_pt_dist;                  // distance between adjacient B-spline control points
+    double feasibility_tolerance_;        // permitted ratio of vel/acc exceeding limits
+    double planning_horizen_;
+    bool use_distinctive_trajs;
+    int drone_id; // single drone: drone_id <= -1, swarm: drone_id >= 0
+
+    /* processing time */
+    double time_search_ = 0.0;
+    double time_optimize_ = 0.0;
+    double time_adjust_ = 0.0;
+  };
+
+  struct LocalTrajData
+  {
+    /* info of generated traj */
+
+    int traj_id_;
+    double duration_;
+    ros::Time start_time_;
+    Eigen::Vector3d start_pos_;
+    UniformBspline position_traj_, velocity_traj_, acceleration_traj_;
+  };
+
+  struct OneTrajDataOfSwarm
+  {
+    /* info of generated traj */
+
+    int drone_id;
+    double duration_;
+    ros::Time start_time_;
+    Eigen::Vector3d start_pos_;
+    UniformBspline position_traj_;
+  };
+
+  typedef std::vector<OneTrajDataOfSwarm> SwarmTrajData;
+
+} // namespace ego_planner
+
+#endif

motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/planning_visualization.h

@@ -0,0 +1,55 @@
+#ifndef _PLANNING_VISUALIZATION_H_
+#define _PLANNING_VISUALIZATION_H_
+
+#include <Eigen/Eigen>
+#include <algorithm>
+//#include <bspline_opt/uniform_bspline.h>
+#include <iostream>
+//#include <bspline_opt/polynomial_traj.h>
+#include <ros/ros.h>
+#include <vector>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <stdlib.h>
+
+using std::vector;
+namespace ego_planner
+{
+  class PlanningVisualization
+  {
+  private:
+    ros::NodeHandle node;
+
+    ros::Publisher goal_point_pub;
+    ros::Publisher global_list_pub;
+    ros::Publisher init_list_pub;
+    ros::Publisher optimal_list_pub;
+    ros::Publisher a_star_list_pub;
+    ros::Publisher guide_vector_pub;
+    ros::Publisher intermediate_state_pub;
+
+  public:
+    PlanningVisualization(/* args */) {}
+    ~PlanningVisualization() {}
+    PlanningVisualization(ros::NodeHandle &nh);
+
+    typedef std::shared_ptr<PlanningVisualization> Ptr;
+
+    void displayMarkerList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale,
+                           Eigen::Vector4d color, int id,  bool show_sphere = true);
+    void generatePathDisplayArray(visualization_msgs::MarkerArray &array,
+                                  const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
+    void generateArrowDisplayArray(visualization_msgs::MarkerArray &array,
+                                   const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
+    void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id);
+    void displayGlobalPathList(vector<Eigen::Vector3d> global_pts, const double scale, int id);
+    void displayInitPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id);
+    void displayMultiInitPathList(vector<vector<Eigen::Vector3d>> init_trajs, const double scale);
+    void displayOptimalList(Eigen::MatrixXd optimal_pts, int id);
+    void displayAStarList(std::vector<std::vector<Eigen::Vector3d>> a_star_paths, int id);
+    void displayArrowList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
+    // void displayIntermediateState(ros::Publisher& intermediate_pub, ego_planner::BsplineOptimizer::Ptr optimizer, double sleep_time, const int start_iteration);
+    // void displayNewArrow(ros::Publisher& guide_vector_pub, ego_planner::BsplineOptimizer::Ptr optimizer);
+  };
+} // namespace ego_planner
+#endif

motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/polynomial_traj.h

@@ -0,0 +1,336 @@
+#ifndef _POLYNOMIAL_TRAJ_H
+#define _POLYNOMIAL_TRAJ_H
+
+#include <Eigen/Eigen>
+#include <vector>
+
+using std::vector;
+
+class PolynomialTraj
+{
+private:
+  vector<double> times;       // time of each segment
+  vector<vector<double>> cxs; // coefficient of x of each segment, from n-1 -> 0
+  vector<vector<double>> cys; // coefficient of y of each segment
+  vector<vector<double>> czs; // coefficient of z of each segment
+
+  double time_sum;
+  int num_seg;
+
+  /* evaluation */
+  vector<Eigen::Vector3d> traj_vec3d;
+  double length;
+
+public:
+  PolynomialTraj(/* args */)
+  {
+  }
+  ~PolynomialTraj()
+  {
+  }
+
+  void reset()
+  {
+    times.clear(), cxs.clear(), cys.clear(), czs.clear();
+    time_sum = 0.0, num_seg = 0;
+  }
+
+  void addSegment(vector<double> cx, vector<double> cy, vector<double> cz, double t)
+  {
+    cxs.push_back(cx), cys.push_back(cy), czs.push_back(cz), times.push_back(t);
+  }
+
+  void init()
+  {
+    num_seg = times.size();
+    time_sum = 0.0;
+    for (int i = 0; i < times.size(); ++i)
+    {
+      time_sum += times[i];
+    }
+  }
+
+  vector<double> getTimes()
+  {
+    return times;
+  }
+
+  vector<vector<double>> getCoef(int axis)
+  {
+    switch (axis)
+    {
+    case 0:
+      return cxs;
+    case 1:
+      return cys;
+    case 2:
+      return czs;
+    default:
+      std::cout << "\033[31mIllegal axis!\033[0m" << std::endl;
+    }
+
+    vector<vector<double>> empty;
+    return empty;
+  }
+
+  Eigen::Vector3d evaluate(double t)
+  {
+    /* detetrmine segment num */
+    int idx = 0;
+    while (times[idx] + 1e-4 < t)
+    {
+      t -= times[idx];
+      ++idx;
+    }
+
+    /* evaluation */
+    int order = cxs[idx].size();
+    Eigen::VectorXd cx(order), cy(order), cz(order), tv(order);
+    for (int i = 0; i < order; ++i)
+    {
+      cx(i) = cxs[idx][i], cy(i) = cys[idx][i], cz(i) = czs[idx][i];
+      tv(order - 1 - i) = std::pow(t, double(i));
+    }
+
+    Eigen::Vector3d pt;
+    pt(0) = tv.dot(cx), pt(1) = tv.dot(cy), pt(2) = tv.dot(cz);
+    return pt;
+  }
+
+  Eigen::Vector3d evaluateVel(double t)
+  {
+    /* detetrmine segment num */
+    int idx = 0;
+    while (times[idx] + 1e-4 < t)
+    {
+      t -= times[idx];
+      ++idx;
+    }
+
+    /* evaluation */
+    int order = cxs[idx].size();
+    Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);
+
+    /* coef of vel */
+    for (int i = 0; i < order - 1; ++i)
+    {
+      vx(i) = double(i + 1) * cxs[idx][order - 2 - i];
+      vy(i) = double(i + 1) * cys[idx][order - 2 - i];
+      vz(i) = double(i + 1) * czs[idx][order - 2 - i];
+    }
+    double ts = t;
+    Eigen::VectorXd tv(order - 1);
+    for (int i = 0; i < order - 1; ++i)
+      tv(i) = pow(ts, i);
+
+    Eigen::Vector3d vel;
+    vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
+    return vel;
+  }
+
+  Eigen::Vector3d evaluateAcc(double t)
+  {
+    /* detetrmine segment num */
+    int idx = 0;
+    while (times[idx] + 1e-4 < t)
+    {
+      t -= times[idx];
+      ++idx;
+    }
+
+    /* evaluation */
+    int order = cxs[idx].size();
+    Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);
+
+    /* coef of vel */
+    for (int i = 0; i < order - 2; ++i)
+    {
+      ax(i) = double((i + 2) * (i + 1)) * cxs[idx][order - 3 - i];
+      ay(i) = double((i + 2) * (i + 1)) * cys[idx][order - 3 - i];
+      az(i) = double((i + 2) * (i + 1)) * czs[idx][order - 3 - i];
+    }
+    double ts = t;
+    Eigen::VectorXd tv(order - 2);
+    for (int i = 0; i < order - 2; ++i)
+      tv(i) = pow(ts, i);
+
+    Eigen::Vector3d acc;
+    acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
+    return acc;
+  }
+
+  /* for evaluating traj, should be called in sequence!!! */
+  double getTimeSum()
+  {
+    return this->time_sum;
+  }
+
+  vector<Eigen::Vector3d> getTraj()
+  {
+    double eval_t = 0.0;
+    traj_vec3d.clear();
+    while (eval_t < time_sum)
+    {
+      Eigen::Vector3d pt = evaluate(eval_t);
+      traj_vec3d.push_back(pt);
+      eval_t += 0.01;
+    }
+    return traj_vec3d;
+  }
+
+  double getLength()
+  {
+    length = 0.0;
+
+    Eigen::Vector3d p_l = traj_vec3d[0], p_n;
+    for (int i = 1; i < traj_vec3d.size(); ++i)
+    {
+      p_n = traj_vec3d[i];
+      length += (p_n - p_l).norm();
+      p_l = p_n;
+    }
+    return length;
+  }
+
+  double getMeanVel()
+  {
+    double mean_vel = length / time_sum;
+  }
+
+  double getAccCost()
+  {
+    double cost = 0.0;
+    int order = cxs[0].size();
+
+    for (int s = 0; s < times.size(); ++s)
+    {
+      Eigen::Vector3d um;
+      um(0) = 2 * cxs[s][order - 3], um(1) = 2 * cys[s][order - 3], um(2) = 2 * czs[s][order - 3];
+      cost += um.squaredNorm() * times[s];
+    }
+
+    return cost;
+  }
+
+  double getJerk()
+  {
+    double jerk = 0.0;
+
+    /* evaluate jerk */
+    for (int s = 0; s < times.size(); ++s)
+    {
+      Eigen::VectorXd cxv(cxs[s].size()), cyv(cys[s].size()), czv(czs[s].size());
+      /* convert coefficient */
+      int order = cxs[s].size();
+      for (int j = 0; j < order; ++j)
+      {
+        cxv(j) = cxs[s][order - 1 - j], cyv(j) = cys[s][order - 1 - j], czv(j) = czs[s][order - 1 - j];
+      }
+      double ts = times[s];
+
+      /* jerk matrix */
+      Eigen::MatrixXd mat_jerk(order, order);
+      mat_jerk.setZero();
+      for (double i = 3; i < order; i += 1)
+        for (double j = 3; j < order; j += 1)
+        {
+          mat_jerk(i, j) =
+              i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) * pow(ts, i + j - 5) / (i + j - 5);
+        }
+
+      jerk += (cxv.transpose() * mat_jerk * cxv)(0, 0);
+      jerk += (cyv.transpose() * mat_jerk * cyv)(0, 0);
+      jerk += (czv.transpose() * mat_jerk * czv)(0, 0);
+    }
+
+    return jerk;
+  }
+
+  void getMeanAndMaxVel(double &mean_v, double &max_v)
+  {
+    int num = 0;
+    mean_v = 0.0, max_v = -1.0;
+    for (int s = 0; s < times.size(); ++s)
+    {
+      int order = cxs[s].size();
+      Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);
+
+      /* coef of vel */
+      for (int i = 0; i < order - 1; ++i)
+      {
+        vx(i) = double(i + 1) * cxs[s][order - 2 - i];
+        vy(i) = double(i + 1) * cys[s][order - 2 - i];
+        vz(i) = double(i + 1) * czs[s][order - 2 - i];
+      }
+      double ts = times[s];
+
+      double eval_t = 0.0;
+      while (eval_t < ts)
+      {
+        Eigen::VectorXd tv(order - 1);
+        for (int i = 0; i < order - 1; ++i)
+          tv(i) = pow(ts, i);
+        Eigen::Vector3d vel;
+        vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
+        double vn = vel.norm();
+        mean_v += vn;
+        if (vn > max_v)
+          max_v = vn;
+        ++num;
+
+        eval_t += 0.01;
+      }
+    }
+
+    mean_v = mean_v / double(num);
+  }
+
+  void getMeanAndMaxAcc(double &mean_a, double &max_a)
+  {
+    int num = 0;
+    mean_a = 0.0, max_a = -1.0;
+    for (int s = 0; s < times.size(); ++s)
+    {
+      int order = cxs[s].size();
+      Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);
+
+      /* coef of acc */
+      for (int i = 0; i < order - 2; ++i)
+      {
+        ax(i) = double((i + 2) * (i + 1)) * cxs[s][order - 3 - i];
+        ay(i) = double((i + 2) * (i + 1)) * cys[s][order - 3 - i];
+        az(i) = double((i + 2) * (i + 1)) * czs[s][order - 3 - i];
+      }
+      double ts = times[s];
+
+      double eval_t = 0.0;
+      while (eval_t < ts)
+      {
+        Eigen::VectorXd tv(order - 2);
+        for (int i = 0; i < order - 2; ++i)
+          tv(i) = pow(ts, i);
+        Eigen::Vector3d acc;
+        acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
+        double an = acc.norm();
+        mean_a += an;
+        if (an > max_a)
+          max_a = an;
+        ++num;
+
+        eval_t += 0.01;
+      }
+    }
+
+    mean_a = mean_a / double(num);
+  }
+
+  static PolynomialTraj minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel,
+                                    const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc,
+                                    const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time);
+
+  static PolynomialTraj one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
+                                             const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
+                                             double t);
+};
+
+#endif

motion_planning/3d/ego_planner/planner/traj_utils/msg/Bspline.msg

@@ -0,0 +1,12 @@
+int32 drone_id
+
+int32 order
+int64 traj_id
+time start_time
+
+float64[] knots
+geometry_msgs/Point[] pos_pts
+
+float64[] yaw_pts
+float64 yaw_dt
+

motion_planning/3d/ego_planner/planner/traj_utils/msg/DataDisp.msg

@@ -0,0 +1,7 @@
+Header header
+
+float64 a
+float64 b
+float64 c
+float64 d
+float64 e

motion_planning/3d/ego_planner/planner/traj_utils/msg/MultiBsplines.msg

@@ -0,0 +1,4 @@
+int32 drone_id_from
+
+Bspline[] traj
+

motion_planning/3d/ego_planner/planner/traj_utils/package.xml

@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>traj_utils</name>
+  <version>0.0.0</version>
+  <description>The traj_utils package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="bzhouai@todo.todo">bzhouai</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/traj_utils</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <!--build_depend>bspline_opt</build_depend-->
+  <!--build_depend>path_searching</build_depend-->
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <build_export_depend>catkin</build_export_depend>
+  <!-- <build_export_depend>bspline_opt</build_export_depend> -->
+  <!--build_export_depend>path_searching</build_export_depend-->
+  <!--build_export_depend>roscpp</build_export_depend-->
+  <build_export_depend>std_msgs</build_export_depend>
+
+  <exec_depend>catkin</exec_depend>
+  <!--exec_depend>bspline_opt</exec_depend-->
+  <!--exec_depend>path_searching</exec_depend-->
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

motion_planning/3d/ego_planner/planner/traj_utils/src/planning_visualization.cpp

@@ -0,0 +1,265 @@
+#include <traj_utils/planning_visualization.h>
+
+using std::cout;
+using std::endl;
+namespace ego_planner
+{
+  PlanningVisualization::PlanningVisualization(ros::NodeHandle &nh)
+  {
+    node = nh;
+
+    goal_point_pub = nh.advertise<visualization_msgs::Marker>("goal_point", 2);
+    global_list_pub = nh.advertise<visualization_msgs::Marker>("global_list", 2);
+    init_list_pub = nh.advertise<visualization_msgs::Marker>("init_list", 2);
+    optimal_list_pub = nh.advertise<visualization_msgs::Marker>("optimal_list", 2);
+    a_star_list_pub = nh.advertise<visualization_msgs::Marker>("a_star_list", 20);
+  }
+
+  // // real ids used: {id, id+1000}
+  void PlanningVisualization::displayMarkerList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale,
+                                                Eigen::Vector4d color, int id, bool show_sphere /* = true */ )
+  {
+    visualization_msgs::Marker sphere, line_strip;
+    sphere.header.frame_id = line_strip.header.frame_id = "world";
+    sphere.header.stamp = line_strip.header.stamp = ros::Time::now();
+    sphere.type = visualization_msgs::Marker::SPHERE_LIST;
+    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
+    sphere.action = line_strip.action = visualization_msgs::Marker::ADD;
+    sphere.id = id;
+    line_strip.id = id + 1000;
+
+    sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
+    sphere.color.r = line_strip.color.r = color(0);
+    sphere.color.g = line_strip.color.g = color(1);
+    sphere.color.b = line_strip.color.b = color(2);
+    sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0;
+    sphere.scale.x = scale;
+    sphere.scale.y = scale;
+    sphere.scale.z = scale;
+    line_strip.scale.x = scale / 2;
+    geometry_msgs::Point pt;
+    for (int i = 0; i < int(list.size()); i++)
+    {
+      pt.x = list[i](0);
+      pt.y = list[i](1);
+      pt.z = list[i](2);
+      //if (show_sphere) sphere.points.push_back(pt);
+      line_strip.points.push_back(pt);
+    }
+    //if (show_sphere) pub.publish(sphere);
+    pub.publish(line_strip);
+  }
+
+  // real ids used: {id, id+1}
+  void PlanningVisualization::generatePathDisplayArray(visualization_msgs::MarkerArray &array,
+                                                       const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
+  {
+    visualization_msgs::Marker sphere, line_strip;
+    sphere.header.frame_id = line_strip.header.frame_id = "map";
+    sphere.header.stamp = line_strip.header.stamp = ros::Time::now();
+    sphere.type = visualization_msgs::Marker::SPHERE_LIST;
+    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
+    sphere.action = line_strip.action = visualization_msgs::Marker::ADD;
+    sphere.id = id;
+    line_strip.id = id + 1;
+
+    sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
+    sphere.color.r = line_strip.color.r = color(0);
+    sphere.color.g = line_strip.color.g = color(1);
+    sphere.color.b = line_strip.color.b = color(2);
+    sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0;
+    sphere.scale.x = scale;
+    sphere.scale.y = scale;
+    sphere.scale.z = scale;
+    line_strip.scale.x = scale / 3;
+    geometry_msgs::Point pt;
+    for (int i = 0; i < int(list.size()); i++)
+    {
+      pt.x = list[i](0);
+      pt.y = list[i](1);
+      pt.z = list[i](2);
+      sphere.points.push_back(pt);
+      line_strip.points.push_back(pt);
+    }
+    array.markers.push_back(sphere);
+    array.markers.push_back(line_strip);
+  }
+
+  // real ids used: {1000*id ~ (arrow nums)+1000*id}
+  void PlanningVisualization::generateArrowDisplayArray(visualization_msgs::MarkerArray &array,
+                                                        const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
+  {
+    visualization_msgs::Marker arrow;
+    arrow.header.frame_id = "map";
+    arrow.header.stamp = ros::Time::now();
+    arrow.type = visualization_msgs::Marker::ARROW;
+    arrow.action = visualization_msgs::Marker::ADD;
+
+    // geometry_msgs::Point start, end;
+    // arrow.points
+
+    arrow.color.r = color(0);
+    arrow.color.g = color(1);
+    arrow.color.b = color(2);
+    arrow.color.a = color(3) > 1e-5 ? color(3) : 1.0;
+    arrow.scale.x = scale;
+    arrow.scale.y = 2 * scale;
+    arrow.scale.z = 2 * scale;
+
+    geometry_msgs::Point start, end;
+    for (int i = 0; i < int(list.size() / 2); i++)
+    {
+      // arrow.color.r = color(0) / (1+i);
+      // arrow.color.g = color(1) / (1+i);
+      // arrow.color.b = color(2) / (1+i);
+
+      start.x = list[2 * i](0);
+      start.y = list[2 * i](1);
+      start.z = list[2 * i](2);
+      end.x = list[2 * i + 1](0);
+      end.y = list[2 * i + 1](1);
+      end.z = list[2 * i + 1](2);
+      arrow.points.clear();
+      arrow.points.push_back(start);
+      arrow.points.push_back(end);
+      arrow.id = i + id * 1000;
+
+      array.markers.push_back(arrow);
+    }
+  }
+
+  void PlanningVisualization::displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id)
+  {
+    visualization_msgs::Marker sphere;
+    sphere.header.frame_id = "world";
+    sphere.header.stamp = ros::Time::now();
+    sphere.type = visualization_msgs::Marker::SPHERE;
+    sphere.action = visualization_msgs::Marker::ADD;
+    sphere.id = id;
+
+    sphere.pose.orientation.w = 1.0;
+    sphere.color.r = color(0);
+    sphere.color.g = color(1);
+    sphere.color.b = color(2);
+    sphere.color.a = color(3);
+    sphere.scale.x = scale;
+    sphere.scale.y = scale;
+    sphere.scale.z = scale;
+    sphere.pose.position.x = goal_point(0);
+    sphere.pose.position.y = goal_point(1);
+    sphere.pose.position.z = goal_point(2);
+
+    goal_point_pub.publish(sphere);
+  }
+
+  void PlanningVisualization::displayGlobalPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id)
+  {
+
+    if (global_list_pub.getNumSubscribers() == 0)
+    {
+      return;
+    }
+
+    Eigen::Vector4d color(0, 0.5, 0.5, 1);
+    displayMarkerList(global_list_pub, init_pts, scale, color, id);
+  }
+
+  void PlanningVisualization::displayMultiInitPathList(vector<vector<Eigen::Vector3d>> init_trajs, const double scale)
+  {
+
+    if (init_list_pub.getNumSubscribers() == 0)
+    {
+      return;
+    }
+
+    static int last_nums = 0;
+
+    for ( int id=0; id<last_nums; id++ )
+    {
+      Eigen::Vector4d color(0, 0, 0, 0);
+      vector<Eigen::Vector3d> blank;
+      displayMarkerList(init_list_pub, blank, scale, color, id, false);
+      ros::Duration(0.001).sleep();
+    }
+    last_nums = 0;
+
+    for ( int id=0; id<init_trajs.size(); id++ )
+    {
+      Eigen::Vector4d color(0, 0, 1, 0.7);
+      displayMarkerList(init_list_pub, init_trajs[id], scale, color, id, false);
+      ros::Duration(0.001).sleep();
+      last_nums++;
+    }
+
+  }
+
+  void PlanningVisualization::displayInitPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id)
+  {
+
+    if (init_list_pub.getNumSubscribers() == 0)
+    {
+      return;
+    }
+
+    Eigen::Vector4d color(0, 0, 1, 1);
+    displayMarkerList(init_list_pub, init_pts, scale, color, id);
+  }
+
+  void PlanningVisualization::displayOptimalList(Eigen::MatrixXd optimal_pts, int id)
+  {
+
+    if (optimal_list_pub.getNumSubscribers() == 0)
+    {
+      return;
+    }
+
+    vector<Eigen::Vector3d> list;
+    for (int i = 0; i < optimal_pts.cols(); i++)
+    {
+      Eigen::Vector3d pt = optimal_pts.col(i).transpose();
+      list.push_back(pt);
+    }
+    Eigen::Vector4d color(1, 0, 0, 1);
+    displayMarkerList(optimal_list_pub, list, 0.15, color, id);
+  }
+
+  void PlanningVisualization::displayAStarList(std::vector<std::vector<Eigen::Vector3d>> a_star_paths, int id /* = Eigen::Vector4d(0.5,0.5,0,1)*/)
+  {
+
+    if (a_star_list_pub.getNumSubscribers() == 0)
+    {
+      return;
+    }
+
+    int i = 0;
+    vector<Eigen::Vector3d> list;
+
+    Eigen::Vector4d color = Eigen::Vector4d(0.5 + ((double)rand() / RAND_MAX / 2), 0.5 + ((double)rand() / RAND_MAX / 2), 0, 1); // make the A star pathes different every time.
+    double scale = 0.05 + (double)rand() / RAND_MAX / 10;
+
+    for (auto block : a_star_paths)
+    {
+      list.clear();
+      for (auto pt : block)
+      {
+        list.push_back(pt);
+      }
+      //Eigen::Vector4d color(0.5,0.5,0,1);
+      displayMarkerList(a_star_list_pub, list, scale, color, id + i); // real ids used: [ id ~ id+a_star_paths.size() ]
+      i++;
+    }
+  }
+
+  void PlanningVisualization::displayArrowList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
+  {
+    visualization_msgs::MarkerArray array;
+    // clear
+    pub.publish(array);
+
+    generateArrowDisplayArray(array, list, scale, color, id);
+
+    pub.publish(array);
+  }
+
+  // PlanningVisualization::
+} // namespace ego_planner

motion_planning/3d/ego_planner/planner/traj_utils/src/polynomial_traj.cpp

@@ -0,0 +1,224 @@
+#include <iostream>
+#include <traj_utils/polynomial_traj.h>
+
+PolynomialTraj PolynomialTraj::minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel,
+                                           const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc,
+                                           const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time)
+{
+  int seg_num = Time.size();
+  Eigen::MatrixXd poly_coeff(seg_num, 3 * 6);
+  Eigen::VectorXd Px(6 * seg_num), Py(6 * seg_num), Pz(6 * seg_num);
+
+  int num_f, num_p; // number of fixed and free variables
+  int num_d;        // number of all segments' derivatives
+
+  const static auto Factorial = [](int x) {
+    int fac = 1;
+    for (int i = x; i > 0; i--)
+      fac = fac * i;
+    return fac;
+  };
+
+  /* ---------- end point derivative ---------- */
+  Eigen::VectorXd Dx = Eigen::VectorXd::Zero(seg_num * 6);
+  Eigen::VectorXd Dy = Eigen::VectorXd::Zero(seg_num * 6);
+  Eigen::VectorXd Dz = Eigen::VectorXd::Zero(seg_num * 6);
+
+  for (int k = 0; k < seg_num; k++)
+  {
+    /* position to derivative */
+    Dx(k * 6) = Pos(0, k);
+    Dx(k * 6 + 1) = Pos(0, k + 1);
+    Dy(k * 6) = Pos(1, k);
+    Dy(k * 6 + 1) = Pos(1, k + 1);
+    Dz(k * 6) = Pos(2, k);
+    Dz(k * 6 + 1) = Pos(2, k + 1);
+
+    if (k == 0)
+    {
+      Dx(k * 6 + 2) = start_vel(0);
+      Dy(k * 6 + 2) = start_vel(1);
+      Dz(k * 6 + 2) = start_vel(2);
+
+      Dx(k * 6 + 4) = start_acc(0);
+      Dy(k * 6 + 4) = start_acc(1);
+      Dz(k * 6 + 4) = start_acc(2);
+    }
+    else if (k == seg_num - 1)
+    {
+      Dx(k * 6 + 3) = end_vel(0);
+      Dy(k * 6 + 3) = end_vel(1);
+      Dz(k * 6 + 3) = end_vel(2);
+
+      Dx(k * 6 + 5) = end_acc(0);
+      Dy(k * 6 + 5) = end_acc(1);
+      Dz(k * 6 + 5) = end_acc(2);
+    }
+  }
+
+  /* ---------- Mapping Matrix A ---------- */
+  Eigen::MatrixXd Ab;
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
+
+  for (int k = 0; k < seg_num; k++)
+  {
+    Ab = Eigen::MatrixXd::Zero(6, 6);
+    for (int i = 0; i < 3; i++)
+    {
+      Ab(2 * i, i) = Factorial(i);
+      for (int j = i; j < 6; j++)
+        Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i);
+    }
+    A.block(k * 6, k * 6, 6, 6) = Ab;
+  }
+
+  /* ---------- Produce Selection Matrix C' ---------- */
+  Eigen::MatrixXd Ct, C;
+
+  num_f = 2 * seg_num + 4; // 3 + 3 + (seg_num - 1) * 2 = 2m + 4
+  num_p = 2 * seg_num - 2; //(seg_num - 1) * 2 = 2m - 2
+  num_d = 6 * seg_num;
+  Ct = Eigen::MatrixXd::Zero(num_d, num_f + num_p);
+  Ct(0, 0) = 1;
+  Ct(2, 1) = 1;
+  Ct(4, 2) = 1; // stack the start point
+  Ct(1, 3) = 1;
+  Ct(3, 2 * seg_num + 4) = 1;
+  Ct(5, 2 * seg_num + 5) = 1;
+
+  Ct(6 * (seg_num - 1) + 0, 2 * seg_num + 0) = 1;
+  Ct(6 * (seg_num - 1) + 1, 2 * seg_num + 1) = 1; // Stack the end point
+  Ct(6 * (seg_num - 1) + 2, 4 * seg_num + 0) = 1;
+  Ct(6 * (seg_num - 1) + 3, 2 * seg_num + 2) = 1; // Stack the end point
+  Ct(6 * (seg_num - 1) + 4, 4 * seg_num + 1) = 1;
+  Ct(6 * (seg_num - 1) + 5, 2 * seg_num + 3) = 1; // Stack the end point
+
+  for (int j = 2; j < seg_num; j++)
+  {
+    Ct(6 * (j - 1) + 0, 2 + 2 * (j - 1) + 0) = 1;
+    Ct(6 * (j - 1) + 1, 2 + 2 * (j - 1) + 1) = 1;
+    Ct(6 * (j - 1) + 2, 2 * seg_num + 4 + 2 * (j - 2) + 0) = 1;
+    Ct(6 * (j - 1) + 3, 2 * seg_num + 4 + 2 * (j - 1) + 0) = 1;
+    Ct(6 * (j - 1) + 4, 2 * seg_num + 4 + 2 * (j - 2) + 1) = 1;
+    Ct(6 * (j - 1) + 5, 2 * seg_num + 4 + 2 * (j - 1) + 1) = 1;
+  }
+
+  C = Ct.transpose();
+
+  Eigen::VectorXd Dx1 = C * Dx;
+  Eigen::VectorXd Dy1 = C * Dy;
+  Eigen::VectorXd Dz1 = C * Dz;
+
+  /* ---------- minimum snap matrix ---------- */
+  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
+
+  for (int k = 0; k < seg_num; k++)
+  {
+    for (int i = 3; i < 6; i++)
+    {
+      for (int j = 3; j < 6; j++)
+      {
+        Q(k * 6 + i, k * 6 + j) =
+            i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5));
+      }
+    }
+  }
+
+  /* ---------- R matrix ---------- */
+  Eigen::MatrixXd R = C * A.transpose().inverse() * Q * A.inverse() * Ct;
+
+  Eigen::VectorXd Dxf(2 * seg_num + 4), Dyf(2 * seg_num + 4), Dzf(2 * seg_num + 4);
+
+  Dxf = Dx1.segment(0, 2 * seg_num + 4);
+  Dyf = Dy1.segment(0, 2 * seg_num + 4);
+  Dzf = Dz1.segment(0, 2 * seg_num + 4);
+
+  Eigen::MatrixXd Rff(2 * seg_num + 4, 2 * seg_num + 4);
+  Eigen::MatrixXd Rfp(2 * seg_num + 4, 2 * seg_num - 2);
+  Eigen::MatrixXd Rpf(2 * seg_num - 2, 2 * seg_num + 4);
+  Eigen::MatrixXd Rpp(2 * seg_num - 2, 2 * seg_num - 2);
+
+  Rff = R.block(0, 0, 2 * seg_num + 4, 2 * seg_num + 4);
+  Rfp = R.block(0, 2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2);
+  Rpf = R.block(2 * seg_num + 4, 0, 2 * seg_num - 2, 2 * seg_num + 4);
+  Rpp = R.block(2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2, 2 * seg_num - 2);
+
+  /* ---------- close form solution ---------- */
+
+  Eigen::VectorXd Dxp(2 * seg_num - 2), Dyp(2 * seg_num - 2), Dzp(2 * seg_num - 2);
+  Dxp = -(Rpp.inverse() * Rfp.transpose()) * Dxf;
+  Dyp = -(Rpp.inverse() * Rfp.transpose()) * Dyf;
+  Dzp = -(Rpp.inverse() * Rfp.transpose()) * Dzf;
+
+  Dx1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dxp;
+  Dy1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dyp;
+  Dz1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dzp;
+
+  Px = (A.inverse() * Ct) * Dx1;
+  Py = (A.inverse() * Ct) * Dy1;
+  Pz = (A.inverse() * Ct) * Dz1;
+
+  for (int i = 0; i < seg_num; i++)
+  {
+    poly_coeff.block(i, 0, 1, 6) = Px.segment(i * 6, 6).transpose();
+    poly_coeff.block(i, 6, 1, 6) = Py.segment(i * 6, 6).transpose();
+    poly_coeff.block(i, 12, 1, 6) = Pz.segment(i * 6, 6).transpose();
+  }
+
+  /* ---------- use polynomials ---------- */
+  PolynomialTraj poly_traj;
+  for (int i = 0; i < poly_coeff.rows(); ++i)
+  {
+    vector<double> cx(6), cy(6), cz(6);
+    for (int j = 0; j < 6; ++j)
+    {
+      cx[j] = poly_coeff(i, j), cy[j] = poly_coeff(i, j + 6), cz[j] = poly_coeff(i, j + 12);
+    }
+    reverse(cx.begin(), cx.end());
+    reverse(cy.begin(), cy.end());
+    reverse(cz.begin(), cz.end());
+    double ts = Time(i);
+    poly_traj.addSegment(cx, cy, cz, ts);
+  }
+
+  return poly_traj;
+}
+
+PolynomialTraj PolynomialTraj::one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
+                                                    const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
+                                                    double t)
+{
+  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(6, 6), Crow(1, 6);
+  Eigen::VectorXd Bx(6), By(6), Bz(6);
+
+  C(0, 5) = 1;
+  C(1, 4) = 1;
+  C(2, 3) = 2;
+  Crow << pow(t, 5), pow(t, 4), pow(t, 3), pow(t, 2), t, 1;
+  C.row(3) = Crow;
+  Crow << 5 * pow(t, 4), 4 * pow(t, 3), 3 * pow(t, 2), 2 * t, 1, 0;
+  C.row(4) = Crow;
+  Crow << 20 * pow(t, 3), 12 * pow(t, 2), 6 * t, 2, 0, 0;
+  C.row(5) = Crow;
+
+  Bx << start_pt(0), start_vel(0), start_acc(0), end_pt(0), end_vel(0), end_acc(0);
+  By << start_pt(1), start_vel(1), start_acc(1), end_pt(1), end_vel(1), end_acc(1);
+  Bz << start_pt(2), start_vel(2), start_acc(2), end_pt(2), end_vel(2), end_acc(2);
+
+  Eigen::VectorXd Cofx = C.colPivHouseholderQr().solve(Bx);
+  Eigen::VectorXd Cofy = C.colPivHouseholderQr().solve(By);
+  Eigen::VectorXd Cofz = C.colPivHouseholderQr().solve(Bz);
+
+  vector<double> cx(6), cy(6), cz(6);
+  for (int i = 0; i < 6; i++)
+  {
+    cx[i] = Cofx(i);
+    cy[i] = Cofy(i);
+    cz[i] = Cofz(i);
+  }
+
+  PolynomialTraj poly_traj;
+  poly_traj.addSegment(cx, cy, cz, t);
+
+  return poly_traj;
+}

motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(cmake_utils)
+
+find_package(catkin REQUIRED COMPONENTS roscpp)
+
+set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake)
+
+file(GLOB CMAKE_UILTS_FILES cmake/*.cmake)
+
+catkin_package(
+  CFG_EXTRAS ${CMAKE_UILTS_FILES}
+)
+

motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/arch.cmake

@@ -0,0 +1,126 @@
+set(archdetect_c_code "
+#if defined(__arm__) || defined(__TARGET_ARCH_ARM)
+    #if defined(__ARM_ARCH_7__) \\
+        || defined(__ARM_ARCH_7A__) \\
+        || defined(__ARM_ARCH_7R__) \\
+        || defined(__ARM_ARCH_7M__) \\
+        || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 7)
+        #error cmake_ARCH armv7
+    #elif defined(__ARM_ARCH_6__) \\
+        || defined(__ARM_ARCH_6J__) \\
+        || defined(__ARM_ARCH_6T2__) \\
+        || defined(__ARM_ARCH_6Z__) \\
+        || defined(__ARM_ARCH_6K__) \\
+        || defined(__ARM_ARCH_6ZK__) \\
+        || defined(__ARM_ARCH_6M__) \\
+        || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 6)
+        #error cmake_ARCH armv6
+    #elif defined(__ARM_ARCH_5TEJ__) \\
+        || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 5)
+        #error cmake_ARCH armv5
+    #else
+        #error cmake_ARCH arm
+    #endif
+#elif defined(__i386) || defined(__i386__) || defined(_M_IX86)
+    #error cmake_ARCH i386
+#elif defined(__x86_64) || defined(__x86_64__) || defined(__amd64) || defined(_M_X64)
+    #error cmake_ARCH x86_64
+#elif defined(__ia64) || defined(__ia64__) || defined(_M_IA64)
+    #error cmake_ARCH ia64
+#elif defined(__ppc__) || defined(__ppc) || defined(__powerpc__) \\
+      || defined(_ARCH_COM) || defined(_ARCH_PWR) || defined(_ARCH_PPC)  \\
+      || defined(_M_MPPC) || defined(_M_PPC)
+    #if defined(__ppc64__) || defined(__powerpc64__) || defined(__64BIT__)
+        #error cmake_ARCH ppc64
+    #else
+        #error cmake_ARCH ppc
+    #endif
+#endif
+#error cmake_ARCH unknown
+")
+
+# Set ppc_support to TRUE before including this file or ppc and ppc64
+# will be treated as invalid architectures since they are no longer supported by Apple
+
+function(target_architecture output_var)
+    if(APPLE AND CMAKE_OSX_ARCHITECTURES)
+        # On OS X we use CMAKE_OSX_ARCHITECTURES *if* it was set
+        # First let's normalize the order of the values
+
+        # Note that it's not possible to compile PowerPC applications if you are using
+        # the OS X SDK version 10.6 or later - you'll need 10.4/10.5 for that, so we
+        # disable it by default
+        # See this page for more information:
+        # http://stackoverflow.com/questions/5333490/how-can-we-restore-ppc-ppc64-as-well-as-full-10-4-10-5-sdk-support-to-xcode-4
+
+        # Architecture defaults to i386 or ppc on OS X 10.5 and earlier, depending on the CPU type detected at runtime.
+        # On OS X 10.6+ the default is x86_64 if the CPU supports it, i386 otherwise.
+
+        foreach(osx_arch ${CMAKE_OSX_ARCHITECTURES})
+            if("${osx_arch}" STREQUAL "ppc" AND ppc_support)
+                set(osx_arch_ppc TRUE)
+            elseif("${osx_arch}" STREQUAL "i386")
+                set(osx_arch_i386 TRUE)
+            elseif("${osx_arch}" STREQUAL "x86_64")
+                set(osx_arch_x86_64 TRUE)
+            elseif("${osx_arch}" STREQUAL "ppc64" AND ppc_support)
+                set(osx_arch_ppc64 TRUE)
+            else()
+                message(FATAL_ERROR "Invalid OS X arch name: ${osx_arch}")
+            endif()
+        endforeach()
+
+        # Now add all the architectures in our normalized order
+        if(osx_arch_ppc)
+            list(APPEND ARCH ppc)
+        endif()
+
+        if(osx_arch_i386)
+            list(APPEND ARCH i386)
+        endif()
+
+        if(osx_arch_x86_64)
+            list(APPEND ARCH x86_64)
+        endif()
+
+        if(osx_arch_ppc64)
+            list(APPEND ARCH ppc64)
+        endif()
+    else()
+        file(WRITE "${CMAKE_BINARY_DIR}/arch.c" "${archdetect_c_code}")
+
+        enable_language(C)
+
+        # Detect the architecture in a rather creative way...
+        # This compiles a small C program which is a series of ifdefs that selects a
+        # particular #error preprocessor directive whose message string contains the
+        # target architecture. The program will always fail to compile (both because
+        # file is not a valid C program, and obviously because of the presence of the
+        # #error preprocessor directives... but by exploiting the preprocessor in this
+        # way, we can detect the correct target architecture even when cross-compiling,
+        # since the program itself never needs to be run (only the compiler/preprocessor)
+        try_run(
+            run_result_unused
+            compile_result_unused
+            "${CMAKE_BINARY_DIR}"
+            "${CMAKE_BINARY_DIR}/arch.c"
+            COMPILE_OUTPUT_VARIABLE ARCH
+            CMAKE_FLAGS CMAKE_OSX_ARCHITECTURES=${CMAKE_OSX_ARCHITECTURES}
+        )
+
+        # Parse the architecture name from the compiler output
+        string(REGEX MATCH "cmake_ARCH ([a-zA-Z0-9_]+)" ARCH "${ARCH}")
+
+        # Get rid of the value marker leaving just the architecture name
+        string(REPLACE "cmake_ARCH " "" ARCH "${ARCH}")
+
+        # If we are compiling with an unknown architecture this variable should
+        # already be set to "unknown" but in the case that it's empty (i.e. due
+        # to a typo in the code), then set it to unknown
+        if (NOT ARCH)
+            set(ARCH unknown)
+        endif()
+    endif()
+
+    set(${output_var} "${ARCH}" PARENT_SCOPE)
+endfunction()

motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake

@@ -0,0 +1,2 @@
+list(APPEND CMAKE_MODULE_PATH "${cmake_utils_SOURCE_PREFIX}/cmake_modules")
+link_directories( ${cmake_utils_SOURCE_PREFIX}/lib/mosek8 )

motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/color.cmake

@@ -0,0 +1,17 @@
+string(ASCII 27 Esc)
+set(ColourReset "${Esc}[m")
+set(ColourBold  "${Esc}[1m")
+set(Red         "${Esc}[31m")
+set(Green       "${Esc}[32m")
+set(Yellow      "${Esc}[33m")
+set(Blue        "${Esc}[34m")
+set(Magenta     "${Esc}[35m")
+set(Cyan        "${Esc}[36m")
+set(White       "${Esc}[37m")
+set(BoldRed     "${Esc}[1;31m")
+set(BoldGreen   "${Esc}[1;32m")
+set(BoldYellow  "${Esc}[1;33m")
+set(BoldBlue    "${Esc}[1;34m")
+set(BoldMagenta "${Esc}[1;35m")
+set(BoldCyan    "${Esc}[1;36m")
+set(BoldWhite   "${Esc}[1;37m")

motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake

@@ -0,0 +1,173 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2015 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+#   used to endorse or promote products derived from this software without
+#   specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: alexs.mac@gmail.com (Alex Stewart)
+#
+
+# FindEigen.cmake - Find Eigen library, version >= 3.
+#
+# This module defines the following variables:
+#
+# EIGEN_FOUND: TRUE iff Eigen is found.
+# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
+#
+# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
+# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
+# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
+# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
+#
+# The following variables control the behaviour of this module:
+#
+# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
+#                          search for eigen includes, e.g: /timbuktu/eigen3.
+#
+# The following variables are also defined by this module, but in line with
+# CMake recommended FindPackage() module style should NOT be referenced directly
+# by callers (use the plural variables detailed above instead).  These variables
+# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
+# are NOT re-called (i.e. search for library is not repeated) if these variables
+# are set with valid values _in the CMake cache_. This means that if these
+# variables are set directly in the cache, either by the user in the CMake GUI,
+# or by the user passing -DVAR=VALUE directives to CMake when called (which
+# explicitly defines a cache variable), then they will be used verbatim,
+# bypassing the HINTS variables and other hard-coded search locations.
+#
+# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
+#                    include directory of any dependencies.
+
+# Called if we failed to find Eigen or any of it's required dependencies,
+# unsets all public (designed to be used externally) variables and reports
+# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
+macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
+  unset(EIGEN_FOUND)
+  unset(EIGEN_INCLUDE_DIRS)
+  # Make results of search visible in the CMake GUI if Eigen has not
+  # been found so that user does not have to toggle to advanced view.
+  mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
+  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
+  # use the camelcase library name, not uppercase.
+  if (Eigen_FIND_QUIETLY)
+    message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
+  elseif (Eigen_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
+  else()
+    # Neither QUIETLY nor REQUIRED, use no priority which emits a message
+    # but continues configuration and allows generation.
+    message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(EIGEN_REPORT_NOT_FOUND)
+
+# Protect against any alternative find_package scripts for this library having
+# been called previously (in a client project) which set EIGEN_FOUND, but not
+# the other variables we require / set here which could cause the search logic
+# here to fail.
+unset(EIGEN_FOUND)
+
+# Search user-installed locations first, so that we prefer user installs
+# to system installs where both exist.
+#
+# TODO: Add standard Windows search locations for Eigen.
+list(APPEND EIGEN_CHECK_INCLUDE_DIRS
+  /usr/local/include
+  /usr/local/homebrew/include # Mac OS X
+  /opt/local/var/macports/software # Mac OS X.
+  /opt/local/include
+  /usr/include)
+# Additional suffixes to try appending to each search path.
+list(APPEND EIGEN_CHECK_PATH_SUFFIXES
+  eigen3 # Default root directory for Eigen.
+  Eigen/include/eigen3 ) # Windows (for C:/Program Files prefix).
+
+# Search supplied hint directories first if supplied.
+find_path(EIGEN_INCLUDE_DIR
+  NAMES Eigen/Core
+  PATHS ${EIGEN_INCLUDE_DIR_HINTS}
+  ${EIGEN_CHECK_INCLUDE_DIRS}
+  PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
+
+if (NOT EIGEN_INCLUDE_DIR OR
+    NOT EXISTS ${EIGEN_INCLUDE_DIR})
+  eigen_report_not_found(
+    "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
+    "path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
+endif (NOT EIGEN_INCLUDE_DIR OR
+       NOT EXISTS ${EIGEN_INCLUDE_DIR})
+
+# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
+# if called.
+set(EIGEN_FOUND TRUE)
+
+# Extract Eigen version from Eigen/src/Core/util/Macros.h
+if (EIGEN_INCLUDE_DIR)
+  set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
+  if (NOT EXISTS ${EIGEN_VERSION_FILE})
+    eigen_report_not_found(
+      "Could not find file: ${EIGEN_VERSION_FILE} "
+      "containing version information in Eigen install located at: "
+      "${EIGEN_INCLUDE_DIR}.")
+  else (NOT EXISTS ${EIGEN_VERSION_FILE})
+    file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
+
+    string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
+      EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
+    string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
+      EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
+
+    string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
+      EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
+    string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
+      EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
+
+    string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
+      EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
+    string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
+      EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
+
+    # This is on a single line s/t CMake does not interpret it as a list of
+    # elements and insert ';' separators which would result in 3.;2.;0 nonsense.
+    set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
+  endif (NOT EXISTS ${EIGEN_VERSION_FILE})
+endif (EIGEN_INCLUDE_DIR)
+
+# Set standard CMake FindPackage variables if found.
+if (EIGEN_FOUND)
+  set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
+endif (EIGEN_FOUND)
+
+# Handle REQUIRED / QUIET optional arguments and version.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Eigen
+  REQUIRED_VARS EIGEN_INCLUDE_DIRS
+  VERSION_VAR EIGEN_VERSION)
+
+# Only mark internal variables as advanced if we found Eigen, otherwise
+# leave it visible in the standard GUI for the user to set manually.
+if (EIGEN_FOUND)
+  mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)
+endif (EIGEN_FOUND)

motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake

@@ -0,0 +1,135 @@
+# Try to find gnu scientific library GSL
+# See
+# http://www.gnu.org/software/gsl/  and
+# http://gnuwin32.sourceforge.net/packages/gsl.htm
+#
+# Based on a script of Felix Woelk and Jan Woetzel
+# (www.mip.informatik.uni-kiel.de)
+#
+# It defines the following variables:
+#  GSL_FOUND - system has GSL lib
+#  GSL_INCLUDE_DIRS - where to find headers
+#  GSL_LIBRARIES - full path to the libraries
+#  GSL_LIBRARY_DIRS, the directory where the PLplot library is found.
+
+#  CMAKE_GSL_CXX_FLAGS  = Unix compiler flags for GSL, essentially "`gsl-config --cxxflags`"
+#  GSL_LINK_DIRECTORIES = link directories, useful for rpath on Unix
+#  GSL_EXE_LINKER_FLAGS = rpath on Unix
+
+set( GSL_FOUND OFF )
+set( GSL_CBLAS_FOUND OFF )
+
+# Windows, but not for Cygwin and MSys where gsl-config is available
+if( WIN32 AND NOT CYGWIN AND NOT MSYS )
+  # look for headers
+  find_path( GSL_INCLUDE_DIR
+    NAMES gsl/gsl_cdf.h gsl/gsl_randist.h
+    )
+  if( GSL_INCLUDE_DIR )
+    # look for gsl library
+    find_library( GSL_LIBRARY
+      NAMES gsl
+    )
+    if( GSL_LIBRARY )
+      set( GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR} )
+      get_filename_component( GSL_LIBRARY_DIRS ${GSL_LIBRARY} PATH )
+      set( GSL_FOUND ON )
+    endif( GSL_LIBRARY )
+
+    # look for gsl cblas library
+    find_library( GSL_CBLAS_LIBRARY
+        NAMES gslcblas
+      )
+    if( GSL_CBLAS_LIBRARY )
+      set( GSL_CBLAS_FOUND ON )
+    endif( GSL_CBLAS_LIBRARY )
+
+    set( GSL_LIBRARIES ${GSL_LIBRARY} ${GSL_CBLAS_LIBRARY} )
+  endif( GSL_INCLUDE_DIR )
+
+  mark_as_advanced(
+    GSL_INCLUDE_DIR
+    GSL_LIBRARY
+    GSL_CBLAS_LIBRARY
+  )
+else( WIN32 AND NOT CYGWIN AND NOT MSYS )
+  if( UNIX OR MSYS )
+    find_program( GSL_CONFIG_EXECUTABLE gsl-config
+      /usr/bin/
+      /usr/local/bin
+    )
+
+    if( GSL_CONFIG_EXECUTABLE )
+      set( GSL_FOUND ON )
+
+      # run the gsl-config program to get cxxflags
+      execute_process(
+        COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --cflags
+        OUTPUT_VARIABLE GSL_CFLAGS
+        RESULT_VARIABLE RET
+        ERROR_QUIET
+        )
+      if( RET EQUAL 0 )
+        string( STRIP "${GSL_CFLAGS}" GSL_CFLAGS )
+        separate_arguments( GSL_CFLAGS )
+
+        # parse definitions from cflags; drop -D* from CFLAGS
+        string( REGEX MATCHALL "-D[^;]+"
+          GSL_DEFINITIONS  "${GSL_CFLAGS}" )
+        string( REGEX REPLACE "-D[^;]+;" ""
+          GSL_CFLAGS "${GSL_CFLAGS}" )
+
+        # parse include dirs from cflags; drop -I prefix
+        string( REGEX MATCHALL "-I[^;]+"
+          GSL_INCLUDE_DIRS "${GSL_CFLAGS}" )
+        string( REPLACE "-I" ""
+          GSL_INCLUDE_DIRS "${GSL_INCLUDE_DIRS}")
+        string( REGEX REPLACE "-I[^;]+;" ""
+          GSL_CFLAGS "${GSL_CFLAGS}")
+
+        message("GSL_DEFINITIONS=${GSL_DEFINITIONS}")
+        message("GSL_INCLUDE_DIRS=${GSL_INCLUDE_DIRS}")
+        message("GSL_CFLAGS=${GSL_CFLAGS}")
+      else( RET EQUAL 0 )
+        set( GSL_FOUND FALSE )
+      endif( RET EQUAL 0 )
+
+      # run the gsl-config program to get the libs
+      execute_process(
+        COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --libs
+        OUTPUT_VARIABLE GSL_LIBRARIES
+        RESULT_VARIABLE RET
+        ERROR_QUIET
+        )
+      if( RET EQUAL 0 )
+        string(STRIP "${GSL_LIBRARIES}" GSL_LIBRARIES )
+        separate_arguments( GSL_LIBRARIES )
+
+        # extract linkdirs (-L) for rpath (i.e., LINK_DIRECTORIES)
+        string( REGEX MATCHALL "-L[^;]+"
+          GSL_LIBRARY_DIRS "${GSL_LIBRARIES}" )
+        string( REPLACE "-L" ""
+          GSL_LIBRARY_DIRS "${GSL_LIBRARY_DIRS}" )
+      else( RET EQUAL 0 )
+        set( GSL_FOUND FALSE )
+      endif( RET EQUAL 0 )
+
+      MARK_AS_ADVANCED(
+        GSL_CFLAGS
+      )
+      message( STATUS "Using GSL from ${GSL_PREFIX}" )
+    else( GSL_CONFIG_EXECUTABLE )
+      message( STATUS "FindGSL: gsl-config not found.")
+    endif( GSL_CONFIG_EXECUTABLE )
+  endif( UNIX OR MSYS )
+endif( WIN32 AND NOT CYGWIN AND NOT MSYS )
+
+if( GSL_FOUND )
+  if( NOT GSL_FIND_QUIETLY )
+    message( STATUS "FindGSL: Found both GSL headers and library" )
+  endif( NOT GSL_FIND_QUIETLY )
+else( GSL_FOUND )
+  if( GSL_FIND_REQUIRED )
+    message( FATAL_ERROR "FindGSL: Could not find GSL headers or library" )
+  endif( GSL_FIND_REQUIRED )
+endif( GSL_FOUND )

motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake

@@ -0,0 +1,124 @@
+macro(mvIMPACT_REPORT_NOT_FOUND REASON_MSG)
+    unset(mvIMPACT_FOUND)
+    unset(mvIMPACT_INCLUDE_DIRS)
+    unset(mvIMPACT_LIBRARIES)
+    unset(mvIMPACT_WORLD_VERSION)
+    unset(mvIMPACT_MAJOR_VERSION)
+    unset(mvIMPACT_MINOR_VERSION)
+    # Make results of search visible in the CMake GUI if mvimpact has not
+    # been found so that user does not have to toggle to advanced view.
+    mark_as_advanced(CLEAR mvIMPACT_INCLUDE_DIR)
+    # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
+    # use the camelcase library name, not uppercase.
+    if(Mvimpact_FIND_QUIETLY)
+        message(STATUS "Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
+    elseif(Mvimpact_FIND_REQUIRED)
+        message(FATAL_ERROR "Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
+    else()
+        # Neither QUIETLY nor REQUIRED, use no priority which emits a message
+        # but continues configuration and allows generation.
+        message("-- Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
+    endif()
+endmacro(mvIMPACT_REPORT_NOT_FOUND)
+
+# Search user-installed locations first, so that we prefer user installs
+# to system installs where both exist.
+get_filename_component(BLUEFOX2_DIR ${CMAKE_CURRENT_SOURCE_DIR} REALPATH)
+list(APPEND mvIMPACT_CHECK_INCLUDE_DIRS
+    /opt/mvIMPACT_acquire
+    /opt/mvIMPACT_Acquire
+    )
+execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCH)
+
+if(NOT ARCH)
+set(ARCH "arm64")
+elseif(${ARCH} STREQUAL "aarch64")
+set(ARCH "arm64")
+endif()
+
+list(APPEND mvIMPACT_CHECK_LIBRARY_DIRS
+    /opt/mvIMPACT_acquire/lib/${ARCH}
+    /opt/mvIMPACT_Acquire/lib/${ARCH}
+    )
+
+# Check general hints
+if(mvIMPACT_HINTS AND EXISTS ${mvIMPACT_HINTS})
+    set(mvIMPACT_INCLUDE_DIR_HINTS ${mvIMPACT_HINTS}/include)
+    set(mvIMPACT_LIBRARY_DIR_HINTS ${mvIMPACT_HINTS}/lib)
+endif()
+
+# Search supplied hint directories first if supplied.
+# Find include directory for mvimpact
+find_path(mvIMPACT_INCLUDE_DIR
+    NAMES mvIMPACT_CPP/mvIMPACT_acquire.h
+    PATHS ${mvIMPACT_INCLUDE_DIR_HINTS}
+    ${mvIMPACT_CHECK_INCLUDE_DIRS}
+    NO_DEFAULT_PATH)
+if(NOT mvIMPACT_INCLUDE_DIR OR NOT EXISTS ${mvIMPACT_INCLUDE_DIR})
+    mvIMPACT_REPORT_NOT_FOUND(
+        "Could not find mvimpact include directory, set mvIMPACT_INCLUDE_DIR to "
+        "path to mvimpact include directory,"
+        "e.g. /opt/mvIMPACT_acquire.")
+else()
+    message(STATUS "mvimpact include dir found: " ${mvIMPACT_INCLUDE_DIR})
+endif()
+
+# Find library directory for mvimpact
+find_library(mvIMPACT_LIBRARY
+    NAMES libmvBlueFOX.so
+    PATHS ${mvIMPACT_LIBRARY_DIR_HINTS}
+    ${mvIMPACT_CHECK_LIBRARY_DIRS}
+    NO_DEFAULT_PATH)
+if(NOT mvIMPACT_LIBRARY OR NOT EXISTS ${mvIMPACT_LIBRARY})
+    mvIMPACT_REPORT_NOT_FOUND(
+        "Could not find mvimpact library, set mvIMPACT_LIBRARY "
+        "to full path to mvimpact library direcotory.${mvIMPACT_CHECK_LIBRARY_DIRS}~~")
+else()
+    # TODO: need to fix this hacky solution for getting mvIMPACT_LIBRARY_DIR
+    string(REGEX MATCH ".*/" mvIMPACT_LIBRARY_DIR ${mvIMPACT_LIBRARY})
+    message(STATUS "mvimpact library dir found: " ${mvIMPACT_LIBRARY_DIR})
+
+endif()
+
+# Mark internally as found, then verify. mvIMPACT_REPORT_NOT_FOUND() unsets if
+# called.
+set(mvIMPACT_FOUND TRUE)
+
+# Extract mvimpact version
+if(mvIMPACT_LIBRARY_DIR)
+    file(GLOB mvIMPACT_LIBS
+        RELATIVE ${mvIMPACT_LIBRARY_DIR}
+        ${mvIMPACT_LIBRARY_DIR}/libmvBlueFOX.so.[0-9].[0-9].[0-9])
+    # TODO: add version support
+    # string(REGEX MATCH ""
+    #       mvIMPACT_WORLD_VERSION ${mvIMPACT_PVBASE})
+    # message(STATUS "mvimpact world version: " ${mvIMPACT_WORLD_VERSION})
+endif()
+
+# Catch case when caller has set mvIMPACT_INCLUDE_DIR in the cache / GUI and
+# thus FIND_[PATH/LIBRARY] are not called, but specified locations are
+# invalid, otherwise we would report the library as found.
+if(NOT mvIMPACT_INCLUDE_DIR AND NOT EXISTS ${mvIMPACT_INCLUDE_DIR}/mvIMPACT_CPP/mvIMPACT_acquire.h)
+    mvIMPACT_REPORT_NOT_FOUND("Caller defined mvIMPACT_INCLUDE_DIR: "
+        ${mvIMPACT_INCLUDE_DIR}
+        " does not contain mvIMPACT_CPP/mvIMPACT_acquire.h header.")
+endif()
+
+# Set standard CMake FindPackage variables if found.
+if(mvIMPACT_FOUND)
+    set(mvIMPACT_INCLUDE_DIRS ${mvIMPACT_INCLUDE_DIR})
+    file(GLOB mvIMPACT_LIBRARIES ${mvIMPACT_LIBRARY_DIR}lib*.so)
+endif()
+
+# Handle REQUIRED / QUIET optional arguments.
+include(FindPackageHandleStandardArgs)
+if(mvIMPACT_FOUND)
+    FIND_PACKAGE_HANDLE_STANDARD_ARGS(Mvimpact DEFAULT_MSG
+        mvIMPACT_INCLUDE_DIRS mvIMPACT_LIBRARIES)
+endif()
+
+# Only mark internal variables as advanced if we found mvimpact, otherwise
+# leave it visible in the standard GUI for the user to set manually.
+if(mvIMPACT_FOUND)
+    mark_as_advanced(FORCE mvIMPACT_INCLUDE_DIR mvIMPACT_LIBRARY)
+endif()

motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package  format="2">
+  <name>cmake_utils</name>
+  <version>0.0.0</version>
+  <description>
+  Once you used this package,
+  then you do not need to copy cmake files among packages
+  </description>
+
+  <maintainer email="william.wu@dji.com"> William.Wu </maintainer>
+
+  <license>LGPLv3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>cmake_modules</depend>
+
+</package>