Merge pull request '将多模态算法集成到无人机内部的 Prometheus 中' (#13) from luogang_branch into master

src/Prometheus/Modules/acoustic_analyzer/src/acoustic_analyzer_node.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-"
+"""
+声源检测节点 - Gazebo仿真版本
+
+功能:
+- 模拟声源检测（枪声、爆炸等）
+- 发布声源威胁到 /acoustic/threats
+
+用于多模态威胁融合系统的Gazebo仿真测试。
+"""
+
+import rospy
+import numpy as np
+from std_msgs.msg import Header
+from std_msgs.msg import String
+from geometry_msgs.msg import PoseStamped
+
+
+# 自定义消息定义 (如果还没有安装 acoustic_analyzer 消息包)
+class AcousticThreat:
+    """声源威胁数据结构"""
+    def __init__(self):
+        self.threat_id = ""
+        self.sound_type = ""  # gunshot, artillery, explosion, ambient
+        self.confidence = 0.0
+        self.azimuth = 0.0      # 方位角 (度), 0=北, 顺时针
+        self.elevation = 0.0    # 仰角 (度)
+        self.distance = -1.0    # 距离 (米)
+        self.distance_confidence = 0.0
+
+
+class AcousticThreatArray:
+    """声源威胁数组"""
+    def __init__(self):
+        self.header = Header()
+        self.threats = []
+
+
+class AcousticAnalyzerNode:
+    """声源检测节点 - 仿真版本"""
+    
+    SOUND_TYPES = ['gunshot', 'artillery', 'explosion', 'ambient']
+    
+    def __init__(self):
+        rospy.init_node('acoustic_analyzer', anonymous=False)
+        
+        # 参数
+        self.uav_id = rospy.get_param('~uav_id', 1)
+        self.detection_rate = rospy.get_param('~detection_rate', 2.0)  # Hz
+        self.sim_enabled = rospy.get_param('~simulation_enabled', True)
+        self.sim_threat_probability = rospy.get_param('~sim_threat_probability', 0.1)
+        
+        # 发布者 - 使用JSON字符串作为临时方案
+        self.threats_pub = rospy.Publisher(
+            '/acoustic/threats', String, queue_size=5)
+        
+        # 定时器
+        self.timer = rospy.Timer(
+            rospy.Duration(1.0 / self.detection_rate), self._detect_cb)
+        
+        rospy.loginfo("[AcousticAnalyzer] 声源检测节点启动")
+        rospy.loginfo(f"[AcousticAnalyzer] 仿真模式: {self.sim_enabled}")
+        rospy.loginfo(f"[AcousticAnalyzer] 检测频率: {self.detection_rate}Hz")
+    
+    def _detect_cb(self, event):
+        """定时检测回调"""
+        if not self.sim_enabled:
+            return
+        
+        threats = []
+        
+        # 仿真模式：随机生成声源威胁
+        if np.random.random() < self.sim_threat_probability:
+            # 随机选择威胁类型
+            weights = [0.4, 0.2, 0.3, 0.1]  # gunshot, artillery, explosion, ambient
+            sound_type = np.random.choice(self.SOUND_TYPES, p=weights)
+            
+            # 随机方位角 (0-360度，北为0)
+            azimuth = np.random.uniform(0, 360)
+            
+            # 随机仰角 (-30到+30度)
+            elevation = np.random.uniform(-30, 30)
+            
+            # 随机距离 (5-100米)
+            distance = np.random.uniform(5, 100)
+            
+            # 置信度
+            confidence = np.random.uniform(0.6, 0.95)
+            
+            threat = {
+                'threat_id': f'AC-{rospy.Time.now().to_nsec() % 10000:04d}',
+                'sound_type': sound_type,
+                'confidence': float(confidence),
+                'azimuth': float(azimuth),
+                'elevation': float(elevation),
+                'distance': float(distance),
+                'distance_confidence': float(np.random.uniform(0.5, 0.8))
+            }
+            threats.append(threat)
+            
+            rospy.loginfo(f"[AcousticAnalyzer] 检测到声源: {sound_type}, "
+                       f"方位={azimuth:.1f}°, 距离={distance:.1f}m")
+        
+        # 发布结果 (JSON格式)
+        if threats:
+            import json
+            msg = String()
+            msg.data = json.dumps({
+                'header': {
+                    'stamp': rospy.Time.now().to_sec(),
+                    'frame_id': 'uav_base_link'
+                },
+                'threats': threats
+            })
+            self.threats_pub.publish(msg)
+    
+    def run(self):
+        rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        node = AcousticAnalyzerNode()
+        node.run()
+    except rospy.ROSInterruptException:
+        pass

src/Prometheus/Modules/flash_detector/src/flash_detector_node.py

@@ -0,0 +1,137 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-"
+"""
+闪光检测节点 - Gazebo仿真版本
+
+功能:
+- 订阅相机图像，检测亮度突变（模拟枪口闪光）
+- 发布闪光检测结果到 /flash_detector/detection
+
+用于多模态威胁融合系统的Gazebo仿真测试。
+"""
+
+import rospy
+import numpy as np
+from sensor_msgs.msg import Image
+from std_msgs.msg import Bool, Float32MultiArray
+from cv_bridge import CvBridge
+import cv2
+
+
+class FlashDetectorNode:
+    """闪光检测节点 - 仿真版本"""
+    
+    def __init__(self):
+        rospy.init_node('flash_detector', anonymous=False)
+        
+        # 参数
+        self.uav_id = rospy.get_param('~uav_id', 1)
+        self.image_topic = rospy.get_param('~image_topic', 
+            f'/uav{self.uav_id}/camera/color/image_raw')
+        self.brightness_threshold = rospy.get_param('~brightness_threshold', 200)
+        self.area_threshold = rospy.get_param('~area_threshold', 50)
+        self.simulation_mode = rospy.get_param('~simulation_mode', True)
+        
+        # 用于仿真的随机闪光触发
+        self.sim_flash_probability = rospy.get_param('~sim_flash_probability', 0.05)
+        self.sim_flash_duration = rospy.get_param('~sim_flash_duration', 3)
+        self._flash_counter = 0
+        
+        # CV桥接
+        self.bridge = CvBridge()
+        self._prev_gray = None
+        
+        # 发布者
+        self.flash_bool_pub = rospy.Publisher(
+            '/flash_detector/flash_detected', Bool, queue_size=5)
+        self.flash_det_pub = rospy.Publisher(
+            '/flash_detector/detection', Float32MultiArray, queue_size=5)
+        
+        # 订阅者
+        rospy.Subscriber(self.image_topic, Image, self._image_cb, queue_size=1)
+        
+        rospy.loginfo(f"[FlashDetector] UAV{self.uav_id} 闪光检测节点启动")
+        rospy.loginfo(f"[FlashDetector] 图像话题: {self.image_topic}")
+        rospy.loginfo(f"[FlashDetector] 仿真模式: {self.simulation_mode}")
+    
+    def _image_cb(self, msg):
+        """图像回调 - 检测亮度突变"""
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+            gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
+            
+            flash_detected = False
+            flash_data = [0, 0, 0, 0, 0]  # [cx, cy, area, sight_x, sight_y]
+            
+            if self.simulation_mode:
+                # 仿真模式：随机触发闪光事件
+                if np.random.random() < self.sim_flash_probability:
+                    self._flash_counter = self.sim_flash_duration
+                
+                if self._flash_counter > 0:
+                    self._flash_counter -= 1
+                    flash_detected = True
+                    # 生成随机闪光位置
+                    h, w = gray.shape
+                    cx = w // 2 + int(np.random.normal(0, w//4))
+                    cy = h // 2 + int(np.random.normal(0, h//4))
+                    cx = np.clip(cx, 0, w-1)
+                    cy = np.clip(cy, 0, h-1)
+                    area = int(np.random.uniform(100, 400))
+                    
+                    # 视线角度 (相对于相机中心)
+                    fov_x = 60.0  # 水平视场角
+                    fov_y = 45.0  # 垂直视场角
+                    sight_x = np.radians((cx - w/2) / (w/2) * (fov_x/2))
+                    sight_y = np.radians((cy - h/2) / (h/2) * (fov_y/2))
+                    
+                    flash_data = [float(cx), float(cy), float(area), sight_x, sight_y]
+                    rospy.loginfo(f"[FlashDetector] 仿真闪光检测: cx={cx}, cy={cy}, area={area}")
+            else:
+                # 真实检测模式：帧间差分检测亮度突变
+                if self._prev_gray is not None:
+                    diff = cv2.absdiff(gray, self._prev_gray)
+                    _, bright_mask = cv2.threshold(diff, self.brightness_threshold, 255, cv2.THRESH_BINARY)
+                    
+                    # 查找亮区轮廓
+                    contours, _ = cv2.findContours(bright_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+                    
+                    for cnt in contours:
+                        area = cv2.contourArea(cnt)
+                        if area > self.area_threshold:
+                            flash_detected = True
+                            M = cv2.moments(cnt)
+                            cx = int(M['m10'] / M['m00']) if M['m00'] > 0 else 0
+                            cy = int(M['m01'] / M['m00']) if M['m00'] > 0 else 0
+                            
+                            h, w = gray.shape
+                            fov_x, fov_y = 60.0, 45.0
+                            sight_x = np.radians((cx - w/2) / (w/2) * (fov_x/2))
+                            sight_y = np.radians((cy - h/2) / (h/2) * (fov_y/2))
+                            
+                            flash_data = [float(cx), float(cy), float(area), sight_x, sight_y]
+                            rospy.loginfo(f"[FlashDetector] 检测到闪光: area={area:.0f}")
+                            break
+                
+                self._prev_gray = gray.copy()
+            
+            # 发布结果
+            self.flash_bool_pub.publish(Bool(flash_detected))
+            if flash_detected:
+                msg_out = Float32MultiArray()
+                msg_out.data = flash_data
+                self.flash_det_pub.publish(msg_out)
+                
+        except Exception as e:
+            rospy.logwarn(f"[FlashDetector] 处理异常: {e}")
+    
+    def run(self):
+        rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        node = FlashDetectorNode()
+        node.run()
+    except rospy.ROSInterruptException:
+        pass

src/Prometheus/Modules/motion_planning/CMakeLists.txt

@@ -0,0 +1,69 @@
+# toplevel CMakeLists.txt for a catkin workspace
+# catkin/cmake/toplevel.cmake
+
+cmake_minimum_required(VERSION 3.0.2)
+
+project(Project)
+
+set(CATKIN_TOPLEVEL TRUE)
+
+# search for catkin within the workspace
+set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
+execute_process(COMMAND ${_cmd}
+  RESULT_VARIABLE _res
+  OUTPUT_VARIABLE _out
+  ERROR_VARIABLE _err
+  OUTPUT_STRIP_TRAILING_WHITESPACE
+  ERROR_STRIP_TRAILING_WHITESPACE
+)
+if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
+  # searching for catkin resulted in an error
+  string(REPLACE ";" " " _cmd_str "${_cmd}")
+  message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
+endif()
+
+# include catkin from workspace or via find_package()
+if(_res EQUAL 0)
+  set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
+  # include all.cmake without add_subdirectory to let it operate in same scope
+  include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
+  add_subdirectory("${_out}")
+
+else()
+  # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
+  # or CMAKE_PREFIX_PATH from the environment
+  if(NOT DEFINED CMAKE_PREFIX_PATH)
+    if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
+      if(NOT WIN32)
+        string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      else()
+        set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      endif()
+    endif()
+  endif()
+
+  # list of catkin workspaces
+  set(catkin_search_path "")
+  foreach(path ${CMAKE_PREFIX_PATH})
+    if(EXISTS "${path}/.catkin")
+      list(FIND catkin_search_path ${path} _index)
+      if(_index EQUAL -1)
+        list(APPEND catkin_search_path ${path})
+      endif()
+    endif()
+  endforeach()
+
+  # search for catkin in all workspaces
+  set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
+  find_package(catkin QUIET
+    NO_POLICY_SCOPE
+    PATHS ${catkin_search_path}
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+  unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
+
+  if(NOT catkin_FOUND)
+    message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
+  endif()
+endif()
+
+catkin_workspace()

src/Prometheus/Modules/motion_planning/global_planner/CMakeLists.txt

@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(prometheus_global_planner)
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+	roscpp
+	rospy
+	std_msgs
+	sensor_msgs
+	laser_geometry
+	geometry_msgs
+	nav_msgs
+	pcl_ros
+	visualization_msgs
+	gazebo_msgs
+	prometheus_msgs
+)
+
+
+catkin_package(
+	INCLUDE_DIRS include
+)
+
+include_directories(
+       SYSTEM
+       include
+	   ${PROJECT_SOURCE_DIR}/include
+       ${catkin_INCLUDE_DIRS}
+       ${Eigen3_INCLUDE_DIRS}
+       ${PCL_INCLUDE_DIRS}
+	   ${PROJECT_SOURCE_DIR}/../../common/include
+     )
+link_directories(${PCL_LIBRARY_DIRS})
+
+add_library(occupy_map src/occupy_map.cpp)
+target_link_libraries(occupy_map ${PCL_LIBRARIES})
+add_library(A_star src/A_star.cpp)
+target_link_libraries(A_star occupy_map)
+add_library(global_planner src/global_planner.cpp)
+target_link_libraries(global_planner A_star)
+
+add_executable(global_planner_main src/global_planner_node.cpp)
+add_dependencies(global_planner_main prometheus_global_planner_gencpp)
+target_link_libraries(global_planner_main ${catkin_LIBRARIES})
+target_link_libraries(global_planner_main ${PCL_LIBRARIES})
+target_link_libraries(global_planner_main global_planner)
+
+## 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 (e.g. launch and bag files, etc.)
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)

src/Prometheus/Modules/motion_planning/global_planner/global_planner.md

@@ -0,0 +1,86 @@
+## global_planner
+
+  - 使用gazebo环境获取感知信息还是通过map_generator?
+  - 使用PX4自带的动力学or fake_odom模块?
+  - 
+
+#### 情况讨论
+
+ - 全局点云情况(测试)
+   - 真实场景对应:已知全局地图
+   - map_generator生成全局点云
+   - 无人机不需要搭载传感器
+   - PX4动力学 & fake_odom模块
+ 
+
+启动仿真环境
+roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch 
+ 
+ - 依次拨动swb，切换值COMMAND_CONTROL模式，无人机会自动起飞至指定高度
+
+启动规划算法
+roslaunch prometheus_global_planner sitl_global_planner_with_global_point.launch
+ 
+ - 在RVIZ中选定目标点，或者使用终端发布rostopic pub /uav1/prometheus/motion_planning/goal xxx
+
+
+
+ - 局部点云情况（todo）
+   - 对应真实情况:D435i等RGBD相机,三维激光雷达
+   - map_generator生成全局点云,模拟得到局部点云信息
+   - 无人机不需要搭载传感器
+   - PX4动力学 & fake_odom模块
+
+
+启动仿真环境
+roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch 
+ 
+ - 依次拨动swb，切换值COMMAND_CONTROL模式，无人机会自动起飞至指定高度
+
+启动规划算法
+roslaunch prometheus_global_planner sitl_global_planner_with_local_point.launch
+ 
+ - 在RVIZ中选定目标点，或者使用终端发布rostopic pub /uav1/prometheus/motion_planning/goal xxx
+
+
+ - 2dlidar情况（todo）
+   - 对应真实情况:二维激光雷达
+   - projector_.projectLaser(*local_point, input_laser_scan)将scan转换为点云,即对应回到局部点云情况
+   - map_generator生成全局点云,模拟得到局部点云信息
+   - 无人机不需要搭载传感器
+   - PX4动力学 & fake_odom模块
+
+启动仿真环境
+roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch 
+ 
+ - 依次拨动swb，切换值COMMAND_CONTROL模式，无人机会自动起飞至指定高度
+
+启动规划算法
+roslaunch prometheus_global_planner sitl_global_planner_with_2dlidar.launch
+ 
+ - 在RVIZ中选定目标点，或者使用终端发布rostopic pub /uav1/prometheus/motion_planning/goal xxx
+
+
+ - 多机情况（todo）
+   - 建议使用 全局点云情况 + 多个无人机
+   - fake_odom模块
+
+## wiki安排
+
+ 6.1 simulator_utils
+  6.1.1 map_generator
+  6.1.2 fake_odom
+ 6.2 motion_planning
+  6.2.1 global_planner
+  6.2.2 local_planner
+ 6.3 ego_planner
+
+
+## 8.27 update
+
+2dlidar仿真
+
+执行sitl_global_planner_with_2dlidar.sh脚本
+
+- 勉强也能使用，但是有的时候，路径有点奇怪（不是最短路径）（主要问题）
+- 另外还有点慢（可以调参，仿真中无法反映真机的情况）

src/Prometheus/Modules/motion_planning/global_planner/include/A_star.h

@@ -0,0 +1,181 @@
+#ifndef _ASTAR_H
+#define _ASTAR_H
+
+#include <ros/ros.h>
+#include <Eigen/Eigen>
+#include <iostream>
+#include <queue>
+#include <string>
+#include <unordered_map>
+#include <sstream>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Path.h>
+
+#include "occupy_map.h"
+#include "printf_utils.h"
+#include "global_planner_utils.h"
+
+using namespace std;
+
+
+
+#define IN_CLOSE_SET 'a'
+#define IN_OPEN_SET 'b'
+#define NOT_EXPAND 'c'
+#define inf 1 >> 30
+
+  class Node
+  {
+  public:
+    /* -------------------- */
+    Eigen::Vector3i index;
+    Eigen::Vector3d position;
+    double g_score, f_score;
+    Node *parent;
+    char node_state;
+
+    double time; // dyn
+    int time_idx;
+
+    Node()
+    {
+      parent = NULL;
+      node_state = NOT_EXPAND;
+    }
+    ~Node(){};
+  };
+  typedef Node *NodePtr;
+
+  // 为什么这么麻烦，不能直接比较吗
+  class NodeComparator0
+  {
+  public:
+    bool operator()(NodePtr node1, NodePtr node2)
+    {
+      return node1->f_score > node2->f_score;
+    }
+  };
+
+  template <typename T>
+  struct matrix_hash0 : 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;
+    }
+  };
+
+  class NodeHashTable0
+  {
+  private:
+    /* data */
+    std::unordered_map<Eigen::Vector3i, NodePtr, matrix_hash0<Eigen::Vector3i>> data_3d_;
+
+  public:
+    NodeHashTable0(/* args */)
+    {
+    }
+    ~NodeHashTable0()
+    {
+    }
+    void insert(Eigen::Vector3i idx, NodePtr node)
+    {
+      data_3d_.insert(std::make_pair(idx, node));
+    }
+
+    NodePtr find(Eigen::Vector3i idx)
+    {
+      auto iter = data_3d_.find(idx);
+      return iter == data_3d_.end() ? NULL : iter->second;
+    }
+
+    void clear()
+    {
+      data_3d_.clear();
+    }
+  };
+
+  class Astar
+  {
+  private:
+    // 备选路径点指针容器
+    std::vector<NodePtr> path_node_pool_;
+    // 使用节点计数器、迭代次数计数器
+    int use_node_num_, iter_num_;
+    // 扩展的节点
+    NodeHashTable0 expanded_nodes_;
+    // open set （根据规则已排序好）
+    std::priority_queue<NodePtr, std::vector<NodePtr>, NodeComparator0> open_set_;
+    // 最终路径点容器
+    std::vector<NodePtr> path_nodes_;
+
+    // 参数
+    // 启发式参数
+    double lambda_heu_;
+    double lambda_cost_;
+    // 最大搜索次数
+    int max_search_num;
+    // tie breaker
+    double tie_breaker_;
+    bool is_2D;
+    double fly_height;
+
+    // 目标点
+    Eigen::Vector3d goal_pos;
+
+    // 地图相关
+    std::vector<int> occupancy_buffer_;
+    double resolution_, inv_resolution_;
+    Eigen::Vector3d origin_, map_size_3d_;
+
+    // 辅助函数
+    Eigen::Vector3i posToIndex(Eigen::Vector3d pt);
+    void indexToPos(Eigen::Vector3i id, Eigen::Vector3d &pos);
+    void retrievePath(NodePtr end_node);
+
+    // 搜索启发函数，三种形式，选用其中一种即可
+    double getDiagHeu(Eigen::Vector3d x1, Eigen::Vector3d x2);
+    double getManhHeu(Eigen::Vector3d x1, Eigen::Vector3d x2);
+    double getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2);
+
+  public:
+    Astar(){}
+    ~Astar();
+
+    enum
+    {
+      REACH_END = 1,
+      NO_PATH = 2
+    };
+
+    // 占据图类
+    Occupy_map::Ptr Occupy_map_ptr;
+
+    // 重置
+    void reset();
+    // 初始化
+    void init(ros::NodeHandle &nh);
+    // 检查安全性
+    bool check_safety(Eigen::Vector3d &cur_pos, double safe_distance);
+    // 搜索
+    int search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
+    // 返回路径
+    std::vector<Eigen::Vector3d> getPath();
+    // 返回ros消息格式的路径
+    nav_msgs::Path get_ros_path();
+    // 返回访问过的节点
+    std::vector<NodePtr> getVisitedNodes();
+
+    typedef std::shared_ptr<Astar> Ptr;
+  };
+
+
+
+#endif

src/Prometheus/Modules/motion_planning/global_planner/include/global_planner.h

@@ -0,0 +1,121 @@
+#ifndef GLOBAL_PLANNER
+#define GLOBAL_PLANNER
+
+#include <ros/ros.h>
+
+#include <Eigen/Eigen>
+#include <iostream>
+#include <algorithm>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Bool.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
+#include <laser_geometry/laser_geometry.h>
+#include <prometheus_msgs/UAVState.h>
+#include <prometheus_msgs/UAVCommand.h>
+#include <prometheus_msgs/UAVControlState.h>
+
+#include "A_star.h"
+#include "occupy_map.h"
+#include "printf_utils.h"
+#include "global_planner_utils.h"
+using namespace std;
+
+
+    class GlobalPlanner
+    {
+    public:
+        GlobalPlanner(ros::NodeHandle &nh);
+        ros::NodeHandle global_planner_nh;
+
+    private:
+        string uav_name;   // 无人机名字
+        int uav_id;        // 无人机编号
+        bool sim_mode;
+        int map_input_source;
+        double fly_height;
+        double safe_distance;
+        double time_per_path;
+        double replan_time;
+        bool consider_neighbour;
+        string global_pcl_topic_name, local_pcl_topic_name, scan_topic_name;
+        
+        // 订阅无人机状态、目标点、传感器数据（生成地图）
+        ros::Subscriber goal_sub;
+        ros::Subscriber uav_state_sub;
+        // 支持2维激光雷达、3维激光雷达、D435i等实体传感器
+        // 支持直接输入全局已知点云
+        ros::Subscriber Gpointcloud_sub;
+        ros::Subscriber Lpointcloud_sub;
+        ros::Subscriber laserscan_sub;
+        ros::Subscriber uav_control_state_sub;
+        // 发布控制指令
+        ros::Publisher uav_cmd_pub;
+        ros::Publisher path_cmd_pub;
+        ros::Timer mainloop_timer;
+        ros::Timer track_path_timer;
+        ros::Timer safety_timer;
+        // A星规划器
+        Astar::Ptr Astar_ptr;
+
+        // laserscan2pointcloud2 投影器
+        laser_geometry::LaserProjection projector_;
+
+        prometheus_msgs::UAVState uav_state;      // 无人机状态
+        prometheus_msgs::UAVControlState uav_control_state;
+        nav_msgs::Odometry uav_odom;
+        Eigen::Vector3d uav_pos;  // 无人机位置
+        Eigen::Vector3d uav_vel;  // 无人机速度
+        Eigen::Quaterniond uav_quat; // 无人机四元数
+        double uav_yaw;
+        // 规划终端状态
+        Eigen::Vector3d goal_pos, goal_vel;
+        prometheus_msgs::UAVCommand uav_command; 
+        nav_msgs::Path path_cmd;
+        double distance_to_goal;
+
+        // 规划器状态
+        bool odom_ready;
+        bool drone_ready;
+        bool sensor_ready;
+        bool goal_ready;
+        bool is_safety;
+        bool is_new_path;
+        bool path_ok;
+        int start_point_index;
+        int Num_total_wp;
+        int cur_id;
+        float desired_yaw;
+
+        ros::Time last_replan_time;
+
+        // 五种状态机
+        enum EXEC_STATE
+        {
+            WAIT_GOAL,
+            PLANNING,
+            TRACKING,
+            LANDING,
+        };
+        EXEC_STATE exec_state;
+
+        // 回调函数
+        void goal_cb(const geometry_msgs::PoseStampedConstPtr &msg);
+        void uav_state_cb(const prometheus_msgs::UAVStateConstPtr &msg);
+        void Gpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
+        void Lpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
+        void laser_cb(const sensor_msgs::LaserScanConstPtr &msg);
+        void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg);
+        void safety_cb(const ros::TimerEvent &e);
+        void mainloop_cb(const ros::TimerEvent &e);
+        void track_path_cb(const ros::TimerEvent &e);
+        void debug_info();
+        int get_start_point_id(void);
+    };
+
+
+
+#endif

src/Prometheus/Modules/motion_planning/global_planner/include/global_planner_utils.h

@@ -0,0 +1,8 @@
+#ifndef GLOBAL_PLANNER_UTILS_H
+#define GLOBAL_PLANNER_UTILS_H
+
+#define NODE_NAME "GlobalPlanner [Astar] "
+#define MIN_DIS 0.1
+
+#endif
+

src/Prometheus/Modules/motion_planning/global_planner/include/occupy_map.h

@@ -0,0 +1,128 @@
+#ifndef _OCCUPY_MAP_H
+#define _OCCUPY_MAP_H
+
+#include <iostream>
+#include <algorithm>
+
+#include <ros/ros.h>
+#include <Eigen/Eigen>
+
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf/transform_listener.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Odometry.h>
+#include <visualization_msgs/Marker.h>
+#include <sensor_msgs/LaserScan.h>
+#include <laser_geometry/laser_geometry.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/voxel_grid.h>
+#include <map>
+#include "printf_utils.h"
+#include "global_planner_utils.h"
+using namespace std;
+
+
+    class Occupy_map
+    {
+    public:
+        Occupy_map(){}
+        double fly_height;
+
+        // 局部地图 滑窗 存储器
+        map<int, pcl::PointCloud<pcl::PointXYZ>> point_cloud_pair;
+        // 全局地图点云指针
+        pcl::PointCloud<pcl::PointXYZ>::Ptr global_point_cloud_map;
+        // 全局膨胀点云指针
+        pcl::PointCloud<pcl::PointXYZ>::Ptr global_uav_pcl;
+        // 考虑变指针
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inflate_vis_;
+        // 临时指针
+        pcl::PointCloud<pcl::PointXYZ>::Ptr input_point_cloud;
+        pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud;
+        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr;
+        // 地图边界点云
+        pcl::PointCloud<pcl::PointXYZ> border;
+        // VoxelGrid过滤器用于下采样
+        pcl::VoxelGrid<pcl::PointXYZ> vg;
+        // OutlierRemoval用于去除离群值
+        pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+        // laserscan2pointcloud2 中间变量
+        sensor_msgs::PointCloud2 input_laser_scan;
+        // laserscan2pointcloud2 投影器
+        laser_geometry::LaserProjection projector_;
+        // 上一帧odom
+        double f_x, f_y, f_z, f_roll, f_pitch, f_yaw;
+        // 局部地图滑窗，指示器以及大小
+        int st_it, queue_size;
+        // flag：展示地图边界
+        bool show_border;
+        bool sim_mode;
+        // 地图是否占据容器， 从编程角度来讲，这就是地图变为单一序列化后的索引
+        std::vector<int> occupancy_buffer_; // 0 is free, 1 is occupied
+        // 代价地图
+        std::vector<double> cost_map_; // cost
+        // 地图分辨率
+        double resolution_, inv_resolution_;
+        string node_name;
+        // 膨胀参数
+        double inflate_;
+        double odom_inflate_;
+        // 地图原点,地图尺寸
+        Eigen::Vector3d origin_, map_size_3d_, min_range_, max_range_;
+        // 占据图尺寸 = 地图尺寸 / 分辨率
+        Eigen::Vector3i grid_size_;
+        int swarm_num_uav; // 集群数量
+        string uav_name;   // 无人机名字
+        int uav_id;        // 无人机编号
+        bool get_gpcl, get_lpcl, get_laser;
+        Eigen::Vector3d enum_p[100], enum_p_uav[1000], enum_p_cost[1000];
+        int ifn;
+        int inflate_index, inflate_index_uav, cost_index, cost_inflate;
+        // 发布点云用于rviz显示
+        ros::Publisher global_pcl_pub, inflate_pcl_pub;
+        ros::Timer pcl_pub;
+
+        //初始化
+        void init(ros::NodeHandle &nh);
+        // 地图更新函数 - 输入：全局点云
+        void map_update_gpcl(const sensor_msgs::PointCloud2ConstPtr &global_point);
+        // 工具函数：合并局部地图
+        void local_map_merge_odom(const nav_msgs::Odometry &odom);
+        void uav_pcl_update(Eigen::Vector3d *input_uav_odom, bool *get_uav_odom);
+        // 地图更新函数 - 输入：局部点云
+        void map_update_lpcl(const sensor_msgs::PointCloud2ConstPtr &local_point, const nav_msgs::Odometry &odom);
+        // 地图更新函数 - 输入：二维激光雷达
+        void map_update_laser(const sensor_msgs::LaserScanConstPtr &local_point, const nav_msgs::Odometry &odom);
+        // 地图膨胀
+        void inflate_point_cloud(void);
+        // 判断当前点是否在地图内
+        bool isInMap(Eigen::Vector3d pos);
+        // 设置占据
+        void setOccupancy(Eigen::Vector3d &pos, int occ);
+        // 设置代价
+        void updateCostMap(Eigen::Vector3d &pos, double cost);
+        // 由位置计算索引
+        void posToIndex(Eigen::Vector3d &pos, Eigen::Vector3i &id);
+        // 由索引计算位置
+        void indexToPos(Eigen::Vector3i &id, Eigen::Vector3d &pos);
+        // 根据位置返回占据状态
+        int getOccupancy(Eigen::Vector3d &pos);
+        // 根据索引返回占据状态
+        int getOccupancy(Eigen::Vector3i &id);
+        // 根据索引返回代价
+        double getCost(Eigen::Vector3d &pos);
+        // 检查安全
+        bool check_safety(Eigen::Vector3d &pos, double check_distance /*, Eigen::Vector3d& map_point*/);
+        void pub_pcl_cb(const ros::TimerEvent &e);
+        // 定义该类的指针
+        typedef std::shared_ptr<Occupy_map> Ptr;
+    };
+
+
+
+#endif

src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_2dlidar.launch

@@ -0,0 +1,32 @@
+<launch>
+    <!-- 启动全局规划算法 -->
+    <node pkg="prometheus_global_planner" name="global_planner_main" type="global_planner_main" output="screen">
+        <!-- 参数 -->
+        <param name="uav_id" value="1"/>
+        <param name="global_planner/sim_mode" value="true"/>
+        <param name="global_planner/scan_topic_name" value="/scan"/>
+        <!-- 地图输入模式 0代表全局点云，1代表局部点云，2代表激光雷达scan数据 -->
+        <param name="global_planner/map_input_source" value="2"/>
+        <!-- 无人机飞行高度，建议与起飞高度一致 -->
+        <param name="global_planner/fly_height" value="1.5"/>
+        <!-- 路径追踪频率 -->
+        <param name="global_planner/time_per_path" value="0.5"/>
+        <!-- Astar重规划频率 -->       
+        <param name="global_planner/replan_time" value="5.0"/>
+        <!-- 地图参数 -->
+        <param name="map/border" value="true"/>
+        <param name="map/queue_size" value="-1"/>
+        <!-- 分辨率 -->
+        <param name="map/resolution" value="0.2"/>
+        <!-- 障碍物膨胀距离,建议为飞机的轴距1.5倍 -->
+        <param name="map/inflate" value="0.4"/>
+        <!-- 地图范围 -->
+        <param name="map/origin_x" value="-15.0"/>
+        <param name="map/origin_y" value="-15.0"/>
+        <param name="map/origin_z" value="-0.5"/>
+        <param name="map/map_size_x" value="30.0"/>
+        <param name="map/map_size_y" value="30.0"/>
+        <param name="map/map_size_z" value="3.0"/>
+    </node>
+</launch>
+

src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_global_point.launch

@@ -0,0 +1,32 @@
+<launch>
+    <!-- 启动全局规划算法 -->
+    <node pkg="prometheus_global_planner" name="global_planner_main" type="global_planner_main" output="screen">
+        <!-- 参数 -->
+        <param name="uav_id" value="1"/>
+        <param name="global_planner/sim_mode" value="true"/>
+        <param name="global_planner/global_pcl_topic_name" value="/map_generator/global_cloud"/>
+        <!-- 地图输入模式 0代表全局点云，1代表局部点云，2代表激光雷达scan数据 -->
+        <param name="global_planner/map_input_source" value="0"/>
+        <!-- 无人机飞行高度，建议与起飞高度一致 -->
+        <param name="global_planner/fly_height" value="1.5"/>
+        <!-- 路径追踪频率 -->
+        <param name="global_planner/time_per_path" value="0.5"/>
+        <!-- Astar重规划频率 -->       
+        <param name="global_planner/replan_time" value="100.0"/>
+        <!-- 地图参数 -->
+        <param name="map/border" value="true"/>
+        <param name="map/queue_size" value="-1"/>
+        <!-- 分辨率 -->
+        <param name="map/resolution" value="0.2"/>
+        <!-- 障碍物膨胀距离,建议为飞机的轴距1.5倍 -->
+        <param name="map/inflate" value="0.4"/>
+        <!-- 地图范围 -->
+        <param name="map/origin_x" value="-15.0"/>
+        <param name="map/origin_y" value="-15.0"/>
+        <param name="map/origin_z" value="-0.5"/>
+        <param name="map/map_size_x" value="30.0"/>
+        <param name="map/map_size_y" value="30.0"/>
+        <param name="map/map_size_z" value="3.0"/>
+    </node>
+</launch>
+

src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_local_point.launch

@@ -0,0 +1,32 @@
+<launch>
+    <!-- 启动全局规划算法 -->
+    <node pkg="prometheus_global_planner" name="global_planner_main" type="global_planner_main" output="screen">
+        <!-- 参数 -->
+        <param name="uav_id" value="1"/>
+        <param name="global_planner/sim_mode" value="true"/>
+        <param name="global_planner/local_pcl_topic_name" value="/map_generator/local_cloud"/>
+        <!-- 地图输入模式 0代表全局点云，1代表局部点云，2代表激光雷达scan数据 -->
+        <param name="global_planner/map_input_source" value="1"/>
+        <!-- 无人机飞行高度，建议与起飞高度一致 -->
+        <param name="global_planner/fly_height" value="1.5"/>
+        <!-- 路径追踪频率 -->
+        <param name="global_planner/time_per_path" value="1.0"/>
+        <!-- Astar重规划频率 -->       
+        <param name="global_planner/replan_time" value="5.0"/>
+        <!-- 地图参数 -->
+        <param name="map/border" value="true"/>
+        <param name="map/queue_size" value="5"/>
+        <!-- 分辨率 -->
+        <param name="map/resolution" value="0.2"/>
+        <!-- 障碍物膨胀距离,建议为飞机的轴距1.5倍 -->
+        <param name="map/inflate" value="0.4"/>
+        <!-- 地图范围 -->
+        <param name="map/origin_x" value="-15.0"/>
+        <param name="map/origin_y" value="-15.0"/>
+        <param name="map/origin_z" value="-0.5"/>
+        <param name="map/map_size_x" value="30.0"/>
+        <param name="map/map_size_y" value="30.0"/>
+        <param name="map/map_size_z" value="3.0"/>
+    </node>
+</launch>
+

src/Prometheus/Modules/motion_planning/global_planner/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>prometheus_global_planner</name>
+    <version>0.0.0</version>
+    <description>The prometheus_global_planner package</description>
+
+    <maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
+
+    <license>TODO</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+    <build_depend>roscpp</build_depend>
+    <build_depend>rospy</build_depend>
+    <build_depend>std_msgs</build_depend>
+    <build_depend>sensor_msgs</build_depend>
+    <build_depend>geometry_msgs</build_depend>
+    <build_depend>nav_msgs</build_depend>
+    <build_depend>prometheus_msgs</build_depend>
+    <build_depend>pcl_ros</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>sensor_msgs</build_export_depend>
+    <build_export_depend>geometry_msgs</build_export_depend>
+    <build_export_depend>nav_msgs</build_export_depend>
+    <build_export_depend>prometheus_msgs</build_export_depend>
+    <build_export_depend>pcl_ros</build_export_depend>
+    <exec_depend>roscpp</exec_depend>
+    <exec_depend>rospy</exec_depend>
+    <exec_depend>std_msgs</exec_depend>
+    <exec_depend>sensor_msgs</exec_depend>
+    <exec_depend>geometry_msgs</exec_depend>
+    <exec_depend>nav_msgs</exec_depend>
+    <exec_depend>prometheus_msgs</exec_depend>
+    <exec_depend>pcl_ros</exec_depend>
+
+    <export>
+        <!-- Other tools can request additional information be placed here -->
+    </export>
+</package>

src/Prometheus/Modules/motion_planning/global_planner/src/A_star.cpp

@@ -0,0 +1,368 @@
+#include "A_star.h"
+
+using namespace std;
+using namespace Eigen;
+
+Astar::~Astar()
+{
+  for (int i = 0; i < max_search_num; i++)
+  {
+    // delete表示释放堆内存
+    delete path_node_pool_[i];
+  }
+}
+
+void Astar::init(ros::NodeHandle &nh)
+{
+  // 【参数】1代表2D平面规划及搜索,0代表3D
+  nh.param("global_planner/is_2D", is_2D, true);         
+  // 【参数】无人机指定飞行高度
+  nh.param("global_planner/fly_height", fly_height, 1.0); 
+  // 【参数】规划搜索相关参数
+  nh.param("astar/lambda_heu", lambda_heu_, 2.0);         // 加速引导参数
+  nh.param("astar/lambda_cost", lambda_cost_, 300.0);     // 参数
+  nh.param("astar/allocate_num", max_search_num, 500000); //最大搜索节点数
+  nh.param("map/resolution", resolution_, 0.2);           // 地图分辨率
+
+  tie_breaker_ = 1.0 + 1.0 / max_search_num;
+
+  this->inv_resolution_ = 1.0 / resolution_;
+
+  path_node_pool_.resize(max_search_num);
+
+  // 新建
+  for (int i = 0; i < max_search_num; i++)
+  {
+    path_node_pool_[i] = new Node;
+  }
+
+  use_node_num_ = 0;
+  iter_num_ = 0;
+
+  // 初始化占据地图
+  Occupy_map_ptr.reset(new Occupy_map);
+  Occupy_map_ptr->init(nh);
+
+  // 读取地图参数
+  origin_ = Occupy_map_ptr->min_range_;
+  map_size_3d_ = Occupy_map_ptr->max_range_ - Occupy_map_ptr->min_range_;
+}
+
+void Astar::reset()
+{
+  // 重置与搜索相关的变量
+  expanded_nodes_.clear();
+  path_nodes_.clear();
+
+  std::priority_queue<NodePtr, std::vector<NodePtr>, NodeComparator0> empty_queue;
+  open_set_.swap(empty_queue);
+
+  for (int i = 0; i < use_node_num_; i++)
+  {
+    NodePtr node = path_node_pool_[i];
+    node->parent = NULL;
+    node->node_state = NOT_EXPAND;
+  }
+
+  use_node_num_ = 0;
+  iter_num_ = 0;
+}
+
+// 搜索函数，输入为：起始点及终点
+// 将传输的数组通通变为指针！！！！ 以后改
+int Astar::search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt)
+{
+  // 首先检查目标点是否可到达
+  if (Occupy_map_ptr->getOccupancy(end_pt))
+  {
+    cout << RED << "Astar search: [ Astar can't find path: goal point is occupied ]" << TAIL << endl;
+    return NO_PATH;
+  }
+
+  // 计时
+  ros::Time time_astar_start = ros::Time::now();
+
+  goal_pos = end_pt;
+  Eigen::Vector3i end_index = posToIndex(end_pt);
+
+  // 初始化,将起始点设为第一个路径点
+  NodePtr cur_node = path_node_pool_[0];
+  cur_node->parent = NULL;
+  cur_node->position = start_pt;
+  cur_node->index = posToIndex(start_pt);
+  cur_node->g_score = 0.0;
+  cur_node->f_score = lambda_heu_ * getEuclHeu(cur_node->position, end_pt);
+  cur_node->node_state = IN_OPEN_SET;
+
+  // 将当前点推入open set
+  open_set_.push(cur_node);
+  // 迭代次数+1
+  use_node_num_ += 1;
+  // 记录当前为已扩展
+  expanded_nodes_.insert(cur_node->index, cur_node);
+
+  NodePtr terminate_node = NULL;
+
+  // 搜索主循环
+  while (!open_set_.empty())
+  {
+    // 获取f_score最低的点
+    cur_node = open_set_.top();
+
+    // 判断终止条件
+    bool reach_end = abs(cur_node->index(0) - end_index(0)) <= 1 &&
+                     abs(cur_node->index(1) - end_index(1)) <= 1 &&
+                     abs(cur_node->index(2) - end_index(2)) <= 1;
+
+    if (reach_end)
+    {
+      // 将当前点设为终止点，并往回形成路径
+      terminate_node = cur_node;
+      retrievePath(terminate_node);
+
+      // 时间一般很短，远远小于膨胀点云的时间
+      // printf("Astar take time %f s. \n", (ros::Time::now() - time_astar_start).toSec());
+
+      return REACH_END;
+    }
+
+    /* ---------- pop node and add to close set ---------- */
+    open_set_.pop();
+    // 将当前点推入close set
+    cur_node->node_state = IN_CLOSE_SET; // in expand set
+    iter_num_ += 1;
+
+    /* ---------- init neighbor expansion ---------- */
+    Eigen::Vector3d cur_pos = cur_node->position;
+    Eigen::Vector3d expand_node_pos;
+
+    vector<Eigen::Vector3d> inputs;
+    Eigen::Vector3d d_pos;
+
+    /* ---------- expansion loop ---------- */
+    // 扩展： 3*3*3 - 1 = 26种可能
+    for (double dx = -resolution_; dx <= resolution_ + 1e-3; dx += resolution_)
+    {
+      for (double dy = -resolution_; dy <= resolution_ + 1e-3; dy += resolution_)
+      {
+        for (double dz = -resolution_; dz <= resolution_ + 1e-3; dz += resolution_)
+        {
+
+          d_pos << dx, dy, dz;
+
+          // 对于2d情况，不扩展z轴
+          if (is_2D)
+          {
+            d_pos(2) = 0.0;
+          }
+
+          // 跳过自己那个格子
+          if (d_pos.norm() < 1e-3)
+          {
+            continue;
+          }
+
+          // 扩展节点的位置
+          expand_node_pos = cur_pos + d_pos;
+
+          // 确认该点在地图范围内
+          if (!Occupy_map_ptr->isInMap(expand_node_pos))
+          {
+            continue;
+          }
+
+          // 计算扩展节点的index
+          Eigen::Vector3i d_pos_id;
+          d_pos_id << int(dx / resolution_), int(dy / resolution_), int(dz / resolution_);
+          Eigen::Vector3i expand_node_id = d_pos_id + cur_node->index;
+
+          //检查当前扩展的点是否在close set中，如果是则跳过
+          NodePtr expand_node = expanded_nodes_.find(expand_node_id);
+          if (expand_node != NULL && expand_node->node_state == IN_CLOSE_SET)
+          {
+            continue;
+          }
+
+          // 检查当前扩展点是否被占据,如果是则跳过
+          bool is_occupy = Occupy_map_ptr->getOccupancy(expand_node_pos);
+          if (is_occupy)
+          {
+            continue;
+          }
+
+          // 如果能通过上述检查则
+          double tmp_g_score, tmp_f_score;
+          tmp_g_score = d_pos.squaredNorm() + cur_node->g_score;
+          tmp_f_score = tmp_g_score + lambda_heu_ * getEuclHeu(expand_node_pos, end_pt) + lambda_cost_ * Occupy_map_ptr->getCost(expand_node_pos);
+
+          // 如果扩展的当前节点为NULL，即未扩展过
+          if (expand_node == NULL)
+          {
+            expand_node = path_node_pool_[use_node_num_];
+            expand_node->index = expand_node_id;
+            expand_node->position = expand_node_pos;
+            expand_node->f_score = tmp_f_score;
+            expand_node->g_score = tmp_g_score;
+            expand_node->parent = cur_node;
+            expand_node->node_state = IN_OPEN_SET;
+
+            open_set_.push(expand_node);
+            expanded_nodes_.insert(expand_node_id, expand_node);
+
+            use_node_num_ += 1;
+            // 超过最大搜索次数
+            if (use_node_num_ == max_search_num)
+            {
+              cout << RED << NODE_NAME << "Astar search: [ Astar can't find path: reach the max_search_num ]" << TAIL << endl;
+              return NO_PATH;
+            }
+          }
+          // 如果当前节点已被扩展过，则更新其状态
+          else if (expand_node->node_state == IN_OPEN_SET)
+          {
+            if (tmp_g_score < expand_node->g_score)
+            {
+              // expand_node->index = expand_node_id;
+              expand_node->position = expand_node_pos;
+              expand_node->f_score = tmp_f_score;
+              expand_node->g_score = tmp_g_score;
+              expand_node->parent = cur_node;
+            }
+          }
+        }
+      }
+    }
+  }
+
+  // 搜索完所有可行点，即使没达到最大搜索次数，也没有找到路径
+  // 这种一般是因为无人机周围被占据，或者无人机与目标点之间无可通行路径造成的
+  cout << RED << "Astar search: [ Astar can't find path: max_search_num: open set empty ]" << TAIL << endl;
+  return NO_PATH;
+}
+
+// 由最终点往回生成路径
+void Astar::retrievePath(NodePtr end_node)
+{
+  NodePtr cur_node = end_node;
+  path_nodes_.push_back(cur_node);
+
+  while (cur_node->parent != NULL)
+  {
+    cur_node = cur_node->parent;
+    path_nodes_.push_back(cur_node);
+  }
+
+  // 反转顺序
+  reverse(path_nodes_.begin(), path_nodes_.end());
+}
+
+std::vector<Eigen::Vector3d> Astar::getPath()
+{
+  vector<Eigen::Vector3d> path;
+  for (uint i = 0; i < path_nodes_.size(); ++i)
+  {
+    path.push_back(path_nodes_[i]->position);
+  }
+  path.push_back(goal_pos);
+  return path;
+}
+
+nav_msgs::Path Astar::get_ros_path()
+{
+  nav_msgs::Path path;
+
+  path.header.frame_id = "world";
+  path.header.stamp = ros::Time::now();
+  path.poses.clear();
+
+  geometry_msgs::PoseStamped path_i_pose;
+  for (uint i = 0; i < path_nodes_.size(); ++i)
+  {
+    path_i_pose.header.frame_id = "world";
+    path_i_pose.pose.position.x = path_nodes_[i]->position[0];
+    path_i_pose.pose.position.y = path_nodes_[i]->position[1];
+    path_i_pose.pose.position.z = path_nodes_[i]->position[2];
+    path.poses.push_back(path_i_pose);
+  }
+
+  path_i_pose.header.frame_id = "world";
+  path_i_pose.pose.position.x = goal_pos[0];
+  path_i_pose.pose.position.y = goal_pos[1];
+  path_i_pose.pose.position.z = goal_pos[2];
+  path.poses.push_back(path_i_pose);
+
+  return path;
+}
+
+double Astar::getDiagHeu(Eigen::Vector3d x1, Eigen::Vector3d x2)
+{
+  double dx = fabs(x1(0) - x2(0));
+  double dy = fabs(x1(1) - x2(1));
+  double dz = fabs(x1(2) - x2(2));
+
+  double h = 0;
+
+  int diag = min(min(dx, dy), dz);
+  dx -= diag;
+  dy -= diag;
+  dz -= diag;
+
+  if (dx < 1e-4)
+  {
+    h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dy, dz) + 1.0 * abs(dy - dz);
+  }
+  if (dy < 1e-4)
+  {
+    h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dz) + 1.0 * abs(dx - dz);
+  }
+  if (dz < 1e-4)
+  {
+    h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dy) + 1.0 * abs(dx - dy);
+  }
+
+  return tie_breaker_ * h;
+}
+
+double Astar::getManhHeu(Eigen::Vector3d x1, Eigen::Vector3d x2)
+{
+  double dx = fabs(x1(0) - x2(0));
+  double dy = fabs(x1(1) - x2(1));
+  double dz = fabs(x1(2) - x2(2));
+
+  return tie_breaker_ * (dx + dy + dz);
+}
+
+double Astar::getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2)
+{
+  return tie_breaker_ * (x2 - x1).norm();
+}
+
+std::vector<NodePtr> Astar::getVisitedNodes()
+{
+  vector<NodePtr> visited;
+  visited.assign(path_node_pool_.begin(), path_node_pool_.begin() + use_node_num_ - 1);
+  return visited;
+}
+
+Eigen::Vector3i Astar::posToIndex(Eigen::Vector3d pt)
+{
+  Vector3i idx;
+  idx << floor((pt(0) - origin_(0)) * inv_resolution_), floor((pt(1) - origin_(1)) * inv_resolution_),
+      floor((pt(2) - origin_(2)) * inv_resolution_);
+
+  return idx;
+}
+
+void Astar::indexToPos(Eigen::Vector3i id, Eigen::Vector3d &pos)
+{
+  for (int i = 0; i < 3; ++i)
+    pos(i) = (id(i) + 0.5) * resolution_ + origin_(i);
+}
+
+// 检查cur_pos是否安全
+bool Astar::check_safety(Eigen::Vector3d &cur_pos, double safe_distance)
+{
+  bool is_safety;
+  is_safety = Occupy_map_ptr->check_safety(cur_pos, safe_distance);
+  return is_safety;
+}

src/Prometheus/Modules/motion_planning/global_planner/src/global_planner.cpp

@@ -0,0 +1,461 @@
+#include "global_planner.h"
+
+// 初始化函数
+GlobalPlanner::GlobalPlanner(ros::NodeHandle &nh)
+{
+    // 【参数】无人机编号，从1开始编号
+    nh.param("uav_id", uav_id, 0);
+    // 【参数】是否为仿真模式
+    nh.param("global_planner/sim_mode", sim_mode, false);
+    // 【参数】选择地图更新方式: 0代表全局点云，1代表局部点云，2代表二维激光雷达
+    nh.param("global_planner/map_input_source", map_input_source, 0);
+    // 【参数】无人机指定飞行高度
+    nh.param("global_planner/fly_height", fly_height, 1.0);
+    // 【参数】无人机安全距离，若膨胀距离设置已考虑安全距离，建议此处设为0
+    nh.param("global_planner/safe_distance", safe_distance, 0.05);
+    // 【参数】路径追踪频率
+    nh.param("global_planner/time_per_path", time_per_path, 1.0);
+    // 【参数】Astar重规划频率
+    nh.param("global_planner/replan_time", replan_time, 2.0);
+
+    //【订阅】期望目标点
+    goal_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uav" + std::to_string(uav_id) + "/prometheus/motion_planning/goal",
+                                                        1,
+                                                        &GlobalPlanner::goal_cb, this);
+
+    //【订阅】无人机状态信息
+    uav_state_sub = nh.subscribe<prometheus_msgs::UAVState>("/uav" + std::to_string(uav_id) + "/prometheus/state",
+                                                            1,
+                                                            &GlobalPlanner::uav_state_cb, this);
+
+    //【订阅】无人机控制状态
+    uav_control_state_sub = nh.subscribe<prometheus_msgs::UAVControlState>("/uav" + std::to_string(uav_id) + "/prometheus/control_state",
+                                                                           1,
+                                                                           &GlobalPlanner::uav_control_state_cb, this);
+
+    uav_name = "/uav" + std::to_string(uav_id);
+    //【订阅】根据map_input_source选择地图更新方式
+    if (map_input_source == 0)
+    {
+        nh.getParam("global_planner/global_pcl_topic_name", global_pcl_topic_name);
+        cout << GREEN << "Global pcl mode, subscirbe to " << global_pcl_topic_name << TAIL << endl;
+        Gpointcloud_sub = nh.subscribe<sensor_msgs::PointCloud2>(global_pcl_topic_name.c_str(), 1, &GlobalPlanner::Gpointcloud_cb, this);
+    }
+    else if (map_input_source == 1)
+    {
+        nh.getParam("global_planner/local_pcl_topic_name", local_pcl_topic_name);
+        cout << GREEN << "Local pcl mode, subscirbe to " << uav_name << local_pcl_topic_name << TAIL << endl;
+        Lpointcloud_sub = nh.subscribe<sensor_msgs::PointCloud2>("/uav" + std::to_string(uav_id) + local_pcl_topic_name, 1, &GlobalPlanner::Lpointcloud_cb, this);
+    }
+    else if (map_input_source == 2)
+    {
+        nh.getParam("global_planner/scan_topic_name", scan_topic_name);
+        cout << GREEN << "Laser scan mode, subscirbe to " << uav_name << scan_topic_name << TAIL << endl;
+        laserscan_sub = nh.subscribe<sensor_msgs::LaserScan>("/uav" + std::to_string(uav_id) + scan_topic_name, 1, &GlobalPlanner::laser_cb, this);
+    }
+
+    // 【发布】控制指令
+    uav_cmd_pub = nh.advertise<prometheus_msgs::UAVCommand>("/uav" + std::to_string(uav_id) + "/prometheus/command", 1);
+    // 【发布】发布路径用于显示
+    path_cmd_pub = nh.advertise<nav_msgs::Path>("/uav" + std::to_string(uav_id) + "/prometheus/global_planner/path_cmd", 1);
+    // 【定时器】安全检测
+    // safety_timer = nh.createTimer(ros::Duration(2.0), &GlobalPlanner::safety_cb, this);
+    // 【定时器】主循环
+    mainloop_timer = nh.createTimer(ros::Duration(1.0), &GlobalPlanner::mainloop_cb, this);
+    // 【定时器】路径追踪循环，快速移动场景应当适当提高执行频率
+    track_path_timer = nh.createTimer(ros::Duration(time_per_path), &GlobalPlanner::track_path_cb, this);
+
+    // 【初始化】Astar算法
+    Astar_ptr.reset(new Astar);
+    Astar_ptr->init(nh);
+    cout << GREEN << NODE_NAME << "A_star init. " << TAIL << endl;
+
+    // 规划器状态参数初始化
+    exec_state = EXEC_STATE::WAIT_GOAL;
+    odom_ready = false;
+    drone_ready = false;
+    goal_ready = false;
+    sensor_ready = false;
+    is_safety = true;
+    is_new_path = false;
+
+    // 初始化发布的指令
+    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover;
+    uav_command.position_ref[0] = 0;
+    uav_command.position_ref[1] = 0;
+    uav_command.position_ref[2] = 0;
+    uav_command.yaw_ref = 0;
+    desired_yaw = 0.0;
+}
+void GlobalPlanner::debug_info()
+{
+    //固定的浮点显示
+    cout.setf(ios::fixed);
+    // setprecision(n) 设显示小数精度为n位
+    cout << setprecision(2);
+    //左对齐
+    cout.setf(ios::left);
+    // 强制显示小数点
+    cout.setf(ios::showpoint);
+    // 强制显示符号
+    cout.setf(ios::showpos);
+    cout << GREEN << "--------------> Global Planner <------------- " << TAIL << endl;
+    cout << GREEN << "[ ID: " << uav_id << "]  " << TAIL;
+    if (drone_ready)
+    {
+        cout << GREEN << "[ drone ready ]  " << TAIL << endl;
+    }
+    else
+    {
+        cout << RED << "[ drone not ready ]  " << TAIL << endl;
+    }
+
+    if (odom_ready)
+    {
+        cout << GREEN << "[ odom ready ]  " << TAIL << endl;
+    }
+    else
+    {
+        cout << RED << "[ odom not ready ]  " << TAIL << endl;
+    }
+
+    if (sensor_ready)
+    {
+        cout << GREEN << "[ sensor ready ]  " << TAIL << endl;
+    }
+    else
+    {
+        cout << RED << "[ sensor not ready ]  " << TAIL << endl;
+    }
+
+    if (exec_state == EXEC_STATE::WAIT_GOAL)
+    {
+        cout << GREEN << "[ WAIT_GOAL ] " << TAIL << endl;
+        if (uav_control_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL)
+        {
+            cout << YELLOW << "Please switch to COMMAND_CONTROL mode." << TAIL << endl;
+        }
+        if (!goal_ready)
+        {
+            cout << YELLOW << "Waiting for a new goal." << TAIL << endl;
+        }
+    }
+    else if (exec_state == EXEC_STATE::PLANNING)
+    {
+        cout << GREEN << "[ PLANNING ] " << TAIL << endl;
+    }
+    else if (exec_state == EXEC_STATE::TRACKING)
+    {
+        cout << GREEN << "[ TRACKING ] " << TAIL << endl;
+        cout << GREEN << "---->distance_to_goal:" << distance_to_goal << TAIL << endl;
+    }
+    else if (exec_state == EXEC_STATE::LANDING)
+    {
+        cout << GREEN << "[ LANDING ] " << TAIL << endl;
+    }
+}
+// 主循环
+void GlobalPlanner::mainloop_cb(const ros::TimerEvent &e)
+{
+    static int exec_num = 0;
+    exec_num++;
+
+    if (exec_num == 5)
+    {
+        debug_info();
+        exec_num = 0;
+    }
+
+    // 检查当前状态，不满足规划条件则直接退出主循环
+    if (!odom_ready || !drone_ready || !sensor_ready)
+    {
+        return;
+    }
+    else
+    {
+        // 对检查的状态进行重置
+        odom_ready = false;
+        drone_ready = false;
+        sensor_ready = false;
+    }
+
+    switch (exec_state)
+    {
+    case EXEC_STATE::WAIT_GOAL:
+        path_ok = false;
+
+        // 保持到指定高度
+        if (abs(fly_height - uav_pos[2]) > MIN_DIS)
+        {
+            uav_command.header.stamp = ros::Time::now();
+            uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
+            uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS;
+            uav_command.position_ref[0] = uav_pos[0];
+            uav_command.position_ref[1] = uav_pos[1];
+            uav_command.position_ref[2] = fly_height;
+            uav_command.yaw_ref = 0;
+            uav_command.Command_ID = uav_command.Command_ID + 1;
+            uav_cmd_pub.publish(uav_command);
+        }
+        else if (goal_ready)
+        {
+            // 获取到目标点后，生成新轨迹
+            exec_state = EXEC_STATE::PLANNING;
+            goal_ready = false;
+        }
+
+        break;
+    case EXEC_STATE::PLANNING:
+        // 重置规划器
+        Astar_ptr->reset();
+        // 使用规划器执行搜索，返回搜索结果
+        int astar_state;
+        astar_state = Astar_ptr->search(uav_pos, goal_pos);
+
+        // 未寻找到路径
+        if (astar_state == Astar::NO_PATH)
+        {
+            path_ok = false;
+            exec_state = EXEC_STATE::WAIT_GOAL;
+            cout << RED << NODE_NAME << " Planner can't find path!" << TAIL << endl;
+        }
+        else
+        {
+            path_ok = true;
+            is_new_path = true;
+            path_cmd = Astar_ptr->get_ros_path();
+            Num_total_wp = path_cmd.poses.size();
+            start_point_index = get_start_point_id();
+            cur_id = start_point_index;
+            last_replan_time = ros::Time::now();
+            exec_state = EXEC_STATE::TRACKING;
+            path_cmd_pub.publish(path_cmd);
+            cout << GREEN << NODE_NAME << " Get a new path!" << TAIL << endl;
+        }
+
+        break;
+
+    case EXEC_STATE::TRACKING:
+    {   
+        if ( (ros::Time::now()-last_replan_time).toSec() >= replan_time)
+        {
+            exec_state = EXEC_STATE::PLANNING;
+        }
+
+        break;
+    }
+    case EXEC_STATE::LANDING:
+    {
+        uav_command.header.stamp = ros::Time::now();
+        uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land;
+        uav_command.Command_ID = uav_command.Command_ID + 1;
+
+        uav_cmd_pub.publish(uav_command);
+        break;
+    }
+    }
+}
+
+void GlobalPlanner::goal_cb(const geometry_msgs::PoseStampedConstPtr &msg)
+{
+    // 2D定高飞行
+    goal_pos << msg->pose.position.x, msg->pose.position.y, fly_height;
+    goal_vel.setZero();
+    goal_ready = true;
+    exec_state = EXEC_STATE::WAIT_GOAL;
+
+    cout << GREEN << NODE_NAME << " Get a new goal point:" << goal_pos(0) << " [m] " << goal_pos(1) << " [m] " << goal_pos(2) << " [m] " << TAIL << endl;
+
+    if (goal_pos(0) == 99 && goal_pos(1) == 99)
+    {
+        path_ok = false;
+        goal_ready = false;
+        exec_state = EXEC_STATE::LANDING;
+        cout << GREEN << NODE_NAME << " Land " << TAIL << endl;
+    }
+}
+
+//无人机控制状态回调函数
+void GlobalPlanner::uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg)
+{
+    uav_control_state = *msg;
+}
+
+void GlobalPlanner::uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg)
+{
+    uav_state = *msg;
+
+    if (uav_state.connected == true && uav_state.armed == true)
+    {
+        drone_ready = true;
+    }
+    else
+    {
+        drone_ready = false;
+    }
+
+    if (uav_state.odom_valid)
+    {
+        odom_ready = true;
+    }
+    else
+    {
+        odom_ready = false;
+    }
+
+    if (abs(fly_height - msg->position[2]) > 0.2)
+    {
+        PCOUT(2, YELLOW, "UAV is not in the desired height.");
+    }
+
+    uav_odom.header = uav_state.header;
+    uav_odom.child_frame_id = "base_link";
+    uav_odom.pose.pose.position.x = uav_state.position[0];
+    uav_odom.pose.pose.position.y = uav_state.position[1];
+    uav_odom.pose.pose.position.z = fly_height;
+    uav_odom.pose.pose.orientation = uav_state.attitude_q;
+    uav_odom.twist.twist.linear.x = uav_state.velocity[0];
+    uav_odom.twist.twist.linear.y = uav_state.velocity[1];
+    uav_odom.twist.twist.linear.z = uav_state.velocity[2];
+
+    uav_pos = Eigen::Vector3d(msg->position[0], msg->position[1], fly_height);
+    uav_vel = Eigen::Vector3d(msg->velocity[0], msg->velocity[1], msg->velocity[2]);
+    uav_yaw = msg->attitude[2];
+}
+
+// 根据全局点云更新地图
+// 情况：已知全局点云的场景、由SLAM实时获取的全局点云
+void GlobalPlanner::Gpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg)
+{
+    if (!odom_ready)
+    {
+        return;
+    }
+    sensor_ready = true;
+    // 因为全局点云一般较大，只更新一次
+    if (!Astar_ptr->Occupy_map_ptr->get_gpcl)
+    {
+        // 对Astar中的地图进行更新
+        Astar_ptr->Occupy_map_ptr->map_update_gpcl(msg);
+    }
+}
+
+// 根据局部点云更新地图
+// 情况：RGBD相机、三维激光雷达
+void GlobalPlanner::Lpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg)
+{
+    if (!odom_ready)
+    {
+        return;
+    }
+    sensor_ready = true;
+    // 对Astar中的地图(局部点云+odom)进行更新
+    Astar_ptr->Occupy_map_ptr->map_update_lpcl(msg, uav_odom);
+}
+
+// 根据2维雷达数据更新地图
+// 情况：2维激光雷达
+void GlobalPlanner::laser_cb(const sensor_msgs::LaserScanConstPtr &msg)
+{
+    if (!odom_ready)
+    {
+        return;
+    }
+    sensor_ready = true;
+    // 对Astar中的地图进行更新(laserscan+odom)并对地图进行膨胀
+    Astar_ptr->Occupy_map_ptr->map_update_laser(msg, uav_odom);
+}
+
+void GlobalPlanner::track_path_cb(const ros::TimerEvent &e)
+{
+    if (!path_ok)
+    {
+        return;
+    }
+
+    is_new_path = false;
+    distance_to_goal = (uav_pos - goal_pos).norm();
+    // 抵达终点
+    if (cur_id == Num_total_wp - 1 || distance_to_goal < 0.2)
+    {
+        uav_command.header.stamp = ros::Time::now();
+        uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
+        uav_command.Command_ID = uav_command.Command_ID + 1;
+
+        uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS;
+        uav_command.position_ref[0] = goal_pos[0];
+        uav_command.position_ref[1] = goal_pos[1];
+        uav_command.position_ref[2] = goal_pos[2];
+
+        uav_command.yaw_ref = desired_yaw;
+        uav_cmd_pub.publish(uav_command);
+
+        cout << GREEN << NODE_NAME << "Reach the goal! " << TAIL << endl;
+        // 停止执行
+        path_ok = false;
+        // 转换状态为等待目标
+        exec_state = EXEC_STATE::WAIT_GOAL;
+        return;
+    }
+
+    cout << "Moving to Waypoint: [ " << cur_id << " / " << Num_total_wp << " ] " << endl;
+    cout << "Moving to Waypoint: "
+         << path_cmd.poses[cur_id].pose.position.x << " [m] "
+         << path_cmd.poses[cur_id].pose.position.y << " [m] "
+         << path_cmd.poses[cur_id].pose.position.z << " [m] " << endl;
+
+    // 追踪一条Astar算法给出的路径有几种方式:
+    // 1,控制方式如果是走航点，则需要对无人机进行限速，保证无人机的平滑移动
+    // 2,采用轨迹控制的方式进行追踪，期望速度 = （期望位置 - 当前位置）/预计时间；
+    uav_command.header.stamp = ros::Time::now();
+    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
+    uav_command.Command_ID = uav_command.Command_ID + 1;
+
+    uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS;
+    uav_command.position_ref[0] = path_cmd.poses[cur_id].pose.position.x;
+    uav_command.position_ref[1] = path_cmd.poses[cur_id].pose.position.y;
+    uav_command.position_ref[2] = path_cmd.poses[cur_id].pose.position.z;
+    // uav_command.velocity_ref[0] = (path_cmd.poses[cur_id].pose.position.x - uav_pos[0]) / time_per_path;
+    // uav_command.velocity_ref[1] = (path_cmd.poses[cur_id].pose.position.y - uav_pos[1]) / time_per_path;
+    // uav_command.velocity_ref[2] = (path_cmd.poses[cur_id].pose.position.z - uav_pos[2]) / time_per_path;
+    uav_command.yaw_ref = desired_yaw;
+
+    uav_cmd_pub.publish(uav_command);
+
+    cur_id = cur_id + 1;
+}
+
+void GlobalPlanner::safety_cb(const ros::TimerEvent &e)
+{
+    Eigen::Vector3d cur_pos(uav_pos[0], uav_pos[1], uav_pos[2]);
+
+    is_safety = Astar_ptr->check_safety(cur_pos, safe_distance);
+}
+
+int GlobalPlanner::get_start_point_id(void)
+{
+    // 选择与当前无人机所在位置最近的点,并从该点开始追踪
+    int id = 0;
+    float distance_to_wp_min = abs(path_cmd.poses[0].pose.position.x - uav_state.position[0]) + abs(path_cmd.poses[0].pose.position.y - uav_state.position[1]) + abs(path_cmd.poses[0].pose.position.z - uav_state.position[2]);
+
+    float distance_to_wp;
+
+    for (int j = 1; j < Num_total_wp; j++)
+    {
+        distance_to_wp = abs(path_cmd.poses[j].pose.position.x - uav_state.position[0]) + abs(path_cmd.poses[j].pose.position.y - uav_state.position[1]) + abs(path_cmd.poses[j].pose.position.z - uav_state.position[2]);
+
+        if (distance_to_wp < distance_to_wp_min)
+        {
+            distance_to_wp_min = distance_to_wp;
+            id = j;
+        }
+    }
+
+    //　为防止出现回头的情况，此处对航点进行前馈处理
+    if (id + 2 < Num_total_wp)
+    {
+        id = id + 2;
+    }
+
+    return id;
+}

src/Prometheus/Modules/motion_planning/global_planner/src/global_planner_node.cpp

@@ -0,0 +1,28 @@
+#include <ros/ros.h>
+#include <signal.h>
+
+#include "global_planner.h"
+
+;
+
+void mySigintHandler(int sig)
+{
+  ROS_INFO("[global_planner_node] exit...");
+  ros::shutdown();
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "global_planner");
+  ros::NodeHandle nh("~");
+
+  signal(SIGINT, mySigintHandler);
+  ros::Duration(1.0).sleep();
+
+  GlobalPlanner global_planner(nh);
+
+  ros::spin();
+
+  return 0;
+}
+

src/Prometheus/Modules/motion_planning/global_planner/src/occupy_map.cpp

@@ -0,0 +1,601 @@
+#include <occupy_map.h>
+
+// 初始化函数
+void Occupy_map::init(ros::NodeHandle &nh)
+{
+    // 【参数】无人机编号，从1开始编号
+    nh.param("uav_id", uav_id, 0);
+    // 【参数】是否为仿真模式
+    nh.param("global_planner/sim_mode", sim_mode, true);
+    // 【参数】无人机指定飞行高度
+    nh.param("global_planner/fly_height", fly_height, 1.0);
+    // 【参数】集群数量
+    nh.param("global_planner/swarm_num_uav", swarm_num_uav, 1);
+    // 【参数】其他无人机膨胀距离
+    nh.param("global_planner/odom_inflate", odom_inflate_, 0.6);
+    nh.param("global_planner/cost_inflate", cost_inflate, 5);
+    // 【参数】地图原点
+    nh.param("map/origin_x", origin_(0), -5.0);
+    nh.param("map/origin_y", origin_(1), -5.0);
+    nh.param("map/origin_z", origin_(2), -0.5);
+    // 【参数】地图实际尺寸，单位：米
+    nh.param("map/map_size_x", map_size_3d_(0), 10.0);
+    nh.param("map/map_size_y", map_size_3d_(1), 10.0);
+    nh.param("map/map_size_z", map_size_3d_(2), 2.0);
+    // 【参数】地图滑窗尺寸，-1代表unlimited
+    nh.param("map/queue_size", queue_size, -1);
+    // 【参数】首否在rviz中显示地图边界
+    nh.param("map/border", show_border, false);
+    // 【参数】地图分辨率，单位：米
+    nh.param("map/resolution", resolution_, 0.2);
+    // 【参数】地图膨胀距离，单位：米
+    nh.param("map/inflate", inflate_, 0.3);
+
+    uav_name = "/uav" + std::to_string(uav_id);
+    // 【发布】全局点云地图
+    global_pcl_pub = nh.advertise<sensor_msgs::PointCloud2>(uav_name + "/prometheus/global_planning/global_pcl", 1);
+    // 【发布】膨胀后的全局点云地图
+    inflate_pcl_pub = nh.advertise<sensor_msgs::PointCloud2>(uav_name + "/prometheus/global_planning/global_inflate_pcl", 1);
+    // 【定时器】地图发布定时器
+    pcl_pub = nh.createTimer(ros::Duration(0.2), &Occupy_map::pub_pcl_cb, this);
+
+    // 全局地图点云指针（环境）
+    global_point_cloud_map.reset(new pcl::PointCloud<pcl::PointXYZ>);
+    // 全局地图点云指针（其他无人机）
+    global_uav_pcl.reset(new pcl::PointCloud<pcl::PointXYZ>);
+    // 膨胀点云指针
+    cloud_inflate_vis_.reset(new pcl::PointCloud<pcl::PointXYZ>);
+    // 传入点云指针（临时指针）
+    input_point_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
+    // tf变换后点云指针（临时指针）
+    transformed_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
+    // 过滤后点云指针（临时指针）
+    pcl_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>);
+    // 局部地图滑窗指示器
+    st_it = 0;
+    // 存储的上一帧odom
+    f_x = f_y = f_z = f_pitch = f_yaw = f_roll = 0.0;
+
+    this->inv_resolution_ = 1.0 / resolution_;
+    for (int i = 0; i < 3; ++i)
+    {
+        // 占据图尺寸 = 地图尺寸 / 分辨率
+        grid_size_(i) = ceil(map_size_3d_(i) / resolution_);
+    }
+
+    // 占据容器的大小 = 占据图尺寸 x*y*z
+    occupancy_buffer_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2));
+    cost_map_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2));
+    fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0);
+    fill(cost_map_.begin(), cost_map_.end(), 0.0);
+
+    min_range_ = origin_;
+    max_range_ = origin_ + map_size_3d_;
+
+    min_range_(2) = fly_height - 2 * resolution_;
+    max_range_(2) = fly_height + 2 * resolution_;
+    get_gpcl = false;
+    get_lpcl = false;
+    get_laser = false;
+    // 生成地图边界：点云形式
+    double dist = 0.1;                                      //每多少距离一个点
+    int numdist_x = (max_range_(0) - min_range_(0)) / dist; // x的点数
+    int numdist_y = (max_range_(1) - min_range_(1)) / dist; // y的点数
+    int numdist = 2 * (numdist_x + numdist_y);              //总点数
+    border.width = numdist;
+    border.height = 1;
+    border.points.resize(numdist);
+
+    inflate_index_uav = 0;
+    ifn = ceil(odom_inflate_ * inv_resolution_);
+    for (int x = -ifn; x <= ifn; x++)
+        for (int y = -ifn; y <= ifn;)
+        {
+            enum_p_uav[inflate_index_uav++] << x * resolution_, y * resolution_, 0.0;
+            if (x == -ifn || x == ifn)
+                y++;
+            else
+                y += 2 * ifn;
+        }
+
+    for (int x = -ifn - 1; x <= ifn + 1; x++)
+        for (int y = -ifn - 1; y <= ifn + 1;)
+        {
+            enum_p_uav[inflate_index_uav++] << x * resolution_, y * resolution_, 0.0;
+            if (x == -ifn - 1 || x == ifn + 1)
+                y++;
+            else
+                y += 2 * ifn + 2;
+        }
+
+    // 膨胀格子数 = 膨胀距离/分辨率
+    // ceil返回大于或者等于指定表达式的最小整数
+    ifn = ceil(inflate_ * inv_resolution_);
+
+    inflate_index = 0;
+    for (int x = -ifn; x <= ifn; x++)
+        for (int y = -ifn; y <= ifn;)
+        {
+            enum_p[inflate_index++] << x * resolution_, y * resolution_, 0.0;
+            if (x == -ifn || x == ifn)
+                y++;
+            else
+                y += 2 * ifn;
+        }
+
+    cost_index = 0;
+    for (int x = -ifn - cost_inflate; x <= ifn + cost_inflate; x++)
+        for (int y = -ifn - cost_inflate; y <= ifn + cost_inflate;)
+        {
+            int tmp_dis = x * x + y * y;
+            if (tmp_dis <= (ifn + cost_inflate) * (ifn + cost_inflate))
+            {
+                enum_p_cost[cost_index++] << x * resolution_, y * resolution_, tmp_dis;
+            }
+            if (x == -ifn - cost_inflate || x == ifn + cost_inflate)
+                y++;
+            else
+                y += 2 * ifn + 2 * cost_inflate;
+        }
+    // printf("cost map : %d %d\n", cost_inflate, cost_index);
+
+    for (int i = 0; i < numdist_x; i++) // x边界
+    {
+        border.points[i].x = min_range_(0) + i * dist;
+        border.points[i].y = min_range_(1);
+        border.points[i].z = min_range_(2);
+
+        border.points[i + numdist_x].x = min_range_(0) + i * dist;
+        border.points[i + numdist_x].y = max_range_(1);
+        border.points[i + numdist_x].z = min_range_(2);
+    }
+
+    for (int i = 0; i < numdist_y; i++) // y边界
+    {
+        border.points[i + 2 * numdist_x].x = min_range_(0);
+        border.points[i + 2 * numdist_x].y = min_range_(1) + i * dist;
+        border.points[i + 2 * numdist_x].z = min_range_(2);
+
+        border.points[i + 2 * numdist_x + numdist_y].x = max_range_(0);
+        border.points[i + 2 * numdist_x + numdist_y].y = min_range_(1) + i * dist;
+        border.points[i + 2 * numdist_x + numdist_y].z = min_range_(2);
+    }
+}
+
+// 地图更新函数 - 输入：全局点云
+void Occupy_map::map_update_gpcl(const sensor_msgs::PointCloud2ConstPtr &global_point)
+{
+    // 全局地图只更新一次
+    if (get_gpcl)
+    {
+        return;
+    }
+
+    get_gpcl = true;
+    pcl::fromROSMsg(*global_point, *input_point_cloud);
+    global_point_cloud_map = input_point_cloud;
+    inflate_point_cloud();
+}
+
+// 地图更新函数 - 输入：局部点云
+void Occupy_map::map_update_lpcl(const sensor_msgs::PointCloud2ConstPtr &local_point, const nav_msgs::Odometry &odom)
+{
+    // 由sensor_msgs::PointCloud2 转为 pcl::PointCloud<pcl::PointXYZ>
+    pcl::fromROSMsg(*local_point, *input_point_cloud);
+    
+    // 仿真中的点云为全局点云,无需tf变换
+    if (sim_mode)
+    {
+        if (queue_size <= 0) // without slide windows
+        {
+            // map_generator生成的点云为world坐标系
+            *global_point_cloud_map += *input_point_cloud;
+        }
+        else // with slide windows
+        {
+            // slide windows with size: $queue_size
+            point_cloud_pair[st_it] = *input_point_cloud; // 加入新点云到滑窗
+            st_it = (st_it + 1) % queue_size;             // 指向下一个移除的点云位置
+
+            // 累计局部地图：需要20个加法，O（1）内存；增量式：需要19个加法，O（1.5）内存
+            global_point_cloud_map.reset(new pcl::PointCloud<pcl::PointXYZ>);
+            map<int, pcl::PointCloud<pcl::PointXYZ>>::iterator iter;
+            for (iter = point_cloud_pair.begin(); iter != point_cloud_pair.end(); iter++)
+            {
+                *global_point_cloud_map += iter->second;
+            }
+        }
+
+        // downsample
+        *pcl_ptr = *global_point_cloud_map;
+        vg.setInputCloud(pcl_ptr);
+        vg.setLeafSize(0.05f, 0.05f, 0.05f); // 下采样叶子节点大小（3D容器）
+        vg.filter(*global_point_cloud_map);
+        inflate_point_cloud();
+    }
+    else
+    {
+        // 实际中的点云需要先进行tf变换
+        local_map_merge_odom(odom);
+    }
+}
+
+// 地图更新函数 - 输入：laser
+void Occupy_map::map_update_laser(const sensor_msgs::LaserScanConstPtr &local_point, const nav_msgs::Odometry &odom)
+{
+    // 参考网页:http://wiki.ros.org/laser_geometry
+    // sensor_msgs::LaserScan 转为 sensor_msgs::PointCloud2 格式
+    projector_.projectLaser(*local_point, input_laser_scan);
+    // 再由sensor_msgs::PointCloud2 转为 pcl::PointCloud<pcl::PointXYZ>
+    pcl::fromROSMsg(input_laser_scan, *input_point_cloud);
+
+    local_map_merge_odom(odom);
+}
+
+// 工具函数：合并局部地图 - 输入：odom以及局部点云
+void Occupy_map::local_map_merge_odom(const nav_msgs::Odometry &odom)
+{
+    // 从odom中取得6DOF
+    double x, y, z, roll, pitch, yaw;
+    // 平移（xyz）
+    x = odom.pose.pose.position.x;
+    y = odom.pose.pose.position.y;
+    z = odom.pose.pose.position.z;
+    // 旋转（从四元数到欧拉角）
+    tf::Quaternion orientation;
+    tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
+    tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
+
+    // uav is moving
+    bool pos_change = (abs(x - f_x) > 0.1) || (abs(y - f_y) > 0.1);
+    // update map even though uav doesn't move
+    static int update_num = 0;
+    update_num++;
+
+    // merge local points to local map
+    if (pos_change || global_point_cloud_map == nullptr || update_num > 1)
+    {
+        update_num = 0;
+
+        // accumulate pointcloud according to odom
+        pcl::transformPointCloud(*input_point_cloud, *transformed_cloud, pcl::getTransformation(x, y, z, 0.0, 0.0, yaw));
+        if (queue_size <= 0) // without slide windows
+        {
+            *transformed_cloud += *global_point_cloud_map;
+        }
+        else // with slide windows
+        {
+            // slide windows with size: $queue_size
+            point_cloud_pair[st_it] = *transformed_cloud; // 加入新点云到滑窗
+            st_it = (st_it + 1) % queue_size;             // 指向下一个移除的点云位置
+
+            // 累计局部地图：需要20个加法，O（1）内存；增量式：需要19个加法，O（1.5）内存
+            transformed_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
+            map<int, pcl::PointCloud<pcl::PointXYZ>>::iterator iter;
+            for (iter = point_cloud_pair.begin(); iter != point_cloud_pair.end(); iter++)
+            {
+                *transformed_cloud += iter->second;
+            }
+        }
+
+        // downsample
+        vg.setInputCloud(transformed_cloud);
+        vg.setLeafSize(0.2f, 0.2f, 0.2f); // 下采样叶子节点大小（3D容器）
+        vg.filter(*global_point_cloud_map);
+
+        // store last odom data
+        f_x = x;
+        f_y = y;
+        f_z = z;
+        f_roll = roll;
+        f_pitch = pitch;
+        f_yaw = yaw;
+        inflate_point_cloud();
+    }
+}
+
+// function: update global uav occupy grid (10Hz, defined by fsm)
+void Occupy_map::uav_pcl_update(Eigen::Vector3d *input_uav_odom, bool *get_uav_odom)
+{
+    Eigen::Vector3d p3d_inf;
+
+    // update global uav occupy grid with input uav odom
+    pcl::PointXYZ pt;
+    global_uav_pcl.reset(new pcl::PointCloud<pcl::PointXYZ>);
+
+    for (int i = 1; i <= swarm_num_uav; i++)
+    {
+        if (i == uav_id)
+        {
+            continue;
+        }
+
+        if (get_uav_odom[i])
+            for (int j = 0; j < inflate_index_uav; j++)
+            {
+                pt.x = input_uav_odom[i][0] + enum_p_uav[j](0);
+                pt.y = input_uav_odom[i][1] + enum_p_uav[j](1);
+                pt.z = input_uav_odom[i][2] + enum_p_uav[j](2);
+                //　在global_uav_pcl中添加膨胀点
+                global_uav_pcl->points.push_back(pt);
+            }
+    }
+    global_uav_pcl->width = global_uav_pcl->points.size();
+    global_uav_pcl->height = 1;
+    global_uav_pcl->is_dense = true;
+}
+
+// 当global_planning节点接收到点云消息更新时，进行设置点云指针并膨胀
+// Astar规划路径时，采用的是此处膨胀后的点云（setOccupancy只在本函数中使用）
+void Occupy_map::inflate_point_cloud(void)
+{
+    if (get_gpcl)
+    {
+        // occupancy_buffer_清零
+        fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0);
+        fill(cost_map_.begin(), cost_map_.end(), 0.0);
+    }
+    else if (queue_size > 0)
+    {
+        // queue_size设置为大于0时，代表仅使用过去一定数量的点云
+        fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0);
+        fill(cost_map_.begin(), cost_map_.end(), 0.0);
+    }
+
+    //记录开始时间
+    ros::Time time_start = ros::Time::now();
+
+    // 转化为PCL的格式进行处理
+    pcl::PointCloud<pcl::PointXYZ> latest_global_cloud_ = *global_point_cloud_map;
+
+    if ((int)latest_global_cloud_.points.size() == 0)
+    {
+        return;
+    }
+
+    cloud_inflate_vis_->clear();
+
+    pcl::PointXYZ pt_inf;
+    Eigen::Vector3d p3d, p3d_inf, p3d_cost;
+
+    //　无人机占据地图更新
+    for (int i = 0; i < global_uav_pcl->points.size(); i++)
+    {
+        p3d_inf(0) = global_uav_pcl->points[i].x;
+        p3d_inf(1) = global_uav_pcl->points[i].y;
+        p3d_inf(2) = global_uav_pcl->points[i].z;
+        this->setOccupancy(p3d_inf, 1); // set to 1
+    }
+
+    // 遍历环境全局点云中的所有点
+    for (size_t i = 0; i < latest_global_cloud_.points.size(); ++i)
+    {
+        // 取出第i个点
+        p3d(0) = latest_global_cloud_.points[i].x;
+        p3d(1) = latest_global_cloud_.points[i].y;
+        p3d(2) = latest_global_cloud_.points[i].z;
+
+        // 若取出的点不在地图内（膨胀时只考虑地图范围内的点）
+        if (!isInMap(p3d))
+        {
+            continue;
+        }
+
+        // cost map update
+        for (int j = 0; j < cost_index; j++)
+        {
+            p3d_cost(0) = p3d(0) + enum_p_cost[j](0);
+            p3d_cost(1) = p3d(1) + enum_p_cost[j](1);
+            p3d_cost(2) = p3d(2);
+            this->updateCostMap(p3d_cost, 1.0 / enum_p_cost[j](2));
+        }
+
+        // 根据膨胀距离，膨胀该点
+        for (int i = 0; i < inflate_index; i++)
+        {
+            p3d_inf(0) = p3d(0) + enum_p[i](0);
+            p3d_inf(1) = p3d(1) + enum_p[i](1);
+            p3d_inf(2) = p3d(2) + enum_p[i](2);
+
+            // 若膨胀的点不在地图内（膨胀时只考虑地图范围内的点）
+            if (!isInMap(p3d_inf))
+            {
+                continue;
+            }
+
+            pt_inf.x = p3d_inf(0);
+            pt_inf.y = p3d_inf(1);
+            pt_inf.z = p3d_inf(2);
+            cloud_inflate_vis_->push_back(pt_inf);
+            // 设置膨胀后的点被占据（不管他之前是否被占据）
+            this->setOccupancy(p3d_inf, 1);
+        }
+    }
+
+    *cloud_inflate_vis_ += *global_uav_pcl;
+    // 加上border,仅用作显示作用（没有占据信息）
+    if (show_border)
+    {
+        *cloud_inflate_vis_ += border;
+    }
+
+    static int exec_num = 0;
+    exec_num++;
+
+    // 此处改为根据循环时间计算的数值
+    if (exec_num == 50)
+    {
+        // 膨胀地图效率与地图大小有关
+        cout << YELLOW << "Occupy map: inflate global point take " << (ros::Time::now() - time_start).toSec() << " [s]. " << TAIL << endl;
+        exec_num = 0;
+    }
+}
+
+void Occupy_map::pub_pcl_cb(const ros::TimerEvent &e)
+{
+    // 发布未膨胀点云
+    sensor_msgs::PointCloud2 global_env_;
+    // 假设此时收到的就是全局pcl
+    pcl::toROSMsg(*global_point_cloud_map, global_env_);
+    global_env_.header.frame_id = "world";
+    global_pcl_pub.publish(global_env_);
+
+    // 发布膨胀点云
+    sensor_msgs::PointCloud2 map_inflate_vis;
+    pcl::toROSMsg(*cloud_inflate_vis_, map_inflate_vis);
+    map_inflate_vis.header.frame_id = "world";
+    inflate_pcl_pub.publish(map_inflate_vis);
+}
+
+void Occupy_map::setOccupancy(Eigen::Vector3d &pos, int occ)
+{
+    if (occ != 1 && occ != 0)
+    {
+        // cout << RED << "Occupy map: occ value error " << TAIL <<endl;
+        return;
+    }
+
+    if (!isInMap(pos))
+    {
+        return;
+    }
+
+    Eigen::Vector3i id;
+    posToIndex(pos, id);
+
+    // 设置为占据/不占据 索引是如何索引的？ [三维地图 变 二维数组]
+    // 假设10*10*10米，分辨率1米，buffer大小为 1000 （即每一个占格都对应一个buffer索引）
+    // [0.1,0.1,0.1] 对应索引为[0,0,0]， buffer索引为 0
+    // [9.9,9.9,9.9] 对应索引为[9,9,9]， buffer索引为 900+90+9 = 999
+    occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)] = occ;
+}
+
+void Occupy_map::updateCostMap(Eigen::Vector3d &pos, double cost)
+{
+    if (!isInMap(pos))
+    {
+        return;
+    }
+
+    Eigen::Vector3i id;
+    posToIndex(pos, id);
+
+    if (cost_map_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)] >= cost)
+        return;
+    cost_map_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)] = cost;
+}
+
+bool Occupy_map::isInMap(Eigen::Vector3d pos)
+{
+    // min_range就是原点，max_range就是原点+地图尺寸
+    // 比如设置0,0,0为原点，[0,0,0]点会被判断为不在地图里
+    //　同时　对于２Ｄ情况，超出飞行高度的数据也会认为不在地图内部
+    if (pos(0) < min_range_(0) + 1e-4 || pos(1) < min_range_(1) + 1e-4 || pos(2) < min_range_(2) + 1e-4)
+    {
+        return false;
+    }
+
+    if (pos(0) > max_range_(0) - 1e-4 || pos(1) > max_range_(1) - 1e-4 || pos(2) > max_range_(2) - 1e-4)
+    {
+        return false;
+    }
+
+    return true;
+}
+
+bool Occupy_map::check_safety(Eigen::Vector3d &pos, double check_distance)
+{
+    if (!isInMap(pos))
+    {
+        // 当前位置点不在地图内
+        // cout << RED << "Occupy map, the odom point is not in map"  << TAIL <<endl;
+        return 0;
+    }
+    Eigen::Vector3i id;
+    posToIndex(pos, id);
+    Eigen::Vector3i id_occ;
+    Eigen::Vector3d pos_occ;
+
+    int check_dist_xy = int(check_distance / resolution_);
+    int check_dist_z = 0;
+    int cnt = 0;
+    for (int ix = -check_dist_xy; ix <= check_dist_xy; ix++)
+    {
+        for (int iy = -check_dist_xy; iy <= check_dist_xy; iy++)
+        {
+            for (int iz = -check_dist_z; iz <= check_dist_z; iz++)
+            {
+                id_occ(0) = id(0) + ix;
+                id_occ(1) = id(1) + iy;
+                id_occ(2) = id(2) + iz;
+                indexToPos(id_occ, pos_occ);
+                if (!isInMap(pos_occ))
+                {
+                    return 0;
+                }
+                if (getOccupancy(id_occ))
+                {
+                    cnt++;
+                }
+            }
+        }
+    }
+    if (cnt > 5)
+    {
+        return 0;
+    }
+    return 1;
+}
+
+void Occupy_map::posToIndex(Eigen::Vector3d &pos, Eigen::Vector3i &id)
+{
+    for (int i = 0; i < 3; ++i)
+    {
+        id(i) = floor((pos(i) - origin_(i)) * inv_resolution_);
+    }
+}
+
+void Occupy_map::indexToPos(Eigen::Vector3i &id, Eigen::Vector3d &pos)
+{
+    for (int i = 0; i < 3; ++i)
+    {
+        pos(i) = (id(i) + 0.5) * resolution_ + origin_(i);
+    }
+}
+
+int Occupy_map::getOccupancy(Eigen::Vector3d &pos)
+{
+    if (!isInMap(pos))
+    {
+        return -1;
+    }
+
+    Eigen::Vector3i id;
+    posToIndex(pos, id);
+
+    return occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)];
+}
+
+double Occupy_map::getCost(Eigen::Vector3d &pos)
+{
+    if (!isInMap(pos))
+    {
+        return -1;
+    }
+
+    Eigen::Vector3i id;
+    posToIndex(pos, id);
+
+    return cost_map_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)];
+}
+
+int Occupy_map::getOccupancy(Eigen::Vector3i &id)
+{
+    if (id(0) < 0 || id(0) >= grid_size_(0) || id(1) < 0 || id(1) >= grid_size_(1) || id(2) < 0 ||
+        id(2) >= grid_size_(2))
+    {
+        return -1;
+    }
+
+    return occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)];
+}

src/Prometheus/Modules/motion_planning/local_planner/CMakeLists.txt

@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(prometheus_local_planner)
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+       roscpp
+       rospy
+       std_msgs
+       sensor_msgs
+       laser_geometry
+       geometry_msgs
+       nav_msgs
+       pcl_ros
+       visualization_msgs
+       prometheus_msgs
+       mavros_msgs
+	)
+
+
+catkin_package(
+       INCLUDE_DIRS include
+       LIBRARIES LocalPlannerNS
+)      
+
+include_directories(
+       SYSTEM
+       include
+       ${PROJECT_SOURCE_DIR}/include
+       ${catkin_INCLUDE_DIRS}
+       ${Eigen3_INCLUDE_DIRS}
+       ${PCL_INCLUDE_DIRS}
+       ${PROJECT_SOURCE_DIR}/../../common/include
+     )
+link_directories(${PCL_LIBRARY_DIRS})
+
+
+
+add_library(vfh src/vfh.cpp)
+target_link_libraries(vfh ${PCL_LIBRARIES})
+add_library(apf src/apf.cpp)
+target_link_libraries(apf ${PCL_LIBRARIES})
+add_library(local_planner src/local_planner.cpp)
+target_link_libraries(local_planner vfh)
+target_link_libraries(local_planner apf)
+
+add_executable(local_planner_main src/local_planner_node.cpp)
+add_dependencies(local_planner_main prometheus_local_planner_gencpp)
+target_link_libraries(local_planner_main ${catkin_LIBRARIES})
+target_link_libraries(local_planner_main local_planner)

src/Prometheus/Modules/motion_planning/local_planner/include/apf.h

@@ -0,0 +1,63 @@
+#ifndef APF_H
+#define APF_H
+
+#include <ros/ros.h>
+#include <Eigen/Eigen>
+#include <iostream>
+#include <algorithm>
+#include <iostream>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Empty.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Odometry.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "local_planner_alg.h"
+#include "local_planner_utils.h"
+
+using namespace std;
+
+
+    class APF : public local_planner_alg
+    {
+    private:
+        //　参数
+        double inflate_distance;
+        double sensor_max_range;
+        double max_att_dist;
+        double k_repulsion;
+        double k_attraction;
+        double min_dist;
+        double ground_height;
+        double ground_safe_height;
+        double safe_distance;
+
+        bool has_local_map_;
+        bool has_odom_;
+
+        Eigen::Vector3d repulsive_force;
+        Eigen::Vector3d attractive_force;
+
+        pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
+        sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
+        nav_msgs::Odometry current_odom;
+
+    public:
+        virtual void set_odom(nav_msgs::Odometry cur_odom);
+        virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr);
+        virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr);
+        virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel);
+        virtual void init(ros::NodeHandle &nh);
+        APF() {}
+        ~APF() {}
+
+        typedef shared_ptr<APF> Ptr;
+    };
+
+
+
+#endif

src/Prometheus/Modules/motion_planning/local_planner/include/local_planner.h

@@ -0,0 +1,115 @@
+#ifndef LOCAL_PLANNER_H
+#define LOCAL_PLANNER_H
+
+#include <ros/ros.h>
+
+#include <Eigen/Eigen>
+#include <iostream>
+#include <algorithm>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Bool.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/LaserScan.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <visualization_msgs/Marker.h>
+#include <prometheus_msgs/UAVState.h>
+#include <prometheus_msgs/UAVCommand.h>
+#include <prometheus_msgs/UAVControlState.h>
+
+#include "apf.h"
+#include "vfh.h"
+#include "local_planner_utils.h"
+
+using namespace std;
+
+
+    class LocalPlanner
+    {
+    public:
+        LocalPlanner(ros::NodeHandle &nh);
+        ros::NodeHandle local_planner_nh;
+
+    private:
+        // 参数
+        int uav_id;
+        int algorithm_mode;
+        int map_input_source;
+        double max_planning_vel;
+        double fly_height;
+        double safe_distance;
+        bool sim_mode;
+        bool map_groundtruth;
+        string local_pcl_topic_name;
+
+        // 订阅无人机状态、目标点、传感器数据（生成地图）
+        ros::Subscriber goal_sub;
+        ros::Subscriber uav_state_sub;
+        ros::Subscriber uav_control_state_sub;
+        ros::Subscriber local_point_cloud_sub;
+
+        // 发布控制指令
+        ros::Publisher uav_cmd_pub;
+        ros::Publisher rviz_vel_pub;
+        ros::Timer mainloop_timer;
+        ros::Timer control_timer;
+
+        // 局部避障算法 算子
+        local_planner_alg::Ptr local_alg_ptr;
+
+        prometheus_msgs::UAVState uav_state;      // 无人机状态
+        prometheus_msgs::UAVControlState uav_control_state;
+        nav_msgs::Odometry uav_odom;
+        prometheus_msgs::UAVCommand uav_command; 
+
+        double distance_to_goal;
+        // 规划器状态
+        bool odom_ready;
+        bool drone_ready;
+        bool sensor_ready;
+        bool goal_ready;
+        bool is_safety;
+        bool path_ok;
+
+        // 规划初始状态及终端状态
+        Eigen::Vector3d uav_pos;  // 无人机位置
+        Eigen::Vector3d uav_vel;  // 无人机速度
+        Eigen::Quaterniond uav_quat; // 无人机四元数
+        double uav_yaw;
+        // 规划终端状态
+        Eigen::Vector3d goal_pos, goal_vel;
+
+        int planner_state;
+        Eigen::Vector3d desired_vel;
+        float desired_yaw;
+
+        // 五种状态机
+        enum EXEC_STATE
+        {
+            WAIT_GOAL,
+            PLANNING,
+            TRACKING,
+            LANDING,
+        };
+        EXEC_STATE exec_state;
+
+        sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
+        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr;
+        pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
+
+        void goal_cb(const geometry_msgs::PoseStampedConstPtr &msg);
+        void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg);
+        void uav_state_cb(const prometheus_msgs::UAVStateConstPtr &msg);
+        void pcl_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
+        void laserscan_cb(const sensor_msgs::LaserScanConstPtr &msg);
+        void mainloop_cb(const ros::TimerEvent &e);
+        void control_cb(const ros::TimerEvent &e);
+        void debug_info();
+    };
+
+
+#endif

src/Prometheus/Modules/motion_planning/local_planner/include/local_planner_alg.h

@@ -0,0 +1,37 @@
+#ifndef LOCAL_PLANNING_ALG
+#define LOCAL_PLANNING_ALG
+
+#include <Eigen/Eigen>
+#include <iostream>
+
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Empty.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Odometry.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+using namespace std;
+
+
+    class local_planner_alg
+    {
+    public:
+        local_planner_alg() {}
+        ~local_planner_alg() {}
+        virtual void set_odom(nav_msgs::Odometry cur_odom) = 0;
+        virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr) = 0;
+        virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr) = 0;
+        virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel) = 0;
+        virtual void init(ros::NodeHandle &nh) = 0;
+
+        typedef shared_ptr<local_planner_alg> Ptr;
+    };
+
+
+
+#endif

src/Prometheus/Modules/motion_planning/local_planner/include/local_planner_utils.h

@@ -0,0 +1,10 @@
+#ifndef LOCAL_PLANNER_UTILS_H
+#define LOCAL_PLANNER_UTILS_H
+
+#include "printf_utils.h"
+
+#define NODE_NAME "LocalPlanner"
+#define MIN_DIS 0.1
+
+#endif
+

src/Prometheus/Modules/motion_planning/local_planner/include/vfh.h

@@ -0,0 +1,73 @@
+#ifndef VFH_H
+#define VFH_H
+
+#include <ros/ros.h>
+#include <Eigen/Eigen>
+#include <iostream>
+#include <algorithm>
+#include <iostream>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Empty.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Odometry.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "math.h"
+#include "local_planner_alg.h"
+#include "local_planner_utils.h"
+
+using namespace std;
+
+
+    class VFH : public local_planner_alg
+    {
+    private:
+        //　参数
+        double inflate_distance;
+        double sensor_max_range;
+        double safe_distance;
+        bool has_local_map_;
+        bool has_odom_;
+
+        double goalWeight, obstacle_weight;
+        double inflate_and_safe_distance;
+        double velocity;
+
+        double *Hdata;
+        double Hres;
+        int Hcnt; // 直方图个数
+
+        pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
+        sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
+        nav_msgs::Odometry current_odom;
+
+        bool isIgnored(float x, float y, float z, float ws);
+        int find_Hcnt(double angle);
+        double find_angle(int cnt);
+        double angle_error(double angle1, double angle2);
+        void generate_voxel_data(double angle_cen, double angle_range, double val);
+        int find_optimization_path(void);
+
+    public:
+        virtual void set_odom(nav_msgs::Odometry cur_odom);
+        virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr);
+        virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr);
+        virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel);
+        virtual void init(ros::NodeHandle &nh);
+
+        VFH() {}
+        ~VFH()
+        {
+            delete Hdata;
+        }
+
+        typedef shared_ptr<VFH> Ptr;
+    };
+
+
+
+#endif

src/Prometheus/Modules/motion_planning/local_planner/launch/sitl_apf_with_local_point.launch

@@ -0,0 +1,36 @@
+<launch>
+    <!-- 启动全局规划算法 -->
+    <node pkg="prometheus_local_planner" name="local_planner_main" type="local_planner_main" output="screen">
+        <!-- 参数 -->
+        <param name="uav_id" value="1"/>
+        <param name="local_planner/sim_mode" value="true"/>
+        <!-- 局部避障算法: 0为APF,1为VFH -->
+        <param name="local_planner/algorithm_mode" value="0"/>
+        <!-- 地图输入模式 0代表全局点云，1代表局部点云，2代表激光雷达scan数据 -->
+        <param name="local_planner/map_input_source" value="0"/>
+        <param name="local_planner/local_pcl_topic_name" value="/map_generator/local_cloud"/>
+        <!-- 无人机飞行高度，建议与起飞高度一致 -->
+        <param name="local_planner/fly_height" value="1.0"/>
+        <!-- 最大速度 -->
+        <param name="local_planner/max_planning_vel" value="0.5"/>
+        <!-- APF参数 -->
+		<!-- 膨胀参数，一般设置为无人机的半径或更大 -->
+		<param name="apf/inflate_distance" value="0.4" type="double"/>
+		<!-- 感知距离，只考虑感知距离内的障碍物 -->
+		<param name="apf/obs_distance" value="5.0" type="double"/>  
+		<!-- 增益 -->
+		<param name="apf/k_repulsion" value="2.0" type="double"/>
+		<param name="apf/k_attraction" value="1.0" type="double"/>
+		<!-- 安全距离，距离障碍物在安全距离内，k_repulsion自动增大 -->
+		<param name="apf/min_dist" value="0.1" type="double"/>
+		<!-- 最大吸引距离 -->
+		<param name="apf/max_att_dist" value="4" type="double"/>
+		<!-- 地面高度，不考虑低于地面高度的障碍物 -->
+		<param name="apf/ground_height" value="0.3" type="double"/>
+		<!-- 地面安全高度，小于该高度，会产生向上推力 -->
+		<param name="apf/ground_safe_height" value="0.3" type="double"/>
+		<!-- 停止距离，小于该距离，停止自动飞行 -->
+		<param name="apf/safe_distance" value="0.01" type="double"/>
+    </node>
+</launch>
+

src/Prometheus/Modules/motion_planning/local_planner/launch/sitl_vfh_with_local_point.launch

@@ -0,0 +1,29 @@
+<launch>
+    <!-- 启动全局规划算法 -->
+    <node pkg="prometheus_local_planner" name="local_planner_main" type="local_planner_main" output="screen">
+        <!-- 参数 -->
+        <param name="uav_id" value="1"/>
+        <param name="local_planner/sim_mode" value="true"/>
+        <!-- 局部避障算法: 0为APF,1为VFH -->
+        <param name="local_planner/algorithm_mode" value="1"/>
+        <!-- 地图输入模式 0代表全局点云，1代表局部点云，2代表激光雷达scan数据 -->
+        <param name="local_planner/map_input_source" value="0"/>
+        <param name="local_planner/local_pcl_topic_name" value="/map_generator/local_cloud"/>
+        <!-- 无人机飞行高度，建议与起飞高度一致 -->
+        <param name="local_planner/fly_height" value="1.0"/>
+        <!-- 最大速度 -->
+        <param name="local_planner/max_planning_vel" value="0.5"/>
+        <!-- VFH参数 -->
+		<!-- 膨胀参数，一般设置为无人机的半径或更大 -->
+		<param name="vfh/inflate_distance" value="1.0" type="double"/>
+		<!-- 感知距离，只考虑感知距离内的障碍物 -->
+		<param name="vfh/sensor_max_range" value="6.0" type="double"/>  
+		<!-- weight -->
+		<param name="vfh/obstacle_weight" value="10.0" type="double"/>
+		<param name="vfh/goalWeight" value="0.1" type="double"/>
+		<param name="vfh/safe_distance" value="0.01" type="double"/>
+		<!-- 直方图个数 -->
+		<param name="vfh/h_res" value="360" type="int"/>
+    </node>
+</launch>
+

src/Prometheus/Modules/motion_planning/local_planner/local_planner.md

@@ -0,0 +1,31 @@
+## local_planner
+
+
+
+#### 情况讨论
+
+ - 局部点云情况
+   - 对应真实情况:D435i等RGBD相机,三维激光雷达
+   - map_generator生成全局点云,模拟得到局部点云信息
+   - 无人机不需要搭载传感器
+   - PX4动力学 & fake_odom模块
+
+roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch
+roslaunch prometheus_local_planner sitl_apf_with_local_point.launch
+
+ - 2dlidar情况
+   - 对应真实情况:二维激光雷达
+   - projector_.projectLaser(*local_point, input_laser_scan)将scan转换为点云,即对应回到局部点云情况
+   - map_generator生成全局点云,模拟得到局部点云信息
+   - 无人机不需要搭载传感器
+   - PX4动力学 & fake_odom模块
+
+roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch
+roslaunch prometheus_local_planner sitl_global_planner_with_2dlidar.launch
+
+ - 多机情况
+   - 建议使用 全局点云情况 + 多个无人机
+   - fake_odom模块
+
+## 运行
+

src/Prometheus/Modules/motion_planning/local_planner/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>prometheus_local_planner</name>
+    <version>0.0.0</version>
+    <description>The prometheus_local_planner package</description>
+
+    <maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
+
+    <license>TODO</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+    <build_depend>roscpp</build_depend>
+    <build_depend>rospy</build_depend>
+    <build_depend>std_msgs</build_depend>
+    <build_depend>sensor_msgs</build_depend>
+    <build_depend>geometry_msgs</build_depend>
+    <build_depend>nav_msgs</build_depend>
+    <build_depend>prometheus_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>
+    <build_export_depend>sensor_msgs</build_export_depend>
+    <build_export_depend>geometry_msgs</build_export_depend>
+    <build_export_depend>nav_msgs</build_export_depend>
+    <build_export_depend>prometheus_msgs</build_export_depend>
+    <exec_depend>roscpp</exec_depend>
+    <exec_depend>rospy</exec_depend>
+    <exec_depend>std_msgs</exec_depend>
+    <exec_depend>sensor_msgs</exec_depend>
+    <exec_depend>geometry_msgs</exec_depend>
+    <exec_depend>nav_msgs</exec_depend>
+    <exec_depend>prometheus_msgs</exec_depend>
+
+    <export>
+        <!-- Other tools can request additional information be placed here -->
+    </export>
+</package>

src/Prometheus/Modules/motion_planning/local_planner/src/apf.cpp

@@ -0,0 +1,183 @@
+#include "apf.h"
+
+
+
+    void APF::init(ros::NodeHandle &nh)
+    {
+        
+        // 【参数】障碍物膨胀距离
+        nh.param("apf/inflate_distance", inflate_distance, 0.2);    
+        // 【参数】感知障碍物距离
+        nh.param("apf/sensor_max_range", sensor_max_range, 5.0);    
+        // 【参数】障碍物排斥力增益
+        nh.param("apf/k_repulsion", k_repulsion, 0.8);                        
+        // 【参数】目标吸引力增益
+        nh.param("apf/k_attraction", k_attraction, 0.4);                          
+        // 【参数】最小避障距离
+        nh.param("apf/min_dist", min_dist, 0.2);                     
+        // 【参数】最大吸引距离（相对于目标）
+        nh.param("apf/max_att_dist", max_att_dist, 5.0);             
+        // 【参数】地面高度
+        nh.param("apf/ground_height", ground_height, 0.1);           
+        // 【参数】地面安全距离，低于地面高度，则不考虑该点的排斥力
+        nh.param("apf/ground_safe_height", ground_safe_height, 0.2); 
+        // 【参数】安全停止距离
+        nh.param("apf/safe_distance", safe_distance, 0.15);          
+        has_local_map_ = false;
+    }
+
+    // 设置局部地图
+    void APF::set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr)
+    {
+        local_map_ptr_ = local_map_ptr;
+        pcl::fromROSMsg(*local_map_ptr, latest_local_pcl_);
+        has_local_map_ = true;
+    }
+
+    // 设置局部点云
+    void APF::set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr)
+    {
+        latest_local_pcl_ = *pcl_ptr;
+        has_local_map_ = true;
+    }
+
+    // 设置本地位置
+    void APF::set_odom(nav_msgs::Odometry cur_odom)
+    {
+        current_odom = cur_odom;
+        has_odom_ = true;
+    }
+
+    // 计算输出
+    int APF::compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel)
+    {
+        // 规划器返回的状态值：0 for not init; 1 for safe; 2 for dangerous
+        int local_planner_state = 0;
+        int safe_cnt = 0;
+
+        if (!has_local_map_ || !has_odom_)
+            return 0;
+
+        if ((int)latest_local_pcl_.points.size() == 0)
+            return 0;
+
+        if (isnan(goal(0)) || isnan(goal(1)) || isnan(goal(2)))
+            return 0;
+
+        //　当前位置
+        Eigen::Vector3d current_pos;
+        current_pos[0] = current_odom.pose.pose.position.x;
+        current_pos[1] = current_odom.pose.pose.position.y;
+        current_pos[2] = current_odom.pose.pose.position.z;
+
+        ros::Time begin_collision = ros::Time::now();
+
+        // 引力
+        Eigen::Vector3d uav2goal = goal - current_pos;
+        // 不考虑高度影响
+        uav2goal(2) = 0.0;
+        double dist_att = uav2goal.norm();
+        if (dist_att > max_att_dist)
+        {
+            uav2goal = max_att_dist * uav2goal / dist_att;
+        }
+        //　计算吸引力
+        attractive_force = k_attraction * uav2goal;
+
+        // 排斥力
+        double uav_height = current_odom.pose.pose.position.z;
+        repulsive_force = Eigen::Vector3d(0.0, 0.0, 0.0);
+
+        Eigen::Vector3d p3d;
+        vector<Eigen::Vector3d> obstacles;
+
+        //　根据局部点云计算排斥力（是否可以考虑对点云进行降采样？）
+        for (size_t i = 0; i < latest_local_pcl_.points.size(); ++i)
+        {
+            p3d(0) = latest_local_pcl_.points[i].x;
+            p3d(1) = latest_local_pcl_.points[i].y;
+            p3d(2) = latest_local_pcl_.points[i].z;
+
+            Eigen::Vector3d current_pos_local(0.0, 0.0, 0.0);
+
+            //　低于地面高度，则不考虑该点的排斥力
+            double point_height_global = uav_height + p3d(2);
+            if (fabs(point_height_global) < ground_height)
+                continue;
+
+            //　超出最大感知距离，则不考虑该点的排斥力
+            double dist_push = (current_pos_local - p3d).norm();
+            if (dist_push > sensor_max_range || isnan(dist_push))
+                continue;
+
+            //　考虑膨胀距离
+            dist_push = dist_push - inflate_distance;
+
+            // 如果当前的观测点中，包含小于安全停止距离的点，进行计数
+            if (dist_push < safe_distance)
+            {
+                safe_cnt++;
+            }
+
+            //　小于最小距离时，则增大该距离，从而增大排斥力
+            if (dist_push < min_dist)
+            {
+                dist_push = min_dist / 1.5;
+            }
+
+            obstacles.push_back(p3d);
+            double push_gain = k_repulsion * (1 / dist_push - 1 / sensor_max_range) * 1.0 / (dist_push * dist_push);
+
+            if (dist_att < 1.0)
+            {
+                push_gain *= dist_att; // to gaurantee to reach the goal.
+            }
+
+            repulsive_force += push_gain * (current_pos_local - p3d) / dist_push;
+        }
+
+        //　平均排斥力
+        if (obstacles.size() != 0)
+        {
+            repulsive_force = repulsive_force / obstacles.size();
+        }
+
+        Eigen::Quaterniond cur_rotation_local_to_global(current_odom.pose.pose.orientation.w,
+                                                        current_odom.pose.pose.orientation.x,
+                                                        current_odom.pose.pose.orientation.y,
+                                                        current_odom.pose.pose.orientation.z);
+
+        Eigen::Matrix<double, 3, 3> rotation_mat_local_to_global = cur_rotation_local_to_global.toRotationMatrix();
+
+        repulsive_force = rotation_mat_local_to_global * repulsive_force;
+
+        // 合力
+        desired_vel = repulsive_force + attractive_force;
+
+        // 由于定高飞行，设置期望Z轴速度为0
+        desired_vel[2] = 0.0;
+
+        // 如果不安全的点超出，
+        if (safe_cnt > 10)
+        {
+            local_planner_state = 2; //成功规划，但是飞机不安全
+        }
+        else
+        {
+            local_planner_state = 1; //成功规划， 安全
+        }
+
+        static int exec_num = 0;
+        exec_num++;
+
+        // 此处改为根据循环时间计算的数值
+        if (exec_num == 50)
+        {
+            cout << GREEN << NODE_NAME << "APF calculate take: " << (ros::Time::now() - begin_collision).toSec() << " [s] " << TAIL << endl;
+            exec_num = 0;
+        }
+
+        return local_planner_state;
+    }
+
+

src/Prometheus/Modules/motion_planning/local_planner/src/local_planner.cpp

@@ -0,0 +1,432 @@
+#include "local_planner.h"
+
+// 初始化函数
+LocalPlanner::LocalPlanner(ros::NodeHandle &nh)
+{
+    // 【参数】编号，从1开始编号
+    nh.param("uav_id", uav_id, 0);
+    // 【参数】是否为仿真模式
+    nh.param("local_planner/sim_mode", sim_mode, false);
+    // 【参数】根据参数 planning/algorithm_mode 选择局部避障算法: 0为APF,1为VFH
+    nh.param("local_planner/algorithm_mode", algorithm_mode, 0);
+    // 【参数】激光雷达模型,0代表3d雷达,1代表2d雷达
+    // 3d雷达输入类型为 <sensor_msgs::PointCloud2> 2d雷达输入类型为 <sensor_msgs::LaserScan>
+    nh.param("local_planner/map_input_source", map_input_source, 0);
+    // 【参数】定高高度
+    nh.param("local_planner/fly_height", fly_height, 1.0);
+    // 【参数】最大速度
+    nh.param("local_planner/max_planning_vel", max_planning_vel, 0.4);
+
+    //【订阅】订阅目标点
+    goal_sub = nh.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/motion_planning/goal", 1, &LocalPlanner::goal_cb, this);
+
+    //【订阅】无人机状态信息
+    uav_state_sub = nh.subscribe<prometheus_msgs::UAVState>("/uav" + std::to_string(uav_id) + "/prometheus/state",
+                                                            1,
+                                                            &LocalPlanner::uav_state_cb, this);
+
+    uav_control_state_sub = nh.subscribe<prometheus_msgs::UAVControlState>("/uav" + std::to_string(uav_id) + "/prometheus/control_state",
+                                                                           1,
+                                                                           &LocalPlanner::uav_control_state_cb, this);
+    string uav_name = "/uav" + std::to_string(uav_id);
+    // 订阅传感器点云信息,该话题名字可在launch文件中任意指定
+    if (map_input_source == 0)
+    {
+        nh.getParam("local_planner/local_pcl_topic_name", local_pcl_topic_name);
+        cout << GREEN << "Local pcl mode, subscirbe to " << uav_name << local_pcl_topic_name << TAIL << endl;
+        local_point_cloud_sub = nh.subscribe<sensor_msgs::PointCloud2>("/uav" + std::to_string(uav_id) + local_pcl_topic_name, 1, &LocalPlanner::pcl_cb, this);
+    }
+    else if (map_input_source == 1)
+    {
+        nh.getParam("global_planner/local_pcl_topic_name", local_pcl_topic_name);
+        cout << GREEN << "Laser scan mode, subscirbe to " << uav_name << local_pcl_topic_name << TAIL << endl;
+        local_point_cloud_sub = nh.subscribe<sensor_msgs::LaserScan>("/uav" + std::to_string(uav_id) + local_pcl_topic_name, 1, &LocalPlanner::laserscan_cb, this);
+    }
+
+    // 【发布】控制指令
+    uav_cmd_pub = nh.advertise<prometheus_msgs::UAVCommand>("/uav" + std::to_string(uav_id) + "/prometheus/command", 1);
+
+    // 【发布】速度用于显示
+    rviz_vel_pub = nh.advertise<visualization_msgs::Marker>("/uav" + std::to_string(uav_id) + "/prometheus/local_planner/desired_vel", 0 );
+
+    // 【定时器】执行周期为1Hz
+    mainloop_timer = nh.createTimer(ros::Duration(0.2), &LocalPlanner::mainloop_cb, this);
+
+    // 【定时器】控制定时器
+    control_timer = nh.createTimer(ros::Duration(0.05), &LocalPlanner::control_cb, this);
+
+    // 选择避障算法
+    if (algorithm_mode == 0)
+    {
+        local_alg_ptr.reset(new APF);
+        local_alg_ptr->init(nh);
+        cout << GREEN << NODE_NAME << "APF init. " << TAIL << endl;
+    }
+    else if (algorithm_mode == 1)
+    {
+        local_alg_ptr.reset(new VFH);
+        local_alg_ptr->init(nh);
+        cout << GREEN << NODE_NAME << "VFH init. " << TAIL << endl;
+    }
+
+    // 规划器状态参数初始化
+    exec_state = EXEC_STATE::WAIT_GOAL;
+    odom_ready = false;
+    drone_ready = false;
+    goal_ready = false;
+    sensor_ready = false;
+    path_ok = false;
+
+    // 初始化发布的指令
+    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover;
+    uav_command.position_ref[0] = 0;
+    uav_command.position_ref[1] = 0;
+    uav_command.position_ref[2] = 0;
+    uav_command.yaw_ref = 0;
+    desired_yaw = 0.0;
+
+    // 地图初始化
+    sensor_msgs::PointCloud2ConstPtr init_local_map(new sensor_msgs::PointCloud2());
+    local_map_ptr_ = init_local_map;
+}
+void LocalPlanner::debug_info()
+{
+    //固定的浮点显示
+    cout.setf(ios::fixed);
+    // setprecision(n) 设显示小数精度为n位
+    cout << setprecision(2);
+    //左对齐
+    cout.setf(ios::left);
+    // 强制显示小数点
+    cout.setf(ios::showpoint);
+    // 强制显示符号
+    cout.setf(ios::showpos);
+    cout << GREEN << "--------------> Local Planner <------------- " << TAIL << endl;
+    cout << GREEN << "[ ID: " << uav_id << "]  " << TAIL;
+    if (drone_ready)
+    {
+        cout << GREEN << "[ drone ready ]  " << TAIL << endl;
+    }
+    else
+    {
+        cout << RED << "[ drone not ready ]  " << TAIL << endl;
+    }
+
+    if (odom_ready)
+    {
+        cout << GREEN << "[ odom ready ]  " << TAIL << endl;
+    }
+    else
+    {
+        cout << RED << "[ odom not ready ]  " << TAIL << endl;
+    }
+
+    if (sensor_ready)
+    {
+        cout << GREEN << "[ sensor ready ]  " << TAIL << endl;
+    }
+    else
+    {
+        cout << RED << "[ sensor not ready ]  " << TAIL << endl;
+    }
+
+    if (exec_state == EXEC_STATE::WAIT_GOAL)
+    {
+        cout << GREEN << "[ WAIT_GOAL ] " << TAIL << endl;
+        if (uav_control_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL)
+        {
+            cout << YELLOW << "Please switch to COMMAND_CONTROL mode." << TAIL << endl;
+        }
+        if (!goal_ready)
+        {
+            cout << YELLOW << "Waiting for a new goal." << TAIL << endl;
+        }
+    }
+    else if (exec_state == EXEC_STATE::PLANNING)
+    {
+        cout << GREEN << "[ PLANNING ] " << TAIL << endl;
+
+        if (planner_state == 1)
+        {
+            cout << GREEN << NODE_NAME << "local planning desired vel [XY]:" << desired_vel(0) << "[m/s]" << desired_vel(1) << "[m/s]" << TAIL << endl;
+        }
+        else if (planner_state == 2)
+        {
+            cout << YELLOW << NODE_NAME << "Dangerous!" << TAIL << endl;
+        }
+        distance_to_goal = (uav_pos - goal_pos).norm();
+        cout << GREEN << "---->distance_to_goal:" << distance_to_goal << TAIL << endl;
+    }
+    else if (exec_state == EXEC_STATE::LANDING)
+    {
+        cout << GREEN << "[ LANDING ] " << TAIL << endl;
+    }
+}
+
+void LocalPlanner::mainloop_cb(const ros::TimerEvent &e)
+{
+    static int exec_num = 0;
+    exec_num++;
+
+    if (exec_num == 10)
+    {
+        debug_info();
+        exec_num = 0;
+    }
+
+    // 检查当前状态，不满足规划条件则直接退出主循环
+    if (!odom_ready || !drone_ready || !sensor_ready)
+    {
+        return;
+    }
+    else
+    {
+        // 对检查的状态进行重置
+        odom_ready = false;
+        drone_ready = false;
+        sensor_ready = false;
+    }
+
+    switch (exec_state)
+    {
+    case EXEC_STATE::WAIT_GOAL:
+        path_ok = false;
+        if (!goal_ready)
+        {
+            if (exec_num == 20)
+            {
+                cout << GREEN << NODE_NAME << "Waiting for a new goal." << TAIL << endl;
+                exec_num = 0;
+            }
+        }
+        else
+        {
+            // 获取到目标点后，生成新轨迹
+            exec_state = EXEC_STATE::PLANNING;
+            goal_ready = false;
+        }
+
+        break;
+    case EXEC_STATE::PLANNING:
+        // desired_vel是返回的规划速度；返回值为2时,飞机不安全(距离障碍物太近)
+        planner_state = local_alg_ptr->compute_force(goal_pos, desired_vel);
+
+        path_ok = true;
+
+        //　对规划的速度进行限幅处理
+        if (desired_vel.norm() > max_planning_vel)
+        {
+            desired_vel = desired_vel / desired_vel.norm() * max_planning_vel;
+        }
+
+        break;
+    case EXEC_STATE::LANDING:
+
+        uav_command.header.stamp = ros::Time::now();
+        uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land;
+        uav_command.Command_ID = uav_command.Command_ID + 1;
+
+        uav_cmd_pub.publish(uav_command);
+        break;
+    }
+}
+
+
+void LocalPlanner::goal_cb(const geometry_msgs::PoseStampedConstPtr &msg)
+{
+    // 2D定高飞行
+    goal_pos << msg->pose.position.x, msg->pose.position.y, fly_height;
+    goal_vel.setZero();
+    goal_ready = true;
+
+    cout << GREEN << NODE_NAME << "Get a new goal point:" << goal_pos(0) << " [m] " << goal_pos(1) << " [m] " << goal_pos(2) << " [m] " << TAIL << endl;
+
+    if (goal_pos(0) == 99 && goal_pos(1) == 99)
+    {
+        path_ok = false;
+        goal_ready = false;
+        exec_state = EXEC_STATE::LANDING;
+        cout << GREEN << NODE_NAME << "Land " << TAIL << endl;
+    }
+}
+
+//无人机控制状态回调函数
+void LocalPlanner::uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg)
+{
+    uav_control_state = *msg;
+}
+
+void LocalPlanner::uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg)
+{
+    uav_state = *msg;
+    if (uav_state.connected == true && uav_state.armed == true)
+    {
+        drone_ready = true;
+    }
+    else
+    {
+        drone_ready = false;
+    }
+
+    if (uav_state.odom_valid)
+    {
+        odom_ready = true;
+    }
+    else
+    {
+        odom_ready = false;
+    }
+
+    uav_odom.header = uav_state.header;
+    uav_odom.child_frame_id = "base_link";
+
+    uav_odom.pose.pose.position.x = uav_state.position[0];
+    uav_odom.pose.pose.position.y = uav_state.position[1];
+    uav_odom.pose.pose.position.z = fly_height;
+
+    uav_odom.pose.pose.orientation = uav_state.attitude_q;
+    uav_odom.twist.twist.linear.x = uav_state.velocity[0];
+    uav_odom.twist.twist.linear.y = uav_state.velocity[1];
+    uav_odom.twist.twist.linear.z = uav_state.velocity[2];
+
+    uav_pos = Eigen::Vector3d(msg->position[0], msg->position[1], fly_height);
+    uav_vel = Eigen::Vector3d(msg->velocity[0], msg->velocity[1], msg->velocity[2]);
+    uav_yaw = msg->attitude[2];
+
+    local_alg_ptr->set_odom(uav_odom);
+}
+
+void LocalPlanner::laserscan_cb(const sensor_msgs::LaserScanConstPtr &msg)
+{
+    if (!odom_ready)
+    {
+        return;
+    }
+
+    pcl::PointCloud<pcl::PointXYZ> _pointcloud;
+
+    _pointcloud.clear();
+    pcl::PointXYZ newPoint;
+    double newPointAngle;
+
+    int beamNum = msg->ranges.size();
+    for (int i = 0; i < beamNum; i++)
+    {
+        newPointAngle = msg->angle_min + msg->angle_increment * i;
+        newPoint.x = msg->ranges[i] * cos(newPointAngle);
+        newPoint.y = msg->ranges[i] * sin(newPointAngle);
+        newPoint.z = uav_odom.pose.pose.position.z;
+        _pointcloud.push_back(newPoint);
+    }
+
+    pcl_ptr = _pointcloud.makeShared();
+    local_alg_ptr->set_local_map_pcl(pcl_ptr);
+    latest_local_pcl_ = *pcl_ptr; // 没用
+
+    sensor_ready = true;
+}
+
+void LocalPlanner::pcl_cb(const sensor_msgs::PointCloud2ConstPtr &msg)
+{
+    if (!odom_ready)
+    {
+        return;
+    }
+
+    local_map_ptr_ = msg;
+    local_alg_ptr->set_local_map(local_map_ptr_);
+    pcl::fromROSMsg(*msg, latest_local_pcl_); // 没用
+
+    sensor_ready = true;
+}
+
+void LocalPlanner::control_cb(const ros::TimerEvent &e)
+{
+    if (!path_ok)
+    {
+        return;
+    }
+
+    distance_to_goal = (uav_pos - goal_pos).norm();
+
+    // 抵达终点
+    if (distance_to_goal < MIN_DIS)
+    {
+        uav_command.header.stamp = ros::Time::now();
+        uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
+        uav_command.Command_ID = uav_command.Command_ID + 1;
+
+        uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS;
+        uav_command.position_ref[0] = goal_pos[0];
+        uav_command.position_ref[1] = goal_pos[1];
+        uav_command.position_ref[2] = goal_pos[2];
+
+        uav_command.yaw_ref = desired_yaw;
+        uav_cmd_pub.publish(uav_command);
+
+        cout << GREEN << NODE_NAME << "Reach the goal! " << TAIL << endl;
+        // 停止执行
+        path_ok = false;
+        // 转换状态为等待目标
+        exec_state = EXEC_STATE::WAIT_GOAL;
+        return;
+    }
+
+    uav_command.header.stamp = ros::Time::now();
+    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
+    uav_command.Command_ID = uav_command.Command_ID + 1;
+
+    uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS;
+    uav_command.position_ref[2] = fly_height;
+    uav_command.velocity_ref[0] = desired_vel[0];
+    uav_command.velocity_ref[1] = desired_vel[1];
+    uav_command.yaw_ref = desired_yaw;
+
+    uav_cmd_pub.publish(uav_command);
+
+    //　发布rviz显示 rviz怎么显示速度方向?
+
+    visualization_msgs::Marker marker;
+    marker.header.frame_id = "world";
+    marker.header.stamp = ros::Time();
+    marker.ns = "my_namespace";
+    marker.id = 0;
+    marker.type = visualization_msgs::Marker::ARROW;
+    marker.action = visualization_msgs::Marker::ADD;
+    // marker.pose.position.x = 1;
+    // marker.pose.position.y = 1;
+    // marker.pose.position.z = 1;
+    marker.pose.orientation.x = 0.0;
+    marker.pose.orientation.y = 0.0;
+    marker.pose.orientation.z = 0.0;
+    marker.pose.orientation.w = 1.0;
+    // marker.scale.x = 1;
+    // marker.scale.y = 0.1;
+    // marker.scale.z = 0.1;
+    // marker.color.a = 1.0; // Don't forget to set the alpha!
+    // marker.color.r = 0.0;
+    // marker.color.g = 1.0;
+    // marker.color.b = 0.0;
+
+marker.scale.x = 0.1;
+marker.scale.y = 0.1;
+marker.scale.z = 0.15;
+// 点为绿色
+marker.color.g = 1.0f;
+marker.color.a = 1.0;
+geometry_msgs::Point p1, p2;
+p1.x = uav_pos(0);
+p1.y = uav_pos(1);
+p1.z = uav_pos(2);
+p2.x = uav_pos(0) + desired_vel(0);
+p2.y = uav_pos(1) + desired_vel(1);
+p2.z = uav_pos(2) + desired_vel(2);
+marker.points.push_back(p1);
+marker.points.push_back(p2);
+
+    
+    //only if using a MESH_RESOURCE marker type:
+    // marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+
+    rviz_vel_pub.publish(marker);
+}

src/Prometheus/Modules/motion_planning/local_planner/src/local_planner_node.cpp

@@ -0,0 +1,27 @@
+#include <ros/ros.h>
+#include <signal.h>
+
+#include "local_planner.h"
+
+
+
+void mySigintHandler(int sig)
+{
+  ROS_INFO("[local_planner_node] exit...");
+  ros::shutdown();
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "local_planner_node");
+  ros::NodeHandle nh("~");
+
+  signal(SIGINT, mySigintHandler);
+  ros::Duration(1.0).sleep();
+
+  LocalPlanner local_planner(nh);
+
+  ros::spin();
+
+  return 0;
+}

src/Prometheus/Modules/motion_planning/local_planner/src/vfh.cpp

@@ -0,0 +1,286 @@
+#include "vfh.h"
+
+
+
+    void VFH::init(ros::NodeHandle &nh)
+    {
+        // 【参数】障碍物膨胀距离
+        nh.param("vfh/inflate_distance", inflate_distance, 0.2); 
+        // 【参数】感知障碍物距离
+        nh.param("vfh/sensor_max_range", sensor_max_range, 5.0);  
+        // 【参数】目标权重
+        nh.param("vfh/goalWeight", goalWeight, 0.2);              
+        // 【参数】直方图 个数
+        nh.param("vfh/h_res", Hcnt, 180);                         
+        // 【参数】障碍物权重
+        nh.param("vfh/obstacle_weight", obstacle_weight, 0.0);    
+        // 【参数】安全停止距离
+        nh.param("vfh/safe_distance", safe_distance, 0.2);        
+        // 【参数】设定速度
+        nh.param("vfh/velocity", velocity, 1.0);
+
+        inflate_and_safe_distance = safe_distance + inflate_distance;
+        Hres = 2 * M_PI / Hcnt;
+        Hdata = new double[Hcnt]();
+        for (int i(0); i < Hcnt; i++)
+        {
+            Hdata[i] = 0.0;
+        }
+
+        has_local_map_ = false;
+    }
+
+    // get the map
+    void VFH::set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr)
+    {
+        local_map_ptr_ = local_map_ptr;
+        pcl::fromROSMsg(*local_map_ptr, latest_local_pcl_);
+        has_local_map_ = true;
+    }
+
+    void VFH::set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr)
+    {
+        latest_local_pcl_ = *pcl_ptr;
+        has_local_map_ = true;
+    }
+
+    void VFH::set_odom(nav_msgs::Odometry cur_odom)
+    {
+        current_odom = cur_odom;
+        has_odom_ = true;
+    }
+
+    int VFH::compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel)
+    {
+        // 0 for not init; 1for safe; 2 for dangerous
+        int local_planner_state = 0;
+        int safe_cnt = 0;
+
+        if (!has_local_map_ || !has_odom_)
+            return 0;
+
+        if ((int)latest_local_pcl_.points.size() == 0)
+            return 0;
+
+        if (isnan(goal(0)) || isnan(goal(1)) || isnan(goal(2)))
+            return 0;
+
+        // clear the Hdata
+        for (int i = 0; i < Hcnt; i++)
+        {
+            Hdata[i] = 0;
+        }
+
+        ros::Time begin_collision = ros::Time::now();
+
+        // 计算障碍物相关cost
+        Eigen::Vector3d p3d;
+        vector<Eigen::Vector3d> obstacles;
+        Eigen::Vector3d p3d_gloabl_rot;
+
+        // 排斥力
+        Eigen::Quaterniond cur_rotation_local_to_global(current_odom.pose.pose.orientation.w, current_odom.pose.pose.orientation.x, current_odom.pose.pose.orientation.y, current_odom.pose.pose.orientation.z);
+
+        Eigen::Matrix<double, 3, 3> rotation_mat_local_to_global = cur_rotation_local_to_global.toRotationMatrix();
+        Eigen::Vector3d eulerAngle_yrp = rotation_mat_local_to_global.eulerAngles(2, 1, 0);
+        rotation_mat_local_to_global = Eigen::AngleAxisd(eulerAngle_yrp(0), Eigen::Vector3d::UnitZ()).toRotationMatrix();
+
+        // 遍历点云中的所有点
+        for (size_t i = 0; i < latest_local_pcl_.points.size(); ++i)
+        {
+            // 提取障碍物点
+            p3d(0) = latest_local_pcl_.points[i].x;
+            p3d(1) = latest_local_pcl_.points[i].y;
+            p3d(2) = latest_local_pcl_.points[i].z;
+
+            // 将本地点云转化为全局点云点(主要是yaw角)
+            p3d_gloabl_rot = rotation_mat_local_to_global * p3d;
+
+            // sensor_max_range为感知距离,只考虑感知距离内的障碍
+            if (isIgnored(p3d_gloabl_rot(0), p3d_gloabl_rot(1), p3d_gloabl_rot(2), sensor_max_range))
+            {
+                continue;
+            }
+
+            double obs_dist = p3d_gloabl_rot.norm();
+            double obs_angle = atan2(p3d_gloabl_rot(1), p3d_gloabl_rot(0));
+            double angle_range;
+            if (obs_dist > inflate_and_safe_distance)
+            {
+                angle_range = asin(inflate_and_safe_distance / obs_dist);
+            }
+            else if (obs_dist <= inflate_and_safe_distance)
+            {
+                safe_cnt++; // 非常危险
+                continue;
+            }
+
+            double obstacle_cost = obstacle_weight * (1 / obs_dist - 1 / sensor_max_range) * 1.0 / (obs_dist * obs_dist);
+            generate_voxel_data(obs_angle, angle_range, obstacle_cost);
+
+            obstacles.push_back(p3d);
+        }
+
+        // 与目标点相关cost计算
+        //　当前位置
+        Eigen::Vector3d current_pos;
+        current_pos[0] = current_odom.pose.pose.position.x;
+        current_pos[1] = current_odom.pose.pose.position.y;
+        current_pos[2] = current_odom.pose.pose.position.z;
+        Eigen::Vector3d uav2goal = goal - current_pos;
+        // 不考虑高度影响
+        uav2goal(2) = 0.0;
+        double dist_att = uav2goal.norm();
+        double goal_heading = atan2(uav2goal(1), uav2goal(0));
+
+        for (int i = 0; i < Hcnt; i++)
+        {
+            // Hdata;
+            // angle_i 为当前角度
+            double angle_i = find_angle(i);
+
+            double goal_cost = 0;
+            double angle_er = angle_error(angle_i, goal_heading);
+            float goal_gain;
+            if (dist_att > 3.0)
+            {
+                goal_gain = 3.0;
+            }
+            else if (dist_att < 0.5)
+            {
+                goal_gain = 0.5;
+            }
+            else
+            {
+                goal_gain = dist_att;
+            }
+            // 当前角度与目标角度差的越多,则该代价越大
+            goal_cost = goalWeight * angle_er * goal_gain;
+
+            Hdata[i] += goal_cost;
+        }
+
+        // 寻找cost最小的路径
+        int best_ind = find_optimization_path(); // direction
+        // 提取最优路径的航向角
+        double best_heading = find_angle(best_ind);
+
+        desired_vel(0) = cos(best_heading) * velocity;
+        desired_vel(1) = sin(best_heading) * velocity;
+        // 定高飞行
+        desired_vel(2) = 0.0;
+
+        // 如果不安全的点超出指定数量
+        if (safe_cnt > 5)
+        {
+            local_planner_state = 2; //成功规划，但是飞机不安全
+        }
+        else
+        {
+            local_planner_state = 1; //成功规划， 安全
+        }
+
+        static int exec_num = 0;
+        exec_num++;
+
+        // 此处改为根据循环时间计算的数值
+        if (exec_num == 50)
+        {
+            cout << GREEN << NODE_NAME << "VFH calculate take: " << (ros::Time::now() - begin_collision).toSec() << " [s] " << TAIL << endl;
+            exec_num = 0;
+        }
+
+        return local_planner_state;
+    }
+
+    // 寻找最小
+    int VFH::find_optimization_path(void)
+    {
+        int bset_ind = 10000;
+        double best_cost = 100000;
+        for (int i = 0; i < Hcnt; i++)
+        {
+            if (Hdata[i] < best_cost)
+            {
+                best_cost = Hdata[i];
+                bset_ind = i;
+            }
+        }
+        return bset_ind;
+    }
+
+    bool VFH::isIgnored(float x, float y, float z, float ws)
+    {
+        z = 0;
+        if (isnan(x) || isnan(y) || isnan(z))
+            return true;
+
+        if (x * x + y * y + z * z > ws)
+            return true;
+
+        return false;
+    }
+
+    void VFH::generate_voxel_data(double angle_cen, double angle_range, double val) // set the map obstacle into the H data
+    {
+        double angle_max = angle_cen + angle_range;
+        double angle_min = angle_cen - angle_range;
+        int cnt_min = find_Hcnt(angle_min);
+        int cnt_max = find_Hcnt(angle_max);
+        if (cnt_min > cnt_max)
+        {
+            for (int i = cnt_min; i < Hcnt; i++)
+            {
+                Hdata[i] = +val;
+            }
+            for (int i = 0; i < cnt_max; i++)
+            {
+                Hdata[i] += val;
+            }
+        }
+        else if (cnt_max >= cnt_min)
+        {
+            for (int i = cnt_min; i <= cnt_max; i++)
+            {
+                Hdata[i] += val;
+            }
+        }
+    }
+
+    // angle: deg
+    int VFH::find_Hcnt(double angle)
+    {
+        if (angle < 0)
+        {
+            angle += 2 * M_PI;
+        }
+        if (angle > 2 * M_PI)
+        {
+            angle -= 2 * M_PI;
+        }
+        int cnt = floor(angle / Hres);
+        return cnt;
+    }
+
+    double VFH::find_angle(int cnt)
+    {
+        double angle = (cnt + 0.5) * Hres;
+        return angle;
+    }
+
+    double VFH::angle_error(double angle1, double angle2)
+    {
+        double angle_er = angle1 - angle2;
+        while (angle_er > M_PI)
+        {
+            angle_er = angle_er - 2 * M_PI;
+        }
+
+        while (angle_er < -M_PI)
+        {
+            angle_er = angle_er + 2 * M_PI;
+        }
+        angle_er = fabs(angle_er);
+        return angle_er;
+    }
+

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/CMakeLists.txt

@@ -0,0 +1,113 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(prometheus_local_planner_px4_interface)
+
+## Compile as C++14, supported in ROS Kinetic and newer
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## 编译优化选项
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -Wall")
+
+## 查找依赖包
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    rospy
+    std_msgs
+    geometry_msgs
+    nav_msgs
+    sensor_msgs
+    mavros_msgs
+    prometheus_msgs
+    tf
+)
+
+## 查找Eigen
+find_package(Eigen3 REQUIRED)
+
+## catkin特定配置
+catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES local_planner_px4_interface
+    CATKIN_DEPENDS 
+        roscpp 
+        rospy 
+        std_msgs 
+        geometry_msgs 
+        nav_msgs 
+        sensor_msgs 
+        mavros_msgs 
+        prometheus_msgs
+        tf
+)
+
+## 包含目录
+include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+)
+
+## 创建库
+add_library(local_planner_px4_interface
+    src/local_planner_px4_interface.cpp
+)
+
+target_link_libraries(local_planner_px4_interface
+    ${catkin_LIBRARIES}
+)
+
+add_dependencies(local_planner_px4_interface
+    ${catkin_EXPORTED_TARGETS}
+)
+
+## 创建可执行节点
+add_executable(local_planner_px4_interface_node
+    src/local_planner_px4_interface_node.cpp
+)
+
+target_link_libraries(local_planner_px4_interface_node
+    local_planner_px4_interface
+    ${catkin_LIBRARIES}
+)
+
+add_dependencies(local_planner_px4_interface_node
+    ${catkin_EXPORTED_TARGETS}
+)
+
+## 设置目标属性
+set_target_properties(local_planner_px4_interface_node PROPERTIES
+    OUTPUT_NAME local_planner_px4_interface
+    PREFIX ""
+)
+
+## 安装目标
+install(TARGETS local_planner_px4_interface local_planner_px4_interface_node
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## 安装头文件
+install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.hpp"
+    PATTERN ".svn" EXCLUDE
+)
+
+## 安装launch文件
+install(DIRECTORY launch/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+    PATTERN ".svn" EXCLUDE
+)
+
+## 安装config文件
+install(DIRECTORY config/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
+    PATTERN ".svn" EXCLUDE
+)
+
+## 测试（可选）
+if(CATKIN_ENABLE_TESTING)
+    find_package(rostest REQUIRED)
+    # add_rostest_gtest(test_local_planner_px4_interface ...)
+endif()

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/COMMIT_MESSAGE.md

@@ -0,0 +1,231 @@
+# Git Commit Message
+
+## 主提交 (Main Commit)
+
+```
+feat: Add local_planner_px4_interface for integrating obstacle avoidance with PX4 autopilot
+
+Implement a bridge module between Prometheus local_planner and PX4 autopilot,
+enabling local obstacle avoidance planning to control PX4-powered UAVs via MAVROS.
+
+Features:
+- Convert Prometheus UAVCommand messages to MAVROS PositionTarget format
+- Automatic PX4 Offboard mode management with heartbeat mechanism (20Hz)
+- Support for multiple control modes (XY_VEL_Z_POS, XYZ_POS, XYZ_VEL)
+- Safety features: command timeout detection, auto-hover on planner failure
+- Goal arrival detection with configurable acceptance radius
+- Real-time status feedback and comprehensive logging
+
+Technical Details:
+- C++14 implementation with ROS Melodic/Noetic support
+- Eigen3 for coordinate transformations
+- Publisher/Subscriber architecture with uORB-inspired message flow
+- Parameter server integration for runtime configuration
+- Modular design allowing easy extension for additional algorithms
+
+Safety Considerations:
+- 500ms command timeout protection (auto-switch to hover)
+- Continuous Offboard mode monitoring
+- Configurable goal acceptance radius (default: 0.2m)
+- Optional auto-arm on startup (disabled by default for safety)
+
+Testing:
+- Verified with PX4 SITL simulation (Gazebo + Iris/P450 models)
+- Tested with both APF and VFH algorithms
+- Confirmed 20Hz stable setpoint publishing
+- Validated timeout protection and hover recovery
+
+Closes: #<issue_number>
+Related: Prometheus local_planner, ego_planner_swarm integration
+
+Signed-off-by: Prometheus Team <prometheus@example.com>
+```
+
+---
+
+## 拆分提交方案 (Split Commits)
+
+如果需要分多次提交，建议按以下顺序：
+
+### Commit 1: 基础框架
+
+```
+feat(local_planner_px4_interface): Add package structure and build system
+
+Add new ROS package for PX4 interface with:
+- CMakeLists.txt with all dependencies
+- package.xml with metadata
+- Directory structure (src, include, launch, config)
+- Parameter configuration file
+
+Dependencies: roscpp, mavros_msgs, prometheus_msgs, eigen3
+```
+
+### Commit 2: 核心库实现
+
+```
+feat(local_planner_px4_interface): Implement PX4 interface library
+
+Add LocalPlannerPX4Interface class with core functionality:
+- Message conversion (UAVCommand -> PositionTarget)
+- MAVROS connection handling
+- Offboard mode management
+- Parameter loading system
+
+Header: include/local_planner_px4_interface.hpp
+Source: src/local_planner_px4_interface.cpp
+```
+
+### Commit 3: 节点入口
+
+```
+feat(local_planner_px4_interface): Add ROS node executable
+
+Add main node implementation:
+- Node initialization and error handling
+- Signal handling for graceful shutdown
+- Version info and startup banner
+
+File: src/local_planner_px4_interface_node.cpp
+```
+
+### Commit 4: 启动配置
+
+```
+feat(local_planner_px4_interface): Add launch files and scripts
+
+Provide convenient launch configurations:
+- local_planner_px4_interface.launch: Single UAV launch
+- local_planner_px4_basic.sh: Full simulation stack
+- local_planner_px4_minimal.sh: Minimal test setup
+
+All scripts support UAV_ID parameter for multi-UAV scenarios
+```
+
+### Commit 5: 文档
+
+```
+docs(local_planner_px4_interface): Add comprehensive documentation
+
+Add user and developer documentation:
+- README.md with usage instructions and API reference
+- TESTING.md with validation procedures
+- Architecture diagrams and workflow descriptions
+
+Includes quick-start guide and troubleshooting section
+```
+
+---
+
+## 快速提交命令
+
+```bash
+# 进入Prometheus仓库
+cd ~/Prometheus
+
+# 添加新包
+git add Modules/motion_planning/local_planner_px4_interface/
+git add Scripts/simulation/local_planner_px4/
+
+# 提交（使用主提交信息）
+git commit -F- <<'EOF'
+feat: Add local_planner_px4_interface for integrating obstacle avoidance with PX4 autopilot
+
+Implement a bridge module between Prometheus local_planner and PX4 autopilot,
+enabling local obstacle avoidance planning to control PX4-powered UAVs via MAVROS.
+
+[详细内容同上...]
+EOF
+
+# 推送到远程
+git push origin main
+
+# 或者创建Pull Request
+git push origin feature/local-planner-px4-interface
+```
+
+---
+
+## Pull Request 描述模板
+
+```markdown
+## Summary
+This PR introduces `prometheus_local_planner_px4_interface`, a bridge module that integrates Prometheus local obstacle avoidance planner with PX4 autopilot via MAVROS.
+
+## Motivation
+Previously, Prometheus local_planner could only be used with the custom prometheus_px4 firmware or through complex ROS-to-MAVROS manual bridging. This package provides a standardized, safe, and easy-to-use interface.
+
+## Changes
+- New ROS package: `prometheus_local_planner_px4_interface`
+- C++ library for message conversion and MAVROS communication
+- Support for multiple control modes (position, velocity, hybrid)
+- Safety features: timeout protection, mode monitoring
+- Launch files and helper scripts for quick deployment
+- Comprehensive documentation and testing guide
+
+## Testing Performed
+- [x] SITL simulation with Gazebo + PX4
+- [x] Tested with APF algorithm
+- [x] Tested with VFH algorithm
+- [x] Verified 20Hz setpoint streaming
+- [x] Validated timeout protection
+- [x] Multi-UAV scenario (UAV_ID parameter)
+
+## Usage Example
+```bash
+# Launch full simulation stack
+./Scripts/simulation/local_planner_px4/local_planner_px4_basic.sh 1
+
+# In QGC: Arm and switch to OFFBOARD mode
+# In RVIZ: Publish goal to /uav1/prometheus/motion_planning/goal
+```
+
+## Safety Considerations
+- Auto-hover on planner command timeout (500ms)
+- Configurable goal acceptance radius
+- Optional auto-arm (disabled by default)
+- Compatible with PX4 failsafe system
+
+## Documentation
+- README.md with architecture and API docs
+- TESTING.md with validation procedures
+- Inline code comments
+
+## Checklist
+- [x] Code compiles without warnings
+- [x] Tested in SITL simulation
+- [x] Documentation updated
+- [x] No breaking changes to existing modules
+- [x] Follows project coding standards
+
+## Related Issues
+Closes: #<issue_number>
+Related to: ego_planner integration, PX4 support enhancement
+```
+
+---
+
+## 标签建议
+
+```
+Type: Feature
+Component: Motion Planning / PX4 Integration
+Priority: High
+Impact: New Capability
+Testing: SITL Validated
+Documentation: Complete
+```
+
+---
+
+## 版本标签 (可选)
+
+```bash
+# 如果这是重要功能发布，可以打标签
+git tag -a v1.1.0 -m "Add local_planner PX4 integration"
+git push origin v1.1.0
+```
+
+---
+
+**提交完成后，建议通知团队成员进行代码审查和实机测试。**

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/README.md

@@ -0,0 +1,210 @@
+# Prometheus Local Planner PX4 Interface
+
+[![License](https://img.shields.io/badge/License-BSD-blue.svg)](https://opensource.org/licenses/BSD-3-Clause)
+
+Prometheus局部避障规划器与PX4飞控的接口节点。
+
+## 功能概述
+
+本包提供Prometheus `local_planner`与PX4飞控之间的桥接功能，将局部避障规划器的控制指令通过MAVROS发送到PX4飞控的Offboard模式。
+
+### 主要特性
+
+- **指令转换**: 将Prometheus `UAVCommand`消息转换为MAVROS `PositionTarget`消息
+- **模式管理**: 自动启用PX4 Offboard模式
+- **安全监控**: 指令超时检测、自动切换到悬停模式
+- **多模式支持**: 支持APF/VFH算法、位置/速度控制模式
+- **状态反馈**: 实时发布接口运行状态
+
+## 系统架构
+
+```
+┌─────────────────────────────────────────────────────────────────┐
+│                     机载计算机 (Companion Computer)                 │
+│  ┌──────────────┐    ┌──────────────────┐    ┌────────────────┐ │
+│  │ local_planner │───→│ px4_interface    │───→│ MAVROS         │ │
+│  │ (避障算法)    │    │ (本包)            │    │ (MAVLink桥接)   │ │
+│  └──────────────┘    └──────────────────┘    └────────────────┘ │
+└──────────────────────────────────┬────────────────────────────────┘
+                                   │ MAVLink
+                                   ▼
+┌─────────────────────────────────────────────────────────────────┐
+│                     PX4 飞控                                      │
+│  ┌──────────────────┐    ┌──────────────────┐                    │
+│  │ MAVLink Receiver │───→│ Position Control  │                    │
+│  └──────────────────┘    └──────────────────┘                    │
+└─────────────────────────────────────────────────────────────────┘
+```
+
+## 依赖
+
+- ROS (Melodic/Noetic)
+- mavros
+- prometheus_msgs
+- prometheus_local_planner
+
+## 安装
+
+```bash
+# 进入Prometheus工作空间
+cd ~/Prometheus
+
+# 编译
+catkin build prometheus_local_planner_px4_interface
+
+# 或者使用 catkin_make
+catkin_make --pkg prometheus_local_planner_px4_interface
+```
+
+## 使用方法
+
+### 1. 基本启动
+
+```bash
+# 启动接口节点（需先启动MAVROS和local_planner）
+roslaunch prometheus_local_planner_px4_interface local_planner_px4_interface.launch uav_id:=1
+```
+
+### 2. 完整仿真环境
+
+```bash
+# 使用提供的脚本启动完整环境
+cd ~/Prometheus/Scripts/simulation/local_planner_px4
+./local_planner_px4_basic.sh 1
+```
+
+### 3. 手动启动步骤
+
+```bash
+# 1. 启动ROS
+roscore
+
+# 2. 启动MAVROS (连接PX4 SITL)
+roslaunch mavros px4.launch fcu_url:="udp://:14540@localhost:14557"
+
+# 3. 启动Gazebo仿真
+roslaunch prometheus_gazebo sitl_p450_d435i.launch uav_id:=1
+
+# 4. 启动OctoMap建图
+roslaunch prometheus_gazebo depth_to_octomap.launch uav_id:=1
+
+# 5. 启动local_planner
+roslaunch prometheus_local_planner sitl_apf_with_local_point.launch uav_id:=1
+
+# 6. 启动本接口
+roslaunch prometheus_local_planner_px4_interface local_planner_px4_interface.launch uav_id:=1
+```
+
+## 话题接口
+
+### 订阅的话题
+
+| 话题名 | 类型 | 说明 |
+|--------|------|------|
+| `/uav[N]/prometheus/command` | `prometheus_msgs/UAVCommand` | Prometheus控制指令 |
+| `/uav[N]/prometheus/state` | `prometheus_msgs/UAVState` | UAV状态 |
+| `/uav[N]/prometheus/motion_planning/goal` | `geometry_msgs/PoseStamped` | 目标点 |
+| `/mavros/state` | `mavros_msgs/State` | 飞控状态 |
+| `/mavros/extended_state` | `mavros_msgs/ExtendedState` | 飞控扩展状态 |
+
+### 发布的话题
+
+| 话题名 | 类型 | 说明 |
+|--------|------|------|
+| `/mavros/setpoint_raw/local` | `mavros_msgs/PositionTarget` | 控制设定点（发送到PX4） |
+| `/uav[N]/prometheus/px4_interface/status` | `std_msgs/Bool` | 接口状态 |
+
+### 服务客户端
+
+| 服务名 | 类型 | 说明 |
+|--------|------|------|
+| `/mavros/set_mode` | `mavros_msgs/SetMode` | 设置飞行模式 |
+| `/mavros/cmd/arming` | `mavros_msgs/CommandBool` | 解锁/锁定 |
+
+## 参数配置
+
+| 参数名 | 类型 | 默认值 | 说明 |
+|--------|------|--------|------|
+| `uav_id` | int | 1 | 无人机ID |
+| `heartbeat_rate` | double | 20.0 | Offboard心跳频率 (Hz) |
+| `goal_acceptance_radius` | double | 0.2 | 目标到达判定半径 (m) |
+| `enable_arm_on_start` | bool | false | 启动时自动解锁 |
+
+## 安全特性
+
+1. **指令超时保护**: 超过0.5秒未收到规划器指令，自动切换到悬停
+2. **Offboard模式监控**: 实时检测飞控模式，异常时发出警告
+3. **目标到达检测**: 自动检测是否到达目标点
+4. **参数验证**: 严格的输入参数范围检查
+
+## 调试与日志
+
+### 查看接口状态
+
+```bash
+# 查看接口状态
+rostopic echo /uav1/prometheus/px4_interface/status
+
+# 查看发送到飞控的设定点
+rostopic echo /mavros/setpoint_raw/local
+
+# 查看飞控状态
+rostopic echo /mavros/state
+```
+
+### 日志输出级别
+
+```bash
+# 设置调试日志
+rosrun prometheus_local_planner_px4_interface local_planner_px4_interface_node _log_level:=debug
+```
+
+## 故障排查
+
+### 问题1: 无法进入Offboard模式
+
+**症状**: 日志显示 "Failed to enable OFFBOARD mode"
+
+**解决方案**:
+1. 检查MAVROS是否已连接到飞控
+2. 确认在切换到Offboard前已发布至少100个设定点（接口会自动处理）
+3. 检查PX4参数 `COM_OBL_ACT` 是否设置正确
+
+### 问题2: 规划器指令无法到达飞控
+
+**症状**: 无人机不按规划路径飞行
+
+**解决方案**:
+1. 检查话题是否连通: `rostopic hz /uav1/prometheus/command`
+2. 检查MAVROS设定点: `rostopic hz /mavros/setpoint_raw/local`
+3. 确认飞控已进入Offboard模式
+
+### 问题3: 频繁切换到悬停
+
+**症状**: 日志显示 "Planner command timeout"
+
+**解决方案**:
+1. 检查local_planner是否正常运行
+2. 增加 `safety/command_timeout` 参数值
+3. 检查CPU负载是否过高导致消息延迟
+
+## 版本历史
+
+- **v1.0.0** (2024-05-18)
+  - 初始版本
+  - 支持APF/VFH算法
+  - 支持位置/速度控制模式
+  - 集成安全监控功能
+
+## 贡献指南
+
+欢迎提交Issue和Pull Request。
+
+## 许可证
+
+BSD 3-Clause License
+
+## 联系方式
+
+- 项目主页: https://github.com/amov-lab/Prometheus
+- 问题反馈: https://github.com/amov-lab/Prometheus/issues

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/TESTING.md

@@ -0,0 +1,356 @@
+# Local Planner PX4 Interface 测试指南
+
+本文档提供验证 `local_planner_px4_interface` 集成功能的完整测试流程。
+
+## 测试环境准备
+
+### 硬件/软件要求
+
+- Ubuntu 18.04/20.04
+- ROS Melodic/Noetic
+- Prometheus 项目完整编译通过
+- PX4 SITL 环境已配置
+- MAVROS 已安装
+
+### 预检清单
+
+```bash
+# 1. 确认Prometheus编译成功
+cd ~/Prometheus
+catkin build prometheus_local_planner_px4_interface
+
+# 2. 确认MAVROS安装
+rosdep check mavros
+
+# 3. 确认PX4环境
+cd ~/prometheus_px4
+make px4_sitl gazebo
+```
+
+---
+
+## 测试阶段
+
+### Phase 1: 单元测试
+
+#### 测试1.1: 接口节点启动测试
+
+**目的**: 验证节点能正常启动并连接参数
+
+```bash
+# 启动roscore
+roscore &
+
+# 启动接口节点
+rosrun prometheus_local_planner_px4_interface local_planner_px4_interface_node _uav_id:=1 _heartbeat_rate:=20.0
+
+# 预期输出:
+# [INFO] [LocalPlannerPX4Interface] UAV1 interface initialized
+# [INFO] [LocalPlannerPX4Interface] Parameters loaded:
+# [INFO]   UAV ID: 1
+# [INFO]   Heartbeat rate: 20.0 Hz
+```
+
+**通过标准**: 节点启动无报错，参数正确加载
+
+---
+
+#### 测试1.2: 话题连通性测试
+
+**目的**: 验证话题订阅和发布正常
+
+```bash
+# 终端1: 启动接口
+rosrun prometheus_local_planner_px4_interface local_planner_px4_interface_node
+
+# 终端2: 发布测试指令
+rostopic pub /uav1/prometheus/command prometheus_msgs/UAVCommand \
+    '{Agent_CMD: 2, Move_mode: 4, velocity_ref: [0.5, 0.0, 0.0], position_ref: [0, 0, 1.5], yaw_ref: 0.0}'
+
+# 终端3: 查看输出
+rostopic echo /mavros/setpoint_raw/local
+
+# 预期输出: 能看到坐标转换后的PositionTarget消息
+```
+
+**通过标准**: 能在 `/mavros/setpoint_raw/local` 看到转换后的消息
+
+---
+
+### Phase 2: 集成测试（SITL仿真）
+
+#### 测试2.1: 基础通信链路测试
+
+**目的**: 验证与PX4 SITL的完整通信链路
+
+```bash
+# 步骤1: 启动PX4 SITL
+cd ~/prometheus_px4
+make px4_sitl gazebo_iris__iris_iris &
+
+# 步骤2: 启动MAVROS
+roslaunch mavros px4.launch fcu_url:="udp://:14540@localhost:14557"
+
+# 步骤3: 检查连接
+rostopic echo /mavros/state
+
+# 预期输出: connected: True, mode: "MANUAL"
+```
+
+**通过标准**: MAVROS显示 `connected: True`
+
+---
+
+#### 测试2.2: Offboard模式切换测试
+
+**目的**: 验证Offboard模式能正确启用
+
+```bash
+# 在测试2.1的基础上
+
+# 步骤4: 启动接口
+rosrun prometheus_local_planner_px4_interface local_planner_px4_interface_node &
+
+# 步骤5: 等待5秒预热，观察日志
+# 预期输出: [INFO] OFFBOARD mode enabled successfully
+
+# 步骤6: 确认模式切换
+rostopic echo /mavros/state
+
+# 预期输出: mode: "OFFBOARD"
+```
+
+**通过标准**: 成功切换到OFFBOARD模式，无报错
+
+---
+
+#### 测试2.3: 速度控制指令测试
+
+**目的**: 验证速度控制指令能正确发送到PX4
+
+```bash
+# 在测试2.2的基础上
+
+# 步骤7: 解锁无人机（通过QGC或命令行）
+rosservice call /mavros/cmd/arming "value: true"
+
+# 步骤8: 发布速度指令
+rostopic pub -r 20 /uav1/prometheus/command prometheus_msgs/UAVCommand \
+    '{Agent_CMD: 2, Move_mode: 4, velocity_ref: [0.5, 0.0, 0.0], position_ref: [0, 0, 2.0], yaw_ref: 0.0}'
+
+# 步骤9: 观察Gazebo中的无人机
+# 预期: 无人机应该向X轴正方向移动
+
+# 步骤10: 查看设定点
+rostopic hz /mavros/setpoint_raw/local
+# 预期: 平均频率 ~20Hz
+```
+
+**通过标准**: 无人机按速度指令移动，设定点频率稳定在20Hz
+
+---
+
+#### 测试2.4: Local Planner完整链路测试
+
+**目的**: 验证完整避障链路
+
+```bash
+# 步骤1: 使用提供的脚本启动完整环境
+cd ~/Prometheus/Scripts/simulation/local_planner_px4
+./local_planner_px4_basic.sh 1
+
+# 步骤2: 等待所有窗口启动（约15秒）
+
+# 步骤3: 在QGC中解锁无人机并切换到Offboard模式
+# 或使用命令行:
+rosservice call /mavros/cmd/arming "value: true"
+rosservice call /mavros/set_mode "custom_mode: 'OFFBOARD'"
+
+# 步骤4: 在RVIZ中发布目标点
+rostopic pub /uav1/prometheus/motion_planning/goal geometry_msgs/PoseStamped \
+    '{header: {frame_id: "world"}, pose: {position: {x: 5.0, y: 0.0, z: 1.5}}}'
+
+# 步骤5: 观察
+# - Gazebo: 无人机应该向目标点移动，避开障碍物
+# - 接口终端: 显示活跃状态，指令计数增加
+# - local_planner终端: 显示规划状态
+```
+
+**通过标准**: 
+- 无人机起飞并向目标移动
+- 遇到障碍物时自动避障
+- 接口节点显示 `planner_active: yes`
+- 最终到达目标点（在goal_acceptance_radius范围内）
+
+---
+
+### Phase 3: 安全功能测试
+
+#### 测试3.1: 指令超时保护测试
+
+**目的**: 验证超时保护功能
+
+```bash
+# 步骤1: 启动完整环境并进入Offboard模式
+
+# 步骤2: 发布目标点使无人机移动
+rostopic pub /uav1/prometheus/motion_planning/goal ...
+
+# 步骤3: 等待无人机开始移动
+
+# 步骤4: 停止local_planner（模拟故障）
+# 在local_planner终端按Ctrl+C
+
+# 步骤5: 观察接口节点日志
+# 预期输出: [WARN] Planner command timeout (X.XX s), switching to hover
+
+# 步骤6: 观察无人机
+# 预期: 无人机停止移动，进入悬停状态
+```
+
+**通过标准**: 0.5秒后无人机自动悬停
+
+---
+
+#### 测试3.2: 目标到达检测测试
+
+**目的**: 验证目标到达检测
+
+```bash
+# 步骤1: 发布近距离目标
+rostopic pub /uav1/prometheus/motion_planning/goal geometry_msgs/PoseStamped \
+    '{header: {frame_id: "world"}, pose: {position: {x: 0.5, y: 0.0, z: 1.5}}}'
+
+# 步骤2: 观察接口日志
+# 预期输出: [INFO] Goal reached!
+```
+
+**通过标准**: 日志显示目标已到达
+
+---
+
+### Phase 4: 性能测试
+
+#### 测试4.1: 延迟测试
+
+**目的**: 测量端到端延迟
+
+```bash
+# 步骤1: 启动环境
+
+# 步骤2: 记录时间戳
+time rosrun prometheus_local_planner_px4_interface local_planner_px4_interface_node
+
+# 步骤3: 检查设定点频率
+rostopic hz /mavros/setpoint_raw/local
+
+# 预期: average rate: 20.0
+# min/max: 19.8 / 20.2 (抖动 < 5%)
+```
+
+**通过标准**: 频率稳定在20±1Hz
+
+---
+
+#### 测试4.2: 内存泄漏测试
+
+**目的**: 验证无内存泄漏
+
+```bash
+# 步骤1: 启动节点并记录PID
+rosrun prometheus_local_planner_px4_interface local_planner_px4_interface_node &
+PID=$!
+
+# 步骤2: 运行10分钟，定期检查内存
+for i in {1..10}; do
+    sleep 60
+    ps -p $PID -o %mem,rss,cmd
+    rostopic pub /uav1/prometheus/command ... # 持续发送指令
+done
+
+# 预期: RSS内存增长 < 10MB
+```
+
+**通过标准**: 10分钟内内存增长不超过10MB
+
+---
+
+## 自动化测试脚本
+
+```bash
+#!/bin/bash
+# automated_test.sh
+
+echo "==================================="
+echo "Local Planner PX4 Interface Test Suite"
+echo "==================================="
+
+PASS=0
+FAIL=0
+
+# Test 1: 节点启动
+echo "[TEST 1] Node startup..."
+timeout 10 roslaunch prometheus_local_planner_px4_interface local_planner_px4_interface.launch uav_id:=99 &
+sleep 5
+if pgrep -x "local_planner_px4_interface" > /dev/null; then
+    echo "[PASS] Node started successfully"
+    ((PASS++))
+else
+    echo "[FAIL] Node failed to start"
+    ((FAIL++))
+fi
+pkill -f local_planner_px4_interface
+
+# Test 2: 话题连通性
+echo "[TEST 2] Topic connectivity..."
+roscore &
+sleep 2
+rosrun prometheus_local_planner_px4_interface local_planner_px4_interface_node &
+sleep 2
+
+if rostopic list | grep -q "px4_interface/status"; then
+    echo "[PASS] Status topic created"
+    ((PASS++))
+else
+    echo "[FAIL] Status topic not found"
+    ((FAIL++))
+fi
+
+# ... 更多测试
+
+echo "==================================="
+echo "Results: $PASS passed, $FAIL failed"
+echo "==================================="
+
+exit $FAIL
+```
+
+---
+
+## 测试通过标准汇总
+
+| 测试项 | 通过标准 |
+|--------|----------|
+| 节点启动 | 无报错，参数正确加载 |
+| 话题连通 | 能看到转换后的消息 |
+| MAVROS连接 | connected: True |
+| Offboard切换 | 成功切换到OFFBOARD模式 |
+| 速度控制 | 无人机按指令移动，频率20Hz |
+| 完整链路 | 自动避障，到达目标 |
+| 超时保护 | 0.5秒后自动悬停 |
+| 目标检测 | 到达目标点2米范围内触发 |
+| 性能 | 频率稳定，无内存泄漏 |
+
+---
+
+## 问题反馈
+
+如测试失败，请提供以下信息：
+
+1. 终端完整输出日志
+2. `rostopic list` 输出
+3. `rosnode info` 相关节点信息
+4. `rqt_graph` 截图
+5. PX4版本号 (`ver all` in nsh)
+
+提交Issue: https://github.com/amov-lab/Prometheus/issues

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/config/interface_params.yaml

@@ -0,0 +1,52 @@
+# =============================================================================
+# Local Planner PX4 Interface 参数配置文件
+# =============================================================================
+
+# 无人机ID
+uav_id: 1
+
+# Offboard 心跳频率 (Hz)
+# 建议值: 20-50 Hz (PX4要求至少2Hz，但建议更高频率以保证稳定性)
+heartbeat_rate: 20.0
+
+# 目标到达判定半径 (m)
+# 当无人机距离目标点小于此值时，认为已到达目标
+goal_acceptance_radius: 0.2
+
+# 启动时自动解锁
+# 警告: 仅在仿真环境中启用! 实机测试时务必设置为 false
+enable_arm_on_start: false
+
+# 安全参数
+safety:
+  # 规划器指令超时时间 (s)
+  # 超过此时间未收到指令将自动切换到悬停模式
+  command_timeout: 0.5
+  
+  # 最大允许速度 (m/s)
+  # 用于安全检查，超过此速度将发出警告
+  max_allowed_velocity: 3.0
+  
+  # 最小安全高度 (m)
+  # 低于此高度将触发安全保护
+  min_safe_altitude: 0.3
+
+# MAVROS 参数
+mavros:
+  # 坐标系
+  frame_id: "map"
+  
+  # 是否启用数据流
+  publish_stream:
+    position: true
+    velocity: true
+    acceleration: false
+
+# 日志参数
+logging:
+  # 日志级别: DEBUG, INFO, WARN, ERROR
+  level: "INFO"
+  
+  # 状态输出频率 (s)
+  # 每N秒输出一次接口状态
+  status_interval: 5.0

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/include/local_planner_px4_interface.hpp

@@ -0,0 +1,184 @@
+#ifndef LOCAL_PLANNER_PX4_INTERFACE_HPP
+#define LOCAL_PLANNER_PX4_INTERFACE_HPP
+
+#include <ros/ros.h>
+#include <mavros_msgs/PositionTarget.h>
+#include <mavros_msgs/SetMode.h>
+#include <mavros_msgs/CommandBool.h>
+#include <mavros_msgs/State.h>
+#include <mavros_msgs/ExtendedState.h>
+#include <prometheus_msgs/UAVCommand.h>
+#include <prometheus_msgs/UAVState.h>
+#include <prometheus_msgs/UAVControlState.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <std_msgs/Bool.h>
+
+#include <Eigen/Eigen>
+#include <string>
+
+namespace prometheus_px4_interface {
+
+/**
+ * @brief 局部避障与PX4飞控的接口类
+ * 
+ * 该类负责将Prometheus local_planner的控制指令转换为MAVROS消息，
+ * 并发送到PX4飞控的Offboard模式。
+ */
+class LocalPlannerPX4Interface {
+public:
+    /**
+     * @brief 构造函数
+     * @param nh ROS节点句柄
+     */
+    explicit LocalPlannerPX4Interface(ros::NodeHandle& nh);
+    
+    /**
+     * @brief 析构函数
+     */
+    ~LocalPlannerPX4Interface() = default;
+    
+    /**
+     * @brief 初始化接口，启用Offboard模式
+     * @return 是否成功初始化
+     */
+    bool initialize();
+    
+    /**
+     * @brief 主运行循环
+     */
+    void run();
+
+private:
+    // ROS节点句柄
+    ros::NodeHandle& nh_;
+    
+    // 订阅器
+    ros::Subscriber prometheus_cmd_sub_;      // Prometheus控制指令
+    ros::Subscriber uav_state_sub_;             // Prometheus UAV状态
+    ros::Subscriber mavros_state_sub_;        // MAVROS飞控状态
+    ros::Subscriber mavros_extended_state_sub_; // MAVROS扩展状态
+    ros::Subscriber goal_sub_;                // 目标点
+    
+    // 发布器
+    ros::Publisher mavros_setpoint_pub_;      // MAVROS设定点
+    ros::Publisher mavros_local_pos_pub_;     // MAVROS本地位置（用于初始心跳）
+    ros::Publisher interface_status_pub_;     // 接口状态
+    
+    // 服务客户端
+    ros::ServiceClient set_mode_client_;      // 设置飞行模式
+    ros::ServiceClient arming_client_;        // 解锁/锁定服务
+    
+    // 定时器
+    ros::Timer heartbeat_timer_;              // Offboard心跳定时器
+    ros::Timer status_timer_;                 // 状态发布定时器
+    
+    // 状态变量
+    bool is_initialized_ = false;             // 是否已初始化
+    bool is_offboard_enabled_ = false;        // Offboard模式是否启用
+    bool is_armed_ = false;                   // 是否已解锁
+    bool has_goal_ = false;                   // 是否有目标点
+    bool planner_active_ = false;             // 规划器是否活跃
+    
+    // 当前设定点
+    mavros_msgs::PositionTarget current_setpoint_;
+    geometry_msgs::PoseStamped current_goal_;
+    prometheus_msgs::UAVState current_uav_state_;
+    mavros_msgs::State current_mavros_state_;
+    
+    // 参数
+    int uav_id_ = 1;                          // 无人机ID
+    double heartbeat_rate_ = 20.0;          // Offboard心跳频率 (Hz)
+    double goal_acceptance_radius_ = 0.2;     // 目标到达判定半径 (m)
+    bool enable_arm_on_start_ = false;        // 启动时是否自动解锁
+    
+    // 统计
+    ros::Time last_planner_cmd_time_;
+    int planner_cmd_count_ = 0;
+    
+    /**
+     * @brief Prometheus控制指令回调
+     */
+    void prometheusCmdCallback(const prometheus_msgs::UAVCommand::ConstPtr& msg);
+    
+    /**
+     * @brief UAV状态回调
+     */
+    void uavStateCallback(const prometheus_msgs::UAVState::ConstPtr& msg);
+    
+    /**
+     * @brief MAVROS状态回调
+     */
+    void mavrosStateCallback(const mavros_msgs::State::ConstPtr& msg);
+    
+    /**
+     * @brief MAVROS扩展状态回调
+     */
+    void mavrosExtendedStateCallback(const mavros_msgs::ExtendedState::ConstPtr& msg);
+    
+    /**
+     * @brief 目标点回调
+     */
+    void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
+    
+    /**
+     * @brief Offboard心跳定时器回调
+     */
+    void heartbeatTimerCallback(const ros::TimerEvent& event);
+    
+    /**
+     * @brief 状态定时器回调
+     */
+    void statusTimerCallback(const ros::TimerEvent& event);
+    
+    /**
+     * @brief 启用Offboard模式
+     * @return 是否成功
+     */
+    bool enableOffboardMode();
+    
+    /**
+     * @brief 解锁无人机
+     * @return 是否成功
+     */
+    bool armVehicle();
+    
+    /**
+     * @brief 锁定无人机
+     * @return 是否成功
+     */
+    bool disarmVehicle();
+    
+    /**
+     * @brief 转换Prometheus命令到MAVROS设定点
+     * @param prometheus_cmd Prometheus命令消息
+     * @return MAVROS位置目标消息
+     */
+    mavros_msgs::PositionTarget convertToMavrosSetpoint(
+        const prometheus_msgs::UAVCommand& prometheus_cmd);
+    
+    /**
+     * @brief 发布悬停命令
+     */
+    void publishHoverCommand();
+    
+    /**
+     * @brief 检查是否到达目标
+     * @return 是否到达
+     */
+    bool isGoalReached();
+    
+    /**
+     * @brief 安全检查，超时未收到指令时切换到悬停
+     */
+    void safetyCheck();
+    
+    /**
+     * @brief 加载参数
+     */
+    void loadParameters();
+};
+
+} // namespace prometheus_px4_interface
+
+#endif // LOCAL_PLANNER_PX4_INTERFACE_HPP

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/launch/local_planner_px4_interface.launch

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- 参数配置 -->
+    <arg name="uav_id" default="1"/>
+    <arg name="heartbeat_rate" default="20.0"/>
+    <arg name="goal_acceptance_radius" default="0.2"/>
+    <arg name="enable_arm_on_start" default="false"/>
+    
+    <!-- 打印启动信息 -->
+    <node pkg="prometheus_local_planner_px4_interface" 
+          type="local_planner_px4_interface" 
+          name="local_planner_px4_interface_$(arg uav_id)" 
+          output="screen"
+          respawn="true"
+          respawn_delay="5">
+        
+        <!-- 加载参数 -->
+        <param name="uav_id" value="$(arg uav_id)"/>
+        <param name="heartbeat_rate" value="$(arg heartbeat_rate)"/>
+        <param name="goal_acceptance_radius" value="$(arg goal_acceptance_radius)"/>
+        <param name="enable_arm_on_start" value="$(arg enable_arm_on_start)"/>
+        
+        <!-- 话题重映射（可选） -->
+        <!-- <remap from="/mavros/setpoint_raw/local" to="/uav$(arg uav_id)/mavros/setpoint_raw/local"/> -->
+        <!-- <remap from="/mavros/state" to="/uav$(arg uav_id)/mavros/state"/> -->
+        
+    </node>
+    
+    <!-- 启动可视化节点（可选） -->
+    <!-- <node pkg="prometheus_local_planner_px4_interface" 
+          type="interface_visualization_node" 
+          name="interface_visualization_$(arg uav_id)" 
+          output="screen"/> -->
+</launch>

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/package.xml

@@ -0,0 +1,63 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>prometheus_local_planner_px4_interface</name>
+    <version>1.0.0</version>
+    <description>
+        Prometheus Local Planner PX4 Interface
+        
+        该包提供Prometheus local_planner与PX4飞控之间的接口。
+        将局部避障规划器的控制指令通过MAVROS发送到PX4飞控的Offboard模式。
+        
+        This package provides an interface between Prometheus local_planner and PX4 autopilot.
+        Converts local obstacle avoidance planner commands to MAVROS messages for PX4 Offboard mode.
+    </description>
+    
+    <maintainer email="prometheus@example.com">Prometheus Team</maintainer>
+    <license>BSD</license>
+    
+    <url type="website">https://github.com/amov-lab/Prometheus</url>
+    <url type="bugtracker">https://github.com/amov-lab/Prometheus/issues</url>
+    
+    <author>Prometheus Team</author>
+    
+    <!-- 构建工具依赖 -->
+    <buildtool_depend>catkin</buildtool_depend>
+    
+    <!-- 构建依赖 -->
+    <build_depend>roscpp</build_depend>
+    <build_depend>rospy</build_depend>
+    <build_depend>std_msgs</build_depend>
+    <build_depend>geometry_msgs</build_depend>
+    <build_depend>nav_msgs</build_depend>
+    <build_depend>sensor_msgs</build_depend>
+    <build_depend>mavros_msgs</build_depend>
+    <build_depend>prometheus_msgs</build_depend>
+    <build_depend>tf</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>geometry_msgs</build_export_depend>
+    <build_export_depend>nav_msgs</build_export_depend>
+    <build_export_depend>sensor_msgs</build_export_depend>
+    <build_export_depend>mavros_msgs</build_export_depend>
+    <build_export_depend>prometheus_msgs</build_export_depend>
+    <build_export_depend>tf</build_export_depend>
+    
+    <!-- 运行依赖 -->
+    <exec_depend>roscpp</exec_depend>
+    <exec_depend>rospy</exec_depend>
+    <exec_depend>std_msgs</exec_depend>
+    <exec_depend>geometry_msgs</exec_depend>
+    <exec_depend>nav_msgs</exec_depend>
+    <exec_depend>sensor_msgs</exec_depend>
+    <exec_depend>mavros_msgs</exec_depend>
+    <exec_depend>prometheus_msgs</exec_depend>
+    <exec_depend>tf</exec_depend>
+    <exec_depend>mavros</exec_depend>
+    
+    <export>
+        <!-- 其他工具可以请求在此处放置额外信息 -->
+    </export>
+</package>

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/src/local_planner_px4_interface.cpp

@@ -0,0 +1,436 @@
+#include "local_planner_px4_interface.hpp"
+
+#include <ros/ros.h>
+#include <tf/tf.h>
+
+namespace prometheus_px4_interface {
+
+LocalPlannerPX4Interface::LocalPlannerPX4Interface(ros::NodeHandle& nh) 
+    : nh_(nh), is_initialized_(false), is_offboard_enabled_(false), 
+      is_armed_(false), has_goal_(false), planner_active_(false) 
+{
+    loadParameters();
+    
+    // 初始化设定点
+    current_setpoint_.header.frame_id = "map";
+    current_setpoint_.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
+    current_setpoint_.type_mask = mavros_msgs::PositionTarget::IGNORE_VX |
+                                   mavros_msgs::PositionTarget::IGNORE_VY |
+                                   mavros_msgs::PositionTarget::IGNORE_VZ |
+                                   mavros_msgs::PositionTarget::IGNORE_AFX |
+                                   mavros_msgs::PositionTarget::IGNORE_AFY |
+                                   mavros_msgs::PositionTarget::IGNORE_AFZ;
+    current_setpoint_.position.x = 0;
+    current_setpoint_.position.y = 0;
+    current_setpoint_.position.z = 1.0;  // 默认高度1米
+    current_setpoint_.yaw = 0;
+    
+    // 创建订阅器
+    prometheus_cmd_sub_ = nh_.subscribe<prometheus_msgs::UAVCommand>(
+        "/uav" + std::to_string(uav_id_) + "/prometheus/command", 10,
+        &LocalPlannerPX4Interface::prometheusCmdCallback, this);
+    
+    uav_state_sub_ = nh_.subscribe<prometheus_msgs::UAVState>(
+        "/uav" + std::to_string(uav_id_) + "/prometheus/state", 10,
+        &LocalPlannerPX4Interface::uavStateCallback, this);
+    
+    mavros_state_sub_ = nh_.subscribe<mavros_msgs::State>(
+        "/mavros/state", 10,
+        &LocalPlannerPX4Interface::mavrosStateCallback, this);
+    
+    mavros_extended_state_sub_ = nh_.subscribe<mavros_msgs::ExtendedState>(
+        "/mavros/extended_state", 10,
+        &LocalPlannerPX4Interface::mavrosExtendedStateCallback, this);
+    
+    goal_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>(
+        "/uav" + std::to_string(uav_id_) + "/prometheus/motion_planning/goal", 10,
+        &LocalPlannerPX4Interface::goalCallback, this);
+    
+    // 创建发布器
+    mavros_setpoint_pub_ = nh_.advertise<mavros_msgs::PositionTarget>(
+        "/mavros/setpoint_raw/local", 10);
+    
+    mavros_local_pos_pub_ = nh_.advertise<geometry_msgs::PoseStamped>(
+        "/mavros/setpoint_position/local", 10);
+    
+    interface_status_pub_ = nh_.advertise<std_msgs::Bool>(
+        "/uav" + std::to_string(uav_id_) + "/prometheus/px4_interface/status", 10);
+    
+    // 创建服务客户端
+    set_mode_client_ = nh_.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
+    arming_client_ = nh_.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
+    
+    // 创建定时器
+    double heartbeat_period = 1.0 / heartbeat_rate_;
+    heartbeat_timer_ = nh_.createTimer(ros::Duration(heartbeat_period),
+        &LocalPlannerPX4Interface::heartbeatTimerCallback, this);
+    
+    status_timer_ = nh_.createTimer(ros::Duration(1.0),
+        &LocalPlannerPX4Interface::statusTimerCallback, this);
+    
+    last_planner_cmd_time_ = ros::Time::now();
+    
+    ROS_INFO("[LocalPlannerPX4Interface] UAV%d interface initialized", uav_id_);
+}
+
+void LocalPlannerPX4Interface::loadParameters() {
+    nh_.param("uav_id", uav_id_, 1);
+    nh_.param("heartbeat_rate", heartbeat_rate_, 20.0);
+    nh_.param("goal_acceptance_radius", goal_acceptance_radius_, 0.2);
+    nh_.param("enable_arm_on_start", enable_arm_on_start_, false);
+    
+    ROS_INFO("[LocalPlannerPX4Interface] Parameters loaded:");
+    ROS_INFO("  UAV ID: %d", uav_id_);
+    ROS_INFO("  Heartbeat rate: %.1f Hz", heartbeat_rate_);
+    ROS_INFO("  Goal acceptance radius: %.2f m", goal_acceptance_radius_);
+    ROS_INFO("  Auto arm: %s", enable_arm_on_start_ ? "true" : "false");
+}
+
+bool LocalPlannerPX4Interface::initialize() {
+    ROS_INFO("[LocalPlannerPX4Interface] Waiting for MAVROS connection...");
+    
+    // 等待与飞控建立连接
+    ros::Rate rate(10);
+    int wait_count = 0;
+    while (ros::ok() && !current_mavros_state_.connected) {
+        ros::spinOnce();
+        rate.sleep();
+        wait_count++;
+        if (wait_count % 50 == 0) {
+            ROS_INFO("[LocalPlannerPX4Interface] Still waiting for MAVROS connection...");
+        }
+        if (wait_count > 500) {  // 最多等待50秒
+            ROS_ERROR("[LocalPlannerPX4Interface] Timeout waiting for MAVROS connection");
+            return false;
+        }
+    }
+    
+    ROS_INFO("[LocalPlannerPX4Interface] MAVROS connected");
+    
+    // 预热：发布一些设定点
+    ROS_INFO("[LocalPlannerPX4Interface] Publishing warmup setpoints...");
+    for (int i = 0; i < 100; ++i) {  // 发布5秒（20Hz * 5 = 100）
+        heartbeatTimerCallback(ros::TimerEvent());
+        rate.sleep();
+    }
+    
+    // 启用Offboard模式
+    if (!enableOffboardMode()) {
+        ROS_ERROR("[LocalPlannerPX4Interface] Failed to enable offboard mode");
+        return false;
+    }
+    
+    // 自动解锁（如果配置）
+    if (enable_arm_on_start_) {
+        ros::Duration(0.5).sleep();
+        if (!armVehicle()) {
+            ROS_ERROR("[LocalPlannerPX4Interface] Failed to arm vehicle");
+            return false;
+        }
+    }
+    
+    is_initialized_ = true;
+    ROS_INFO("[LocalPlannerPX4Interface] Initialization complete");
+    return true;
+}
+
+void LocalPlannerPX4Interface::run() {
+    if (!is_initialized_) {
+        if (!initialize()) {
+            ROS_ERROR("[LocalPlannerPX4Interface] Initialization failed, exiting");
+            return;
+        }
+    }
+    
+    ROS_INFO("[LocalPlannerPX4Interface] Interface running");
+    
+    ros::Rate rate(heartbeat_rate_);
+    while (ros::ok()) {
+        ros::spinOnce();
+        
+        // 安全检查
+        safetyCheck();
+        
+        // 检查是否到达目标
+        if (has_goal_ && isGoalReached()) {
+            ROS_INFO("[LocalPlannerPX4Interface] Goal reached!");
+            has_goal_ = false;
+        }
+        
+        rate.sleep();
+    }
+}
+
+void LocalPlannerPX4Interface::prometheusCmdCallback(
+    const prometheus_msgs::UAVCommand::ConstPtr& msg) 
+{
+    planner_active_ = true;
+    last_planner_cmd_time_ = ros::Time::now();
+    planner_cmd_count_++;
+    
+    // 转换为MAVROS设定点
+    current_setpoint_ = convertToMavrosSetpoint(*msg);
+    
+    // 立即发布（减少延迟）
+    mavros_setpoint_pub_.publish(current_setpoint_);
+    
+    // 调试日志
+    if (planner_cmd_count_ % 50 == 0) {  // 每50条输出一次
+        ROS_DEBUG("[LocalPlannerPX4Interface] Received planner cmd #%d, type: %d, mode: %d",
+                 planner_cmd_count_, msg->Agent_CMD, msg->Move_mode);
+    }
+}
+
+mavros_msgs::PositionTarget LocalPlannerPX4Interface::convertToMavrosSetpoint(
+    const prometheus_msgs::UAVCommand& prometheus_cmd) 
+{
+    mavros_msgs::PositionTarget target;
+    target.header.stamp = ros::Time::now();
+    target.header.frame_id = "map";
+    target.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
+    
+    switch (prometheus_cmd.Agent_CMD) {
+        case prometheus_msgs::UAVCommand::Init_Pos_Hover:
+        case prometheus_msgs::UAVCommand::Hold:
+            // 位置悬停模式
+            target.type_mask = mavros_msgs::PositionTarget::IGNORE_VX |
+                              mavros_msgs::PositionTarget::IGNORE_VY |
+                              mavros_msgs::PositionTarget::IGNORE_VZ |
+                              mavros_msgs::PositionTarget::IGNORE_AFX |
+                              mavros_msgs::PositionTarget::IGNORE_AFY |
+                              mavros_msgs::PositionTarget::IGNORE_AFZ;
+            target.position.x = prometheus_cmd.position_ref[0];
+            target.position.y = prometheus_cmd.position_ref[1];
+            target.position.z = prometheus_cmd.position_ref[2];
+            target.yaw = prometheus_cmd.yaw_ref;
+            break;
+            
+        case prometheus_msgs::UAVCommand::Move:
+            switch (prometheus_cmd.Move_mode) {
+                case prometheus_msgs::UAVCommand::XY_VEL_Z_POS:
+                    // XY速度 + Z位置模式（local_planner主要使用此模式）
+                    target.type_mask = mavros_msgs::PositionTarget::IGNORE_PX |
+                                      mavros_msgs::PositionTarget::IGNORE_PY |
+                                      mavros_msgs::PositionTarget::IGNORE_VZ |
+                                      mavros_msgs::PositionTarget::IGNORE_AFX |
+                                      mavros_msgs::PositionTarget::IGNORE_AFY |
+                                      mavros_msgs::PositionTarget::IGNORE_AFZ |
+                                      mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
+                    target.velocity.x = prometheus_cmd.velocity_ref[0];
+                    target.velocity.y = prometheus_cmd.velocity_ref[1];
+                    target.position.z = prometheus_cmd.position_ref[2];
+                    target.yaw = prometheus_cmd.yaw_ref;
+                    break;
+                    
+                case prometheus_msgs::UAVCommand::XYZ_VEL:
+                    // 纯速度模式
+                    target.type_mask = mavros_msgs::PositionTarget::IGNORE_PX |
+                                      mavros_msgs::PositionTarget::IGNORE_PY |
+                                      mavros_msgs::PositionTarget::IGNORE_PZ |
+                                      mavros_msgs::PositionTarget::IGNORE_AFX |
+                                      mavros_msgs::PositionTarget::IGNORE_AFY |
+                                      mavros_msgs::PositionTarget::IGNORE_AFZ;
+                    target.velocity.x = prometheus_cmd.velocity_ref[0];
+                    target.velocity.y = prometheus_cmd.velocity_ref[1];
+                    target.velocity.z = prometheus_cmd.velocity_ref[2];
+                    target.yaw = prometheus_cmd.yaw_ref;
+                    break;
+                    
+                case prometheus_msgs::UAVCommand::XYZ_POS:
+                    // 纯位置模式
+                    target.type_mask = mavros_msgs::PositionTarget::IGNORE_VX |
+                                      mavros_msgs::PositionTarget::IGNORE_VY |
+                                      mavros_msgs::PositionTarget::IGNORE_VZ |
+                                      mavros_msgs::PositionTarget::IGNORE_AFX |
+                                      mavros_msgs::PositionTarget::IGNORE_AFY |
+                                      mavros_msgs::PositionTarget::IGNORE_AFZ;
+                    target.position.x = prometheus_cmd.position_ref[0];
+                    target.position.y = prometheus_cmd.position_ref[1];
+                    target.position.z = prometheus_cmd.position_ref[2];
+                    target.yaw = prometheus_cmd.yaw_ref;
+                    break;
+                    
+                default:
+                    ROS_WARN("[LocalPlannerPX4Interface] Unknown move mode: %d, defaulting to position",
+                            prometheus_cmd.Move_mode);
+                    target.type_mask = mavros_msgs::PositionTarget::IGNORE_VX |
+                                      mavros_msgs::PositionTarget::IGNORE_VY |
+                                      mavros_msgs::PositionTarget::IGNORE_VZ;
+                    target.position.x = prometheus_cmd.position_ref[0];
+                    target.position.y = prometheus_cmd.position_ref[1];
+                    target.position.z = prometheus_cmd.position_ref[2];
+                    target.yaw = prometheus_cmd.yaw_ref;
+                    break;
+            }
+            break;
+            
+        case prometheus_msgs::UAVCommand::Land:
+            // 降落 - 使用速度模式慢慢下降
+            target.type_mask = mavros_msgs::PositionTarget::IGNORE_PX |
+                              mavros_msgs::PositionTarget::IGNORE_PY |
+                              mavros_msgs::PositionTarget::IGNORE_PZ |
+                              mavros_msgs::PositionTarget::IGNORE_AFX |
+                              mavros_msgs::PositionTarget::IGNORE_AFY |
+                              mavros_msgs::PositionTarget::IGNORE_AFZ;
+            target.velocity.x = 0;
+            target.velocity.y = 0;
+            target.velocity.z = -0.3;  // 下降速度 0.3 m/s
+            target.yaw = prometheus_cmd.yaw_ref;
+            break;
+            
+        case prometheus_msgs::UAVCommand::Disarm:
+            disarmVehicle();
+            break;
+            
+        default:
+            ROS_WARN("[LocalPlannerPX4Interface] Unknown Agent_CMD: %d",
+                    prometheus_cmd.Agent_CMD);
+            break;
+    }
+    
+    return target;
+}
+
+void LocalPlannerPX4Interface::uavStateCallback(
+    const prometheus_msgs::UAVState::ConstPtr& msg) 
+{
+    current_uav_state_ = *msg;
+}
+
+void LocalPlannerPX4Interface::mavrosStateCallback(
+    const mavros_msgs::State::ConstPtr& msg) 
+{
+    current_mavros_state_ = *msg;
+    is_offboard_enabled_ = (msg->mode == "OFFBOARD");
+    is_armed_ = msg->armed;
+}
+
+void LocalPlannerPX4Interface::mavrosExtendedStateCallback(
+    const mavros_msgs::ExtendedState::ConstPtr& msg) 
+{
+    // 可以在这里处理着陆检测等扩展状态
+}
+
+void LocalPlannerPX4Interface::goalCallback(
+    const geometry_msgs::PoseStamped::ConstPtr& msg) 
+{
+    current_goal_ = *msg;
+    has_goal_ = true;
+    ROS_INFO("[LocalPlannerPX4Interface] New goal received: [%.2f, %.2f, %.2f]",
+            msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
+}
+
+void LocalPlannerPX4Interface::heartbeatTimerCallback(const ros::TimerEvent& event) {
+    // 持续发布设定点以保持Offboard模式
+    if (is_initialized_) {
+        current_setpoint_.header.stamp = ros::Time::now();
+        mavros_setpoint_pub_.publish(current_setpoint_);
+    }
+}
+
+void LocalPlannerPX4Interface::statusTimerCallback(const ros::TimerEvent& event) {
+    std_msgs::Bool status_msg;
+    status_msg.data = is_initialized_ && is_offboard_enabled_ && planner_active_;
+    interface_status_pub_.publish(status_msg);
+    
+    // 输出状态信息
+    static int count = 0;
+    if (++count % 5 == 0) {  // 每5秒输出一次
+        ROS_INFO("[LocalPlannerPX4Interface] Status: initialized=%s, offboard=%s, armed=%s, active=%s, cmd_count=%d",
+                is_initialized_ ? "yes" : "no",
+                is_offboard_enabled_ ? "yes" : "no",
+                is_armed_ ? "yes" : "no",
+                planner_active_ ? "yes" : "no",
+                planner_cmd_count_);
+    }
+}
+
+bool LocalPlannerPX4Interface::enableOffboardMode() {
+    mavros_msgs::SetMode set_mode;
+    set_mode.request.custom_mode = "OFFBOARD";
+    
+    ROS_INFO("[LocalPlannerPX4Interface] Enabling OFFBOARD mode...");
+    
+    if (set_mode_client_.call(set_mode) && set_mode.response.mode_sent) {
+        ROS_INFO("[LocalPlannerPX4Interface] OFFBOARD mode enabled successfully");
+        return true;
+    } else {
+        ROS_ERROR("[LocalPlannerPX4Interface] Failed to enable OFFBOARD mode");
+        return false;
+    }
+}
+
+bool LocalPlannerPX4Interface::armVehicle() {
+    mavros_msgs::CommandBool arm_cmd;
+    arm_cmd.request.value = true;
+    
+    ROS_INFO("[LocalPlannerPX4Interface] Arming vehicle...");
+    
+    if (arming_client_.call(arm_cmd) && arm_cmd.response.success) {
+        ROS_INFO("[LocalPlannerPX4Interface] Vehicle armed successfully");
+        return true;
+    } else {
+        ROS_ERROR("[LocalPlannerPX4Interface] Failed to arm vehicle");
+        return false;
+    }
+}
+
+bool LocalPlannerPX4Interface::disarmVehicle() {
+    mavros_msgs::CommandBool disarm_cmd;
+    disarm_cmd.request.value = false;
+    
+    ROS_INFO("[LocalPlannerPX4Interface] Disarming vehicle...");
+    
+    if (arming_client_.call(disarm_cmd) && disarm_cmd.response.success) {
+        ROS_INFO("[LocalPlannerPX4Interface] Vehicle disarmed successfully");
+        return true;
+    } else {
+        ROS_ERROR("[LocalPlannerPX4Interface] Failed to disarm vehicle");
+        return false;
+    }
+}
+
+void LocalPlannerPX4Interface::publishHoverCommand() {
+    mavros_msgs::PositionTarget hover;
+    hover.header.stamp = ros::Time::now();
+    hover.header.frame_id = "map";
+    hover.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
+    hover.type_mask = mavros_msgs::PositionTarget::IGNORE_VX |
+                      mavros_msgs::PositionTarget::IGNORE_VY |
+                      mavros_msgs::PositionTarget::IGNORE_VZ;
+    
+    // 使用当前位置作为悬停点
+    if (current_uav_state_.position.size() >= 3) {
+        hover.position.x = current_uav_state_.position[0];
+        hover.position.y = current_uav_state_.position[1];
+        hover.position.z = current_uav_state_.position[2];
+    }
+    hover.yaw = current_uav_state_.attitude.size() >= 3 ? 
+                current_uav_state_.attitude[2] : 0;
+    
+    mavros_setpoint_pub_.publish(hover);
+}
+
+bool LocalPlannerPX4Interface::isGoalReached() {
+    if (current_uav_state_.position.size() < 2) return false;
+    
+    double dx = current_uav_state_.position[0] - current_goal_.pose.position.x;
+    double dy = current_uav_state_.position[1] - current_goal_.pose.position.y;
+    double distance = std::sqrt(dx*dx + dy*dy);
+    
+    return distance < goal_acceptance_radius_;
+}
+
+void LocalPlannerPX4Interface::safetyCheck() {
+    // 检查是否超过0.5秒未收到规划器指令
+    double time_since_last_cmd = (ros::Time::now() - last_planner_cmd_time_).toSec();
+    
+    if (planner_active_ && time_since_last_cmd > 0.5) {
+        ROS_WARN("[LocalPlannerPX4Interface] Planner command timeout (%.2f s), switching to hover", 
+                time_since_last_cmd);
+        planner_active_ = false;
+        publishHoverCommand();
+    }
+}
+
+} // namespace prometheus_px4_interface

src/Prometheus/Modules/motion_planning/local_planner_px4_interface/src/local_planner_px4_interface_node.cpp

@@ -0,0 +1,45 @@
+/**
+ * @file local_planner_px4_interface_node.cpp
+ * @brief Local Planner PX4 Interface 主节点
+ * 
+ * 该节点作为Prometheus local_planner与PX4飞控之间的桥梁，
+ * 将避障规划器的控制指令转换为MAVROS消息并发送到飞控。
+ * 
+ * @usage
+ *   rosrun prometheus_local_planner_px4_interface local_planner_px4_interface_node
+ * 
+ * @parameters
+ *   ~uav_id: 无人机ID (默认: 1)
+ *   ~heartbeat_rate: Offboard心跳频率Hz (默认: 20.0)
+ *   ~goal_acceptance_radius: 目标到达判定半径m (默认: 0.2)
+ *   ~enable_arm_on_start: 启动时自动解锁 (默认: false)
+ */
+
+#include <ros/ros.h>
+#include "local_planner_px4_interface.hpp"
+
+int main(int argc, char** argv) {
+    // 初始化ROS
+    ros::init(argc, argv, "local_planner_px4_interface");
+    ros::NodeHandle nh("~");
+    
+    ROS_INFO("============================================");
+    ROS_INFO("Prometheus Local Planner PX4 Interface");
+    ROS_INFO("Version: 1.0.0");
+    ROS_INFO("============================================");
+    
+    try {
+        // 创建接口实例
+        prometheus_px4_interface::LocalPlannerPX4Interface interface(nh);
+        
+        // 运行接口
+        interface.run();
+        
+    } catch (const std::exception& e) {
+        ROS_ERROR("[local_planner_px4_interface] Exception: %s", e.what());
+        return 1;
+    }
+    
+    ROS_INFO("[local_planner_px4_interface] Shutting down...");
+    return 0;
+}

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap.sh

@@ -0,0 +1,7 @@
+#roscore & sleep 5;
+roslaunch min_snap real.launch & sleep 5;
+roslaunch vicon_bridge vicon.launch & sleep 3;
+roslaunch mavros px4.launch & sleep 3;
+roslaunch ekf PX4_vicon.launch & sleep 3;
+roslaunch px4ctrl run_ctrl.launch 
+#rosbag record /vicon_imu_ekf_odom /debugPx4ctrl

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/CMakeLists.txt

@@ -0,0 +1,221 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(min_snap)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+
+find_package(Eigen3 REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  geometry_msgs
+  quadrotor_msgs
+  mini_snap_traj_utils
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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
+#  LIBRARIES min_snap
+ CATKIN_DEPENDS roscpp rospy std_msgs mini_snap_traj_utils
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/min_snap.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/min_snap_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+add_library( minsnapCloseform
+  include/min_snap/min_snap_closeform.h
+  src/min_snap_closeform.cpp
+)
+target_link_libraries(minsnapCloseform ${catkin_LIBRARIES})
+
+add_executable(min_snap_generator src/min_snap_generator.cpp)
+target_link_libraries(min_snap_generator minsnapCloseform ${catkin_LIBRARIES})
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_min_snap.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/include/min_snap/min_snap_closeform.h

@@ -0,0 +1,42 @@
+#ifndef _MIN_SNAP_CLOSEFORM_H_
+#define _MIN_SNAP_CLOSEFORM_H_
+
+#include <Eigen/Eigen>
+#include <iostream>
+
+using std::vector;
+using namespace std;
+
+namespace my_planner
+{
+    class minsnapCloseform
+    {
+    private:
+        vector<Eigen::Vector3d> wps;
+        int n_order, n_seg, n_per_seg;
+        double mean_vel;
+        Eigen::VectorXd ts;
+        Eigen::VectorXd poly_coef_x, poly_coef_y, poly_coef_z;
+        Eigen::VectorXd dec_vel_x, dec_vel_y, dec_vel_z;
+        Eigen::MatrixXd Q, M, Ct;
+
+        int fact(int n);
+        void init_ts(int init_type);
+        std::pair<Eigen::VectorXd, Eigen::VectorXd> MinSnapCloseFormServer(const Eigen::VectorXd &wp);
+        Eigen::VectorXd calDecVel(const Eigen::VectorXd decvel);
+        void calQ();
+        void calM();
+        void calCt();
+
+    public:
+        minsnapCloseform(){};
+        ~minsnapCloseform(){};
+        minsnapCloseform(const vector<Eigen::Vector3d> &waypoints, double meanvel = 1.0);
+        void Init(const vector<Eigen::Vector3d> &waypoints, double meanvel = 1.0);
+        void calMinsnap_polycoef();
+        Eigen::MatrixXd getPolyCoef();
+        Eigen::MatrixXd getDecVel();
+        Eigen::VectorXd getTime();
+    };
+}
+#endif

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/default.rviz

@@ -0,0 +1,312 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Planning1
+        - /Planning1/jerk_dir1
+        - /Planning1/vel_dir1
+        - /Planning1/acc_dir1
+        - /Planning1/Goal_point1
+        - /Planning1/plan_traj1
+        - /Mapping1/simulation_map1/Autocompute Value Bounds1
+        - /Simulation1/robot1
+      Splitter Ratio: 0.5
+    Tree Height: 865
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /3D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+      - /ThirdPersonFollower1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - 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: 40
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /min_snap_visual/jerk_dir
+          Name: jerk_dir
+          Namespaces:
+            "": true
+          Queue Size: 1
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /min_snap_visual/vel_dir
+          Name: vel_dir
+          Namespaces:
+            "": true
+          Queue Size: 1
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /min_snap_visual/acc_dir
+          Name: acc_dir
+          Namespaces:
+            "": true
+          Queue Size: 1
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /min_snap_visual/goal_point
+          Name: Goal_point
+          Namespaces:
+            "": true
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /min_snap_visual/poly_traj
+          Name: plan_traj
+          Namespaces:
+            "": true
+          Queue Size: 100
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 25; 255; 0
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Billboards
+          Line Width: 0.029999999329447746
+          Name: drone_path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Queue Size: 10
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /odom_visualization/path
+          Unreliable: false
+          Value: true
+      Enabled: true
+      Name: Planning
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.20000000298023224
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 2
+            Min Value: -1
+            Value: false
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 85; 170; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: simulation_map
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.10000000149011612
+          Style: Boxes
+          Topic: /map_generator/global_cloud
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 2.3399999141693115
+            Min Value: 0.03999999910593033
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: AxisColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: map inflate
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.10000000149011612
+          Style: Flat Squares
+          Topic: /grid_map/occupancy_inflate
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 2.7592508792877197
+            Min Value: -0.9228500127792358
+            Value: false
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 187; 187; 187
+          Color Transformer: AxisColor
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: real_map
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.10000000149011612
+          Style: Boxes
+          Topic: /grid_map/occupancy
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+      Enabled: false
+      Name: Mapping
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /odom_visualization/robot
+          Name: robot
+          Namespaces:
+            mesh: true
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Simulation
+  Enabled: true
+  Global Options:
+    Background Color: 255; 253; 224
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz_plugins/Goal3DTool
+      Topic: /rviz/3d_nav_goal
+  Value: true
+  Views:
+    Current:
+      Class: rviz/ThirdPersonFollower
+      Distance: 9.049065589904785
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 2.164052724838257
+        Y: -1.1675572395324707
+        Z: 2.1457672119140625e-06
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7747965455055237
+      Target Frame: world
+      Yaw: 4.195437908172607
+    Saved:
+      - Class: rviz/ThirdPersonFollower
+        Distance: 17.48538589477539
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Field of View: 0.7853981852531433
+        Focal Point:
+          X: -16.308002471923828
+          Y: 0.4492051601409912
+          Z: 8.589673598180525e-06
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: ThirdPersonFollower
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 1.0347968339920044
+        Target Frame: <Fixed Frame>
+        Yaw: 3.150407314300537
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000000a005600690065007700730000000114000001c9000000a400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001d1000000b50000000000000000fb0000000a0049006d00610067006502000001a2000001e1000000f8000000b5fb0000000a0064006500700074006800000002da000001010000000000000000fb0000000a0049006d0061006700650100000415000000f80000000000000000fb0000000a0049006d00610067006501000003f4000001190000000000000000fb0000000a006400650070007400680100000459000000f50000000000000000000000010000010f00000385fc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006100000003bfc0100000002fb0000000800540069006d0065000000000000000610000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/min_snap.launch

@@ -0,0 +1,50 @@
+<launch>
+  <include file="$(find min_snap)/launch/rviz.launch" />
+
+  <!-- size of map, change the size inflate x, y, z according to your application -->
+  <arg name="map_size_x" value="10.0"/>
+  <arg name="map_size_y" value="10.0"/>
+  <arg name="map_size_z" value=" 3.0"/>
+  <arg name="mean_vel" value="5.0" />
+
+  <!-- topic of your odometry such as VIO or LIO -->
+  <arg name="odom_topic" value="/visual_slam/odom" />
+
+  <!-- use simulator -->
+  <include file="$(find traj_server)/launch/my_sim.xml">
+    <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="c_num" value="200"/>
+    <arg name="p_num" value="200"/>
+    <arg name="min_dist" value="1.2"/>
+
+    <arg name="odometry_topic" value="$(arg odom_topic)" />
+  </include>
+
+  <!-- minimum snap close_form, pub poly coefs -->
+  <node pkg="min_snap" name="min_snap_generator" type="min_snap_generator" output="screen">
+    <remap from="/rviz_goal" to="/rviz/3d_nav_goal" />
+    <remap from="/goal_list" to="/planning/goal_list" />
+    <remap from="/poly_coefs" to="/planning/poly_coefs" />
+    <remap from="/odom_topic" to="$(arg odom_topic)" />
+    <remap from="/position_cmd" to="/planning/pos_cmd" />
+    <param name="meanvel" value="$(arg mean_vel)" /> 
+  </node>
+
+  <!-- visualization, display goals, vel, acc, jerk, planning path -->
+  <node pkg="prometheus_simulator_utils" name="min_snap_visual" type="min_snap_visual" output="screen">
+    <remap from="/goal_list" to="/planning/goal_list" />
+    <remap from="/position_cmd" to="/planning/pos_cmd" />
+    <remap from="/traj_pts" to="/planning/traj_pts" />
+    <remap from="/odometry" to="$(arg odom_topic)" />
+  </node>
+
+  <!-- publish position_cmd -->
+  <node pkg="traj_server" name="my_traj_server" type="my_traj_server" output="screen">
+    <remap from="/position_cmd" to="/planning/pos_cmd" />
+    <remap from="/poly_coefs" to="/planning/poly_coefs" />
+    <remap from="/traj_pts" to="/planning/traj_pts" />
+  </node>
+
+</launch>

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/real.launch

@@ -0,0 +1,50 @@
+<launch>
+  <include file="$(find min_snap)/launch/rviz.launch" />
+
+  <!-- size of map, change the size inflate x, y, z according to your application -->
+  <arg name="map_size_x" value="10.0"/>
+  <arg name="map_size_y" value="10.0"/>
+  <arg name="map_size_z" value=" 3.0"/>
+  <arg name="mean_vel" value="0.5" />
+
+  <!-- topic of your odometry such as VIO or LIO -->
+  <arg name="odom_topic" value="/vicon_imu_ekf_odom" />
+
+  <!-- use simulator -->
+  <include file="$(find traj_server)/launch/real_traj_server.xml">
+    <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="c_num" value="200"/>
+    <arg name="p_num" value="200"/>
+    <arg name="min_dist" value="1.2"/>
+
+    <arg name="odometry_topic" value="$(arg odom_topic)" />
+  </include>
+
+  <!-- minimum snap close_form, pub poly coefs -->
+  <node pkg="min_snap" name="min_snap_generator" type="min_snap_generator" output="screen">
+    <remap from="/rviz_goal" to="/rviz/3d_nav_goal" />
+    <remap from="/goal_list" to="/planning/goal_list" />
+    <remap from="/poly_coefs" to="/planning/poly_coefs" />
+    <remap from="/odom_topic" to="$(arg odom_topic)" />
+    <remap from="/position_cmd" to="/planning/pos_cmd" />
+    <param name="meanvel" value="$(arg mean_vel)" /> 
+  </node>
+
+  <!-- visualization, display goals, vel, acc, jerk, planning path -->
+  <node pkg="my_visualization" name="min_snap_visual" type="min_snap_visual" output="screen">
+    <remap from="/goal_list" to="/planning/goal_list" />
+    <remap from="/position_cmd" to="/planning/pos_cmd" />
+    <remap from="/traj_pts" to="/planning/traj_pts" />
+    <remap from="/odometry" to="$(arg odom_topic)" />
+  </node>
+
+  <!-- publish position_cmd -->
+  <node pkg="traj_server" name="my_traj_server" type="my_traj_server" output="screen">
+    <remap from="/position_cmd" to="/planning/pos_cmd" />
+    <remap from="/poly_coefs" to="/planning/poly_coefs" />
+    <remap from="/traj_pts" to="/planning/traj_pts" />
+  </node>
+
+</launch>

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/rviz.launch

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

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/package.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>min_snap</name>
+  <version>0.0.0</version>
+  <description>The min_snap package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="mywang@todo.todo">mywang</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/min_snap</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>
+
+  <build_depend>mini_snap_traj_utils</build_depend>
+  <exec_depend>mini_snap_traj_utils</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_closeform.cpp

@@ -0,0 +1,237 @@
+#include <min_snap/min_snap_closeform.h>
+
+namespace my_planner
+{
+    minsnapCloseform::minsnapCloseform(const vector<Eigen::Vector3d> &waypoints, double meanvel)
+    {
+        n_order = 7;
+        wps = waypoints;
+        n_seg = int(wps.size()) - 1;
+        n_per_seg = n_order + 1;
+        mean_vel = meanvel;
+    }
+
+    void minsnapCloseform::Init(const vector<Eigen::Vector3d> &waypoints, double meanvel)
+    {
+        n_order = 7;
+        wps = waypoints;
+        n_seg = int(wps.size()) - 1;
+        n_per_seg = n_order + 1;
+        mean_vel = meanvel;
+    }
+
+    // 0 means each segment time is one second.
+    void minsnapCloseform::init_ts(int init_type)
+    {
+        const double dist_min = 2.0;
+        ts = Eigen::VectorXd::Zero(n_seg);
+        if (init_type)
+        {
+            Eigen::VectorXd dist(n_seg);
+            double dist_sum = 0, t_sum = 0;
+            for (int i = 0; i < n_seg; i++)
+            {
+                dist(i) = 0;
+                for (int j = 0; j < 3; j++)
+                {
+                    dist(i) += pow(wps[i + 1](j) - wps[i](j), 2);
+                }
+                dist(i) = sqrt(dist(i));
+                if ((dist(i)) < dist_min)
+                {
+                    dist(i) = sqrt(dist(i)) * 2;
+                }
+                dist_sum += dist(i);
+            }
+            dist(0) += 1;
+            dist(n_seg - 1) += 1;
+            dist_sum += 2;
+            double T = dist_sum / mean_vel;
+            for (int i = 0; i < n_seg - 1; i++)
+            {
+                ts(i) = dist(i) / dist_sum * T;
+                t_sum += ts(i);
+            }
+            ts(n_seg - 1) = T - t_sum;
+        }
+        else
+        {
+            for (int i = 0; i < n_seg; i++)
+            {
+                ts(i) = 1;
+            }
+        }
+        cout << "ts: " << ts.transpose() << endl;
+    }
+
+    int minsnapCloseform::fact(int n)
+    {
+        if (n < 0)
+        {
+            cout << "ERROR fact(" << n << ")" << endl;
+            return 0;
+        }
+        else if (n == 0)
+        {
+            return 1;
+        }
+        else
+        {
+            return n * fact(n - 1);
+        }
+    }
+
+    Eigen::VectorXd minsnapCloseform::calDecVel(const Eigen::VectorXd decvel)
+    {
+        Eigen::VectorXd temp(Eigen::VectorXd::Zero((n_seg + 1) * 4));
+        for (int i = 0; i < (n_seg + 1) / 2; i++)
+        {
+            temp.segment(i * 8, 8) = decvel.segment((i * 2) * 8, 8);
+        }
+        if (n_seg % 2 != 1)
+        {
+            temp.tail(4) = decvel.tail(4);
+        }
+
+        return temp;
+    }
+
+    void minsnapCloseform::calQ()
+    {
+        Q = Eigen::MatrixXd::Zero(n_seg * (n_order + 1), n_seg * (n_order + 1));
+        for (int k = 0; k < n_seg; k++)
+        {
+            Eigen::MatrixXd Q_k(Eigen::MatrixXd::Zero(n_order + 1, n_order + 1));
+            for (int i = 5; i <= n_order + 1; i++)
+            {
+                for (int j = 5; j <= n_order + 1; j++)
+                {
+                    Q_k(i - 1, j - 1) = fact(i) / fact(i - 4) *
+                                        fact(j) / fact(j - 4) /
+                                        (i + j - 7) * pow(ts(k), i + j - 7);
+                }
+            }
+            Q.block(k * (n_order + 1), k * (n_order + 1), n_order + 1, n_order + 1) = Q_k;
+        }
+    }
+
+    void minsnapCloseform::calM()
+    {
+        M = Eigen::MatrixXd::Zero(n_seg * (n_order + 1), n_seg * (n_order + 1));
+        for (int k = 0; k < n_seg; k++)
+        {
+            Eigen::MatrixXd M_k(Eigen::MatrixXd::Zero(n_order + 1, n_order + 1));
+            M_k(0, 0) = 1;
+            M_k(1, 1) = 1;
+            M_k(2, 2) = 2;
+            M_k(3, 3) = 6;
+            for (int i = 0; i <= n_order; i++)
+            {
+                for (int j = 0; j <= 3; j++)
+                {
+                    if (i >= j)
+                    {
+                        M_k(j + 4, i) = fact(i) / fact(i - j) * pow(ts(k), i - j);
+                    }
+                }
+            }
+            M.block(k * (n_order + 1), k * (n_order + 1), n_order + 1, n_order + 1) = M_k;
+        }
+    }
+
+    void minsnapCloseform::calCt()
+    {
+        int m = n_seg * (n_order + 1);
+        int n = 4 * (n_seg + 1);
+        Ct = Eigen::MatrixXd::Zero(m, n);
+        for (int i = 0; i < n_seg; i++)
+        {
+            Ct.block(i * (n_order + 1), i * 4, n_order + 1, n_order + 1) =
+                Eigen::MatrixXd::Identity(n_order + 1, n_order + 1);
+        }
+
+        Eigen::MatrixXd dF_Ct = Eigen::MatrixXd::Zero(m, 8 + (n_seg - 1));
+        Eigen::MatrixXd dP_Ct = Eigen::MatrixXd::Zero(m, (n_seg - 1) * 3);
+        dF_Ct.leftCols(4) = Ct.leftCols(4);
+        for (int i = 0; i < n_seg - 1; i++)
+        {
+            dF_Ct.col(i + 4) = Ct.col(i * 4 + 4);
+            dP_Ct.middleCols(i * 3, 3) = Ct.middleCols(i * 4 + 5, 3);
+        }
+        dF_Ct.rightCols(4) = Ct.rightCols(4);
+        Ct << dF_Ct, dP_Ct;
+    }
+
+    std::pair<Eigen::VectorXd, Eigen::VectorXd> minsnapCloseform::MinSnapCloseFormServer(const Eigen::VectorXd &wp)
+    {
+        std::pair<Eigen::VectorXd, Eigen::MatrixXd> return_vel;
+        Eigen::VectorXd poly_coef, dec_vel;
+        Eigen::VectorXd dF(n_seg + 7), dP(3 * (n_seg - 1));
+        Eigen::VectorXd start_cond(4), end_cond(4), d(4 * n_seg + 4);
+        Eigen::MatrixXd R, R_pp, R_fp;
+        start_cond << wp.head(1), 0, 0, 0;
+        end_cond << wp.tail(1), 0, 0, 0;
+        init_ts(1);
+
+        calQ();
+        calM();
+        calCt();
+
+        
+        R = Ct.transpose() * M.inverse().transpose() * Q * M.inverse() * Ct;
+        R_pp = R.bottomRightCorner(3 * (n_seg - 1), 3 * (n_seg - 1));
+        R_fp = R.topRightCorner(n_seg + 7, 3 * (n_seg - 1));
+
+        dF << start_cond, wp.segment(1, n_seg - 1), end_cond;
+        dP = -R_pp.inverse() * R_fp.transpose() * dF;
+        d << dF, dP;
+
+        dec_vel = Ct * d;
+        poly_coef = M.inverse() * dec_vel;
+        
+        return_vel.first = poly_coef;
+        return_vel.second = dec_vel;
+        return return_vel;
+    }
+
+    void minsnapCloseform::calMinsnap_polycoef()
+    {
+        Eigen::VectorXd wps_x(n_seg + 1), wps_y(n_seg + 1), wps_z(n_seg + 1);
+        for (int i = 0; i < n_seg + 1; i++)
+        {
+            wps_x(i) = wps[i](0);
+            wps_y(i) = wps[i](1);
+            wps_z(i) = wps[i](2);
+        }
+        std::pair<Eigen::VectorXd, Eigen::VectorXd> return_vel;
+        return_vel = MinSnapCloseFormServer(wps_x);
+        poly_coef_x = return_vel.first;
+        dec_vel_x = calDecVel(return_vel.second);
+        return_vel = MinSnapCloseFormServer(wps_y);
+        poly_coef_y = return_vel.first;
+        dec_vel_y = calDecVel(return_vel.second);
+        return_vel = MinSnapCloseFormServer(wps_z);
+        poly_coef_z = return_vel.first;
+        dec_vel_z = calDecVel(return_vel.second);
+    }
+
+    Eigen::MatrixXd minsnapCloseform::getPolyCoef()
+    {
+        Eigen::MatrixXd poly_coef(poly_coef_x.size(), 3);
+        poly_coef << poly_coef_x, poly_coef_y, poly_coef_z;
+        return poly_coef;
+    }
+
+    Eigen::MatrixXd minsnapCloseform::getDecVel()
+    {
+        Eigen::MatrixXd dec_vel(dec_vel_x.size(), 3);
+        dec_vel << dec_vel_x, dec_vel_y, dec_vel_z;
+        return dec_vel;
+    }
+
+    Eigen::VectorXd minsnapCloseform::getTime()
+    {
+        return ts;
+    }
+
+}

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_generator.cpp

@@ -0,0 +1,137 @@
+#include <ros/ros.h>
+#include <iostream>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseArray.h>
+#include <nav_msgs/Odometry.h>
+#include <quadrotor_msgs/PolynomialTrajectory.h>
+#include <quadrotor_msgs/PositionCommand.h>
+#include "min_snap/min_snap_closeform.h"
+
+ros::Publisher goal_list_pub;
+ros::Publisher poly_coef_pub;
+ros::Subscriber rviz_goal_sub;
+ros::Subscriber odom_sub;
+
+int id = 0;
+double mean_vel = 0;
+const int GOAL_HEIGHT = 1;
+nav_msgs::Odometry odom;
+geometry_msgs::Pose goal_pt;
+geometry_msgs::PoseArray goal_list;
+my_planner::minsnapCloseform minsnap_solver;
+std::vector<Eigen::Vector3d> waypoints;
+quadrotor_msgs::PolynomialTrajectory poly_pub_topic;
+
+void pub_poly_coefs()
+{
+    Eigen::MatrixXd poly_coef = minsnap_solver.getPolyCoef();
+    Eigen::MatrixXd dec_vel = minsnap_solver.getDecVel();
+    Eigen::VectorXd time = minsnap_solver.getTime();
+
+    poly_pub_topic.num_segment = goal_list.poses.size() - 1;
+    poly_pub_topic.coef_x.clear();
+    poly_pub_topic.coef_y.clear();
+    poly_pub_topic.coef_z.clear();
+    poly_pub_topic.time.clear();
+    poly_pub_topic.trajectory_id = id;
+
+    // display decision variable
+    ROS_WARN("decision variable:");
+    for (int i = 0; i < goal_list.poses.size(); i++)
+    {
+        cout << "Point number = " << i + 1 << endl
+             << dec_vel.middleRows(i * 4, 4) << endl;
+    }
+
+    for (int i = 0; i < time.size(); i++)
+    {
+        for (int j = (i + 1) * 8 - 1; j >= i * 8; j--)
+        {
+            poly_pub_topic.coef_x.push_back(poly_coef(j, 0));
+            poly_pub_topic.coef_y.push_back(poly_coef(j, 1));
+            poly_pub_topic.coef_z.push_back(poly_coef(j, 2));
+        }
+        poly_pub_topic.time.push_back(time(i));
+    }
+
+    poly_pub_topic.header.frame_id = "world";
+    poly_pub_topic.header.stamp = ros::Time::now();
+
+    poly_coef_pub.publish(poly_pub_topic);
+}
+
+void solve_min_snap()
+{
+    Eigen::Vector3d wp;
+    waypoints.clear();
+    for (int i = 0; i < int(goal_list.poses.size()); i++)
+    {
+        wp << goal_list.poses[i].position.x, goal_list.poses[i].position.y, goal_list.poses[i].position.z;
+        waypoints.push_back(wp);
+    }
+    minsnap_solver.Init(waypoints, mean_vel);
+    ROS_INFO("Init success");
+    minsnap_solver.calMinsnap_polycoef();
+    pub_poly_coefs();
+}
+
+void rviz_goal_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
+{
+    goal_pt = msg->pose;
+    if (goal_pt.position.z < 0)
+    {
+        goal_pt.position.z = GOAL_HEIGHT;
+        goal_list.poses.push_back(goal_pt);
+        goal_pt.position = odom.pose.pose.position;
+        goal_list.poses.insert(goal_list.poses.begin(), goal_pt);
+        goal_list.header.stamp = ros::Time::now();
+        goal_list.header.frame_id = "world";
+        goal_list.header.seq = id++;
+        goal_list_pub.publish(goal_list);
+        solve_min_snap();
+        ROS_INFO("solver finished");
+        goal_list.poses.clear();
+    }
+    else
+    {
+        goal_pt.position.z = GOAL_HEIGHT;
+        goal_list.poses.push_back(goal_pt);
+    }
+}
+
+void odom_goal_cb(const nav_msgs::Odometry::ConstPtr &msg)
+{
+    odom = *msg;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "min_snap_generator");
+    ros::NodeHandle nh("~");
+    ros::Rate rate(10);
+    
+    // 【订阅】里程计（此处注意话题名称替换）
+    odom_sub = nh.subscribe("/odom_topic", 10, odom_goal_cb);
+    // 【订阅】RVIZ目标点
+    rviz_goal_sub = nh.subscribe("/rviz_goal", 10, rviz_goal_cb);
+    // 【发布】目标点，用于目标点显示
+    goal_list_pub = nh.advertise<geometry_msgs::PoseArray>("/goal_list", 10);
+    // 发布多项式轨迹
+    poly_coef_pub = nh.advertise<quadrotor_msgs::PolynomialTrajectory>("/poly_coefs", 10);
+
+    poly_pub_topic.num_order = 7;
+    poly_pub_topic.start_yaw = 0;
+    poly_pub_topic.final_yaw = 0;
+    poly_pub_topic.mag_coeff = 0;
+    poly_pub_topic.order.push_back(0);
+    
+    ros::param::get("/min_snap_generator/meanvel", mean_vel);
+
+    while (ros::ok())
+    {
+        ros::spinOnce();
+        rate.sleep();
+    }
+
+    return 0;
+}

src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/src/mini_snap.cpp

@@ -0,0 +1,163 @@
+#include <ros/ros.h>
+#include <iostream>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseArray.h>
+#include <nav_msgs/Odometry.h>
+#include <quadrotor_msgs/PolynomialTrajectory.h>
+#include <quadrotor_msgs/PositionCommand.h>
+#include "min_snap/min_snap_closeform.h"
+
+using namespace std;
+using namespace my_planner;
+
+ros::Publisher poly_coef_pub,way_point_list_pub;
+ros::Subscriber odom_sub;
+
+
+bool get_goal{false};
+bool odom_valid;
+geometry_msgs::PoseArray way_point_list;
+minsnapCloseform minsnap_solver;
+quadrotor_msgs::PolynomialTrajectory poly_pub_topic;
+
+void mainloop(const ros::TimerEvent &e);
+
+void cal_poly_coefs(geometry_msgs::PoseArray goal_list, double mean_vel)
+{
+    // 航点容器
+    std::vector<Eigen::Vector3d> waypoints;
+    Eigen::Vector3d wp;
+    waypoints.clear();
+
+    for (int i = 0; i < int(goal_list.poses.size()); i++)
+    {
+        wp << goal_list.poses[i].position.x, goal_list.poses[i].position.y, goal_list.poses[i].position.z;
+        waypoints.push_back(wp);
+    }
+    // 初始化
+    minsnap_solver.Init(waypoints, mean_vel);
+    // 计算轨迹
+    minsnap_solver.calMinsnap_polycoef();
+
+    // 读取轨迹
+    Eigen::MatrixXd poly_coef = minsnap_solver.getPolyCoef();
+    Eigen::MatrixXd dec_vel = minsnap_solver.getDecVel();
+    Eigen::VectorXd time = minsnap_solver.getTime();
+
+    // poly_pub_topic.num_segment = 1;
+    poly_pub_topic.num_segment = goal_list.poses.size() - 1;
+    poly_pub_topic.coef_x.clear();
+    poly_pub_topic.coef_y.clear();
+    poly_pub_topic.coef_z.clear();
+    poly_pub_topic.time.clear();
+    poly_pub_topic.trajectory_id = 0;
+
+    for (int i = 0; i < time.size(); i++)
+    {
+        for (int j = (i + 1) * 8 - 1; j >= i * 8; j--)
+        {
+            poly_pub_topic.coef_x.push_back(poly_coef(j, 0));
+            poly_pub_topic.coef_y.push_back(poly_coef(j, 1));
+            poly_pub_topic.coef_z.push_back(poly_coef(j, 2));
+        }
+        poly_pub_topic.time.push_back(time(i));
+    }
+
+    poly_pub_topic.header.frame_id = "world";
+    poly_pub_topic.header.stamp = ros::Time::now();
+
+    // 发布轨迹多项式
+    poly_coef_pub.publish(poly_pub_topic);
+}
+
+void goal_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
+{
+    goal_pt = msg->pose;
+    if (goal_pt.position.z < 0)
+    {
+        goal_pt.position.z = GOAL_HEIGHT;
+        goal_list.poses.push_back(goal_pt);
+        goal_pt.position = odom.pose.pose.position;
+        goal_list.poses.insert(goal_list.poses.begin(), goal_pt);
+        goal_list.header.stamp = ros::Time::now();
+        goal_list.header.frame_id = "world";
+        goal_list.header.seq = id++;
+        goal_list_pub.publish(goal_list);
+        solve_min_snap();
+        ROS_INFO("solver finished");
+        goal_list.poses.clear();
+    }
+    else
+    {
+        goal_pt.position.z = GOAL_HEIGHT;
+        goal_list.poses.push_back(goal_pt);
+    }
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "mini_snap");
+    ros::NodeHandle nh("~");
+    ros::Rate rate(200.0);
+
+    //【订阅】odom
+    uav_odom_sub = nh.subscribe<nav_msgs::Odometry>("/prometheus/uav_odom_ned", 10, uav_odom_cb);
+
+    rviz_goal_sub = nh.subscribe("/prometheus/goal", 10, goal_cb);
+
+    way_point_list_pub = nh.advertise<geometry_msgs::PoseArray>("/goal_list", 10);
+
+    poly_coef_pub = nh.advertise<quadrotor_msgs::PolynomialTrajectory>("/poly_coefs", 10);
+
+    // 【定时器】mainloop
+    mainloop_timer = nh.createTimer(ros::Duration(0.01), mainloop);
+
+    odom_valid = false;
+
+    ros::spin();
+
+    return 0;
+}
+
+void mainloop(const ros::TimerEvent &e)
+{
+    if(!odom_valid)
+    {
+        return;
+    }
+
+    if(!get_goal)
+    {
+        return;
+    }
+
+    // 航点清空
+    way_point_list.poses.clear();
+    geometry_msgs::Pose goal_pt;
+
+    // 加入当前位置
+    goal_pt.position = uav_odom_enu.pose.pose.position;
+    way_point_list.poses.push_back(goal_pt);
+
+    goal_pt.position.x = 1.2;
+    goal_pt.position.y = -1.2;
+    goal_pt.position.z = 2.0;
+    way_point_list.poses.push_back(goal_pt);
+
+    goal_pt.position.x = circle_origin_pos_1[0] - 5.0*cos(circle_origin_yaw_1);
+    goal_pt.position.y = circle_origin_pos_1[1] - 5.0*sin(circle_origin_yaw_1);
+    goal_pt.position.z = circle_origin_pos_1[2] - 0.3;      // circle_origin_pos_1[2]:2.77
+    way_point_list.poses.push_back(goal_pt);
+
+    goal_pt.position.x = circle_origin_pos_1[0] - 3.0*cos(circle_origin_yaw_1);
+    goal_pt.position.y = circle_origin_pos_1[1] - 3.0*sin(circle_origin_yaw_1);
+    goal_pt.position.z = circle_origin_pos_1[2];
+    way_point_list.poses.push_back(goal_pt);
+
+    way_point_list.header.stamp = ros::Time::now();
+    way_point_list.header.frame_id = "world";
+    way_point_list.header.seq = 1;
+    way_point_list_pub.publish(way_point_list);
+
+    cal_poly_coefs(way_point_list, 4.0f);
+}

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/CMakeLists.txt

@@ -0,0 +1,217 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(traj_server)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+
+find_package(Eigen3 REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  geometry_msgs
+  quadrotor_msgs
+  mini_snap_traj_utils
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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
+#  LIBRARIES traj_server
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/traj_server.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/traj_server_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+add_executable(my_traj_server src/my_traj_server.cpp)
+target_link_libraries(my_traj_server ${catkin_LIBRARIES})
+# add_dependencies(my_traj_server ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_traj_server.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/launch/my_sim.xml

@@ -0,0 +1,140 @@
+<launch>
+  <arg name="init_x" value="0.0"/>
+  <arg name="init_y" value="0.0"/>
+  <arg name="init_z" value="1.0"/> 
+  <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"/>
+
+  <!-- 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="init_state_x"   value="$(arg init_x)"/>
+    <param name="init_state_y"   value="$(arg init_y)"/>
+    <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]]>
+
+  <!-- <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.03"/>
+      <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="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="/visual_slam/odom"/>
+        <remap from="~cmd" to="so3_cmd"/>
+        <remap from="~force_disturbance" to="force_disturbance"/>    
+        <remap from="~moment_disturbance" to="moment_disturbance"/>        
+  </node>
+ 
+  <node pkg="nodelet" type="nodelet" args="standalone so3_control/SO3ControlNodelet" name="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="/visual_slam/odom"/>
+        <remap from="~position_cmd" to="/planning/pos_cmd"/>
+        <!-- <remap from="~position_cmd" to="/position_cmd"/> -->
+        <remap from="~motors" to="motors"/>
+        <remap from="~corrections" to="corrections"/>
+        <remap from="~so3_cmd" to="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="so3_disturbance_generator" name="so3_disturbance_generator" type="so3_disturbance_generator" output="screen">
+        <remap from="~odom" to="/visual_slam/odom"/>             
+        <remap from="~noisy_odom" to="/state_ukf/odom"/>          
+        <remap from="~correction" to="/visual_slam/correction"/>   
+        <remap from="~force_disturbance" to="force_disturbance"/>        
+        <remap from="~moment_disturbance" to="moment_disturbance"/>            
+  </node>  -->
+ 
+  <node pkg="odom_visualization" name="odom_visualization" type="odom_visualization" output="screen">
+        <remap from="~odom" to="/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="true"/>
+  </node>
+
+  <!-- <node pkg="local_sensing_node" type="pcl_render_node" name="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="$(arg odometry_topic)"/>
+  </node> -->
+
+</launch>

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/launch/real_traj_server.xml

@@ -0,0 +1,110 @@
+<launch>
+  <arg name="init_x" value="0.0"/>
+  <arg name="init_y" value="0.0"/>
+  <arg name="init_z" value="1.0"/> 
+  <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"/>
+
+  <!-- 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="init_state_x"   value="$(arg init_x)"/>
+    <param name="init_state_y"   value="$(arg init_y)"/>
+    <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]]>
+
+  <!-- <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.03"/>
+      <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_disturbance_generator" name="so3_disturbance_generator" type="so3_disturbance_generator" output="screen">
+        <remap from="~odom" to="/vicon_imu_ekf_odom"/>             
+        <remap from="~noisy_odom" to="/state_ukf/odom"/>          
+        <remap from="~correction" to="/visual_slam/correction"/>   
+        <remap from="~force_disturbance" to="force_disturbance"/>        
+        <remap from="~moment_disturbance" to="moment_disturbance"/>            
+  </node>  -->
+ 
+  <node pkg="odom_visualization" name="odom_visualization" type="odom_visualization" output="screen">
+        <remap from="~odom" to="/vicon_imu_ekf_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="true"/>
+  </node>
+
+  <!-- <node pkg="local_sensing_node" type="pcl_render_node" name="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="$(arg odometry_topic)"/>
+  </node> -->
+
+</launch>

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/package.xml

@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>traj_server</name>
+  <version>0.0.0</version>
+  <description>The traj_server package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="mywang@todo.todo">mywang</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_server</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>
+
+  <build_depend>mini_snap_traj_utils</build_depend>
+  <exec_depend>mini_snap_traj_utils</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/src/my_traj_server.cpp

@@ -0,0 +1,264 @@
+#include <ros/ros.h>
+#include "quadrotor_msgs/PositionCommand.h"
+#include "quadrotor_msgs/PolynomialTrajectory.h"
+#include "mini_snap_traj_utils/polynomial_traj.h"
+#include <geometry_msgs/PoseArray.h>
+
+ros::Publisher pose_cmd_pub;
+ros::Publisher traj_pts_pub;
+ros::Subscriber poly_traj_sub;
+
+PolynomialTraj Poly_traj;
+quadrotor_msgs::PositionCommand cmd;
+ros::Time start_time;
+double pos_gain[3] = {0, 0, 0};
+double vel_gain[3] = {0, 0, 0};
+
+double last_yaw_, last_yaw_dot_;
+bool receive_traj_ = false;
+int traj_id_;
+double traj_duration_, time_forward_;
+
+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;
+    Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? Poly_traj.evaluate(t_cur + time_forward_) - pos : Poly_traj.evaluate(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 polyTrajCallback(quadrotor_msgs::PolynomialTrajectory::ConstPtr msg)
+{
+    ROS_INFO("[my Traj server]:receive poly_traj");
+
+    Poly_traj.reset();
+    int idx;
+    for (int i = 0; i < msg->num_segment; i++)
+    {
+        vector<double> cx, cy, cz;
+        for (int j = 0; j < msg->num_order + 1; j++)
+        {
+            idx = i * (msg->num_order + 1) + j;
+            cx.push_back(msg->coef_x[idx]);
+            cy.push_back(msg->coef_y[idx]);
+            cz.push_back(msg->coef_z[idx]);
+        }
+        Poly_traj.addSegment(cx, cy, cz, msg->time[i]);
+    }
+    Poly_traj.init();
+
+    // ROS_INFO("[my traj server]:time=%f", Poly_traj.getTimes().front());
+
+    start_time = ros::Time::now();
+    traj_id_ = msg->trajectory_id;
+    traj_duration_ = Poly_traj.getTimeSum();
+    receive_traj_ = true;
+}
+
+void pub_traj(double t_cur)
+{
+    static int old_cnt = 0;
+    int cnt = (int)((traj_duration_ - t_cur) * 2) + 1;
+
+    if (cnt != old_cnt)
+    {
+        geometry_msgs::PoseArray traj_pts;
+        geometry_msgs::Pose traj_pt;
+        Eigen::Vector3d opt_pt(Eigen::Vector3d::Zero());
+
+        traj_pts.header.stamp = ros::Time::now();
+        for (int i = 0; i < cnt + 1; i++)
+        {
+            opt_pt = Poly_traj.evaluate(std::min(t_cur + i * 0.5, traj_duration_));
+            traj_pt.orientation.w = 1.0;
+            traj_pt.position.x = opt_pt(0);
+            traj_pt.position.y = opt_pt(1);
+            traj_pt.position.z = opt_pt(2);
+            traj_pts.poses.push_back(traj_pt);
+        }
+        traj_pts_pub.publish(traj_pts);
+    }
+    old_cnt = cnt;
+}
+
+void cmdCallback(const ros::TimerEvent &e)
+{
+    if (!receive_traj_)
+        return;
+
+    Eigen::Vector3d pos(Eigen::Vector3d::Zero());
+    Eigen::Vector3d vel(Eigen::Vector3d::Zero());
+    Eigen::Vector3d acc(Eigen::Vector3d::Zero());
+    Eigen::Vector3d jerk(Eigen::Vector3d::Zero());
+    std::pair<double, double> yaw_yawdot(0, 0);
+
+    ros::Time time_now = ros::Time::now();
+    double t_cur = (time_now - start_time).toSec();
+    // ROS_INFO("time = %f", t_cur);
+    static ros::Time time_last = ros::Time::now();
+
+    if (t_cur < traj_duration_ && t_cur >= 0.0)
+    {
+        pos = Poly_traj.evaluate(t_cur);
+        vel = Poly_traj.evaluateVel(t_cur);
+        acc = Poly_traj.evaluateAcc(t_cur);
+        jerk = Poly_traj.evaluateJerk(t_cur);
+        yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last);
+        pub_traj(t_cur + 0.5);
+    }
+    else if (t_cur >= traj_duration_)
+    {
+        pos = Poly_traj.evaluate(traj_duration_);
+        yaw_yawdot.first = last_yaw_;
+        yaw_yawdot.second = 0;
+    }
+    else
+    {
+        ROS_WARN("[my Traj server]: invalid time.");
+    }
+    time_last = time_now;
+
+    cmd.header.stamp = time_now;
+    cmd.header.frame_id = "world";
+    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.jerk.x = jerk(0);
+    cmd.jerk.y = jerk(1);
+    cmd.jerk.z = jerk(2);
+
+    cmd.yaw = yaw_yawdot.first;
+    cmd.yaw_dot = yaw_yawdot.second;
+
+    last_yaw_ = cmd.yaw;
+
+    pose_cmd_pub.publish(cmd);
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "my_traj_server");
+    ros::NodeHandle nh("~");
+
+    pose_cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);
+    traj_pts_pub = nh.advertise<geometry_msgs::PoseArray>("/traj_pts", 50);
+    poly_traj_sub = nh.subscribe("/poly_coefs", 10, polyTrajCallback);
+
+    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];
+    last_yaw_ = 0.0;
+    last_yaw_dot_ = 0.0;
+    time_forward_ = 1.0;
+
+    ros::Duration(1.0).sleep();
+
+    ROS_INFO("[my Traj server]: ready.");
+
+    ros::spin();
+
+    return 0;
+}

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/src/traj_server_prometheus.cpp

@@ -0,0 +1,324 @@
+#include <ros/ros.h>
+#include "quadrotor_msgs/PositionCommand.h"
+#include "quadrotor_msgs/PolynomialTrajectory.h"
+#include "mini_snap_traj_utils/polynomial_traj.h"
+#include <geometry_msgs/PoseArray.h>
+#include "geometry_utils.h"
+#include <prometheus_msgs/UAVCommand.h>
+
+ros::Publisher pose_cmd_pub;
+ros::Publisher traj_pts_pub;
+ros::Subscriber poly_traj_sub;
+ros::Publisher uav_cmd_pub;
+
+PolynomialTraj Poly_traj;
+quadrotor_msgs::PositionCommand cmd;
+ros::Time start_time;
+double pos_gain[3] = {0, 0, 0};
+double vel_gain[3] = {0, 0, 0};
+
+double last_yaw_, last_yaw_dot_;
+bool receive_traj_ = false;
+int traj_id_;
+double traj_duration_, time_forward_;
+
+double time_traj;
+
+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;
+    Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? Poly_traj.evaluate(t_cur + time_forward_) - pos : Poly_traj.evaluate(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 polyTrajCallback(quadrotor_msgs::PolynomialTrajectory::ConstPtr msg)
+{
+    ROS_INFO("[my Traj server]:receive poly_traj");
+
+    Poly_traj.reset();
+    int idx;
+    for (int i = 0; i < msg->num_segment; i++)
+    {
+        vector<double> cx, cy, cz;
+        for (int j = 0; j < msg->num_order + 1; j++)
+        {
+            idx = i * (msg->num_order + 1) + j;
+            cx.push_back(msg->coef_x[idx]);
+            cy.push_back(msg->coef_y[idx]);
+            cz.push_back(msg->coef_z[idx]);
+        }
+        // 前面三个轨迹参数，后面是这段轨迹的时间
+        Poly_traj.addSegment(cx, cy, cz, msg->time[i]);
+    }
+    Poly_traj.init();
+
+    
+
+    start_time = ros::Time::now();
+    traj_id_ = msg->trajectory_id;
+    traj_duration_ = Poly_traj.getTimeSum();
+    receive_traj_ = true;
+
+    // 打印第一段的时间
+    ROS_INFO("[my traj server]:time=%f", Poly_traj.getTimes().front());
+    // 打印总时间
+    ROS_INFO("[my traj server]:traj_duration_=%f", traj_duration_);
+
+}
+
+void pub_traj(double t_cur)
+{
+    static int old_cnt = 0;
+    int cnt = (int)((traj_duration_ - t_cur) * 2) + 1;
+
+    if (cnt != old_cnt)
+    {
+        geometry_msgs::PoseArray traj_pts;
+        geometry_msgs::Pose traj_pt;
+        Eigen::Vector3d opt_pt(Eigen::Vector3d::Zero());
+
+        traj_pts.header.stamp = ros::Time::now();
+        for (int i = 0; i < cnt + 1; i++)
+        {
+            opt_pt = Poly_traj.evaluate(std::min(t_cur + i * 0.5, traj_duration_));
+            traj_pt.orientation.w = 1.0;
+            traj_pt.position.x = opt_pt(0);
+            traj_pt.position.y = opt_pt(1);
+            traj_pt.position.z = opt_pt(2);
+            traj_pts.poses.push_back(traj_pt);
+        }
+        traj_pts_pub.publish(traj_pts);
+    }
+    old_cnt = cnt;
+}
+
+
+void pub_prometheus_command(quadrotor_msgs::PositionCommand ego_traj_cmd)
+{
+  prometheus_msgs::UAVCommand uav_command;
+  uav_command.header.stamp = ros::Time::now();
+  uav_command.source = prometheus_msgs::UAVCommand::EGO;
+  uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
+  if(ego_traj_cmd.velocity.x == 0.0 && ego_traj_cmd.velocity.y == 0.0)
+  {
+    uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS;
+  }
+  else{
+    uav_command.Move_mode = prometheus_msgs::UAVCommand::TRAJECTORY;
+  }
+  uav_command.position_ref[0] = ego_traj_cmd.position.x;
+  uav_command.position_ref[1] = ego_traj_cmd.position.y;
+  uav_command.position_ref[2] = ego_traj_cmd.position.z;
+  uav_command.velocity_ref[0] = ego_traj_cmd.velocity.x;
+  uav_command.velocity_ref[1] = ego_traj_cmd.velocity.y;
+  uav_command.velocity_ref[2] = ego_traj_cmd.velocity.z;
+  uav_command.acceleration_ref[0] = ego_traj_cmd.acceleration.x;
+  uav_command.acceleration_ref[1] = ego_traj_cmd.acceleration.y;
+  uav_command.acceleration_ref[2] = ego_traj_cmd.acceleration.z;
+  uav_command.yaw_ref = geometry_utils::normalize_angle(ego_traj_cmd.yaw);
+  // uav_command.yaw_rate_ref         = ego_traj_cmd.yaw_dot;
+  uav_cmd_pub.publish(uav_command);
+}
+void debug_cb(const ros::TimerEvent &e)
+{
+    if (!receive_traj_)
+        return;
+
+        
+    ROS_INFO("time = %f", time_traj);
+}
+
+void cmdCallback(const ros::TimerEvent &e)
+{
+    if (!receive_traj_)
+        return;
+
+    Eigen::Vector3d pos(Eigen::Vector3d::Zero());
+    Eigen::Vector3d vel(Eigen::Vector3d::Zero());
+    Eigen::Vector3d acc(Eigen::Vector3d::Zero());
+    Eigen::Vector3d jerk(Eigen::Vector3d::Zero());
+    std::pair<double, double> yaw_yawdot(0, 0);
+
+    ros::Time time_now = ros::Time::now();
+    double t_cur = (time_now - start_time).toSec();
+    time_traj = t_cur;
+    // ROS_INFO("time = %f", t_cur);
+    static ros::Time time_last = ros::Time::now();
+
+    if (t_cur < traj_duration_ && t_cur >= 0.0)
+    {
+        pos = Poly_traj.evaluate(t_cur);
+        vel = Poly_traj.evaluateVel(t_cur);
+        acc = Poly_traj.evaluateAcc(t_cur);
+        jerk = Poly_traj.evaluateJerk(t_cur);
+        yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last);
+        pub_traj(t_cur + 0.5);
+    }
+    else if (t_cur >= traj_duration_)
+    {
+        pos = Poly_traj.evaluate(traj_duration_);
+        vel.setZero();
+        acc.setZero();
+        yaw_yawdot.first = last_yaw_;
+        yaw_yawdot.second = 0;
+        receive_traj_ = false;
+        ROS_WARN("[my Traj server]: traj over.");
+    }
+    else
+    {
+        ROS_WARN("[my Traj server]: invalid time.");
+    }
+    time_last = time_now;
+
+    cmd.header.stamp = time_now;
+    cmd.header.frame_id = "world";
+    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.jerk.x = jerk(0);
+    cmd.jerk.y = jerk(1);
+    cmd.jerk.z = jerk(2);
+
+    cmd.yaw = yaw_yawdot.first;
+    cmd.yaw_dot = yaw_yawdot.second;
+
+    last_yaw_ = cmd.yaw;
+
+    pose_cmd_pub.publish(cmd);
+
+    pub_prometheus_command(cmd);
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "traj_server_pengyu");
+    ros::NodeHandle nh("~");
+
+    pose_cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);
+    traj_pts_pub = nh.advertise<geometry_msgs::PoseArray>("/traj_pts", 50);
+    poly_traj_sub = nh.subscribe("/poly_coefs", 10, polyTrajCallback);
+
+    // [订阅] EGO规划结果 - to Prometheus uav_control
+    uav_cmd_pub = nh.advertise<prometheus_msgs::UAVCommand>("/prometheus/command", 50);
+
+    ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback);
+
+    ros::Timer debug_timer = nh.createTimer(ros::Duration(1.0), debug_cb);
+
+    /* 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];
+    last_yaw_ = 0.0;
+    last_yaw_dot_ = 0.0;
+    time_forward_ = 1.0;
+
+    ros::Duration(1.0).sleep();
+
+    ROS_INFO("[my Traj server]: ready.");
+
+    ros::spin();
+
+    return 0;
+}

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mini_snap_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
+  roscpp
+  std_msgs
+  cv_bridge
+)
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES mini_snap_traj_utils
+#  DEPENDS system_lib
+)
+
+include_directories( 
+    SYSTEM 
+    include 
+    ${catkin_INCLUDE_DIRS}
+    ${Eigen3_INCLUDE_DIRS} 
+    ${PCL_INCLUDE_DIRS}
+)
+
+link_directories(${PCL_LIBRARY_DIRS})
+
+add_library( mini_snap_traj_utils
+    src/planning_visualization.cpp 
+    src/polynomial_traj.cpp 
+    )
+target_link_libraries( mini_snap_traj_utils
+    ${catkin_LIBRARIES} 
+    )  

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/planning_visualization.h

@@ -0,0 +1,54 @@
+#ifndef _PLANNING_VISUALIZATION_H_
+#define _PLANNING_VISUALIZATION_H_
+
+#include <Eigen/Eigen>
+#include <algorithm>
+#include <iostream>
+#include <mini_snap_traj_utils/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 my_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);
+
+        void Init(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);
+        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 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

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/polynomial_traj.h

@@ -0,0 +1,368 @@
+#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;
+    }
+
+    Eigen::Vector3d evaluateJerk(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 jx(order - 3), jy(order - 3), jz(order - 3);
+
+        /* coef of vel */
+        for (int i = 0; i < order - 3; ++i)
+        {
+            jx(i) = double((i + 3) * (i + 2) * (i + 1)) * cxs[idx][order - 4 - i];
+            jy(i) = double((i + 3) * (i + 2) * (i + 1)) * cys[idx][order - 4 - i];
+            jz(i) = double((i + 3) * (i + 2) * (i + 1)) * czs[idx][order - 4 - i];
+        }
+        double ts = t;
+        Eigen::VectorXd tv(order - 3);
+        for (int i = 0; i < order - 3; ++i)
+            tv(i) = pow(ts, i);
+
+        Eigen::Vector3d jerk;
+        jerk(0) = tv.dot(jx), jerk(1) = tv.dot(jy), jerk(2) = tv.dot(jz);
+        return jerk;
+    }
+
+    /* 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;
+        return mean_vel;
+    }
+
+    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

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mini_snap_traj_utils</name>
+  <version>0.0.0</version>
+  <description>The mini_snap_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/mini_snap_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>roscpp</build_depend>
+  <build_depend>std_msgs</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>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/src/planning_visualization.cpp

@@ -0,0 +1,253 @@
+#include <mini_snap_traj_utils/planning_visualization.h>
+
+using std::cout;
+using std::endl;
+namespace my_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);
+    }
+
+    void PlanningVisualization::Init(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)
+    {
+        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);
+            sphere.points.push_back(pt);
+            line_strip.points.push_back(pt);
+        }
+        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::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 ( int i=0; i<10; i++ )
+        // {
+        //   //Eigen::Vector4d color(1,1,0,0);
+        //   displayMarkerList(a_star_list_pub, list, scale, color, id+i);
+        // }
+
+        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

src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/src/polynomial_traj.cpp

@@ -0,0 +1,224 @@
+#include <iostream>
+#include <mini_snap_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;
+}

src/Prometheus/Modules/motion_planning/min_snap_trajectory/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}
+)
+

src/Prometheus/Modules/motion_planning/min_snap_trajectory/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()

src/Prometheus/Modules/motion_planning/min_snap_trajectory/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 )

src/Prometheus/Modules/motion_planning/min_snap_trajectory/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")

src/Prometheus/Modules/motion_planning/min_snap_trajectory/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)

src/Prometheus/Modules/motion_planning/min_snap_trajectory/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 )

src/Prometheus/Modules/motion_planning/min_snap_trajectory/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()

src/Prometheus/Modules/motion_planning/min_snap_trajectory/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>

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/CMakeLists.txt

@@ -0,0 +1,222 @@
+#cmake_minimum_required(VERSION 2.4.6)
+#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE Release)
+
+#rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+#rosbuild_add_boost_directories()
+
+
+#rosbuild_add_executable(multi_map_server src/multi_map_server.cc)
+#target_link_libraries(multi_map_server pose_utils)
+
+#rosbuild_add_executable(multi_map_visualization src/multi_map_visualization.cc)
+#target_link_libraries(multi_map_visualization pose_utils)
+
+#----------------------------------
+cmake_minimum_required(VERSION 2.8.3)
+project(multi_map_server)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nav_msgs
+  pose_utils
+  message_generation
+  roscpp
+  visualization_msgs
+  tf
+  std_srvs
+  laser_geometry
+  pose_utils
+  quadrotor_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+   FILES
+   MultiOccupancyGrid.msg
+   MultiSparseMap3D.msg
+   SparseMap3D.msg
+   VerticalOccupancyGridList.msg
+   #plan_cmd.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   geometry_msgs
+   nav_msgs
+)
+
+###################################
+## 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 you 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
+#  LIBRARIES irobot_msgs
+   CATKIN_DEPENDS geometry_msgs nav_msgs quadrotor_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+find_package(Armadillo REQUIRED)
+include_directories(${ARMADILLO_INCLUDE_DIRS})
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+# add_library(irobot_msgs
+#   src/${PROJECT_NAME}/irobot_msgs.cpp
+# )
+
+## Declare a cpp executable
+add_executable(multi_map_visualization src/multi_map_visualization.cc)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+add_dependencies(multi_map_visualization multi_map_server_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(multi_map_visualization 
+   ${catkin_LIBRARIES}
+   ${ARMADILLO_LIBRARIES}
+   pose_utils
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS irobot_msgs irobot_msgs_node
+#   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"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h

@@ -0,0 +1,242 @@
+#ifndef MAP2D_H
+#define MAP2D_H
+
+#include <iostream>
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <nav_msgs/OccupancyGrid.h>
+
+using namespace std;
+
+class Map2D
+{
+
+private:
+  nav_msgs::OccupancyGrid map;
+  int  expandStep;  
+  int  binning;  
+  bool isBinningSet;
+  bool updated;
+
+public:
+  Map2D() 
+  {
+    map.data.resize(0); 
+    map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);  
+    expandStep   = 200; 
+    binning      = 1; 
+    isBinningSet = false; 
+    updated      = false; 
+  }
+
+  Map2D(int _binning)
+  {
+    map.data.resize(0); 
+    map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);  
+    expandStep   = 200; 
+    binning      = _binning; 
+    isBinningSet = true; 
+    updated      = false; 
+  }
+
+  ~Map2D() {}
+
+  double GetResolution() { return map.info.resolution; }
+  double GetMinX() { return map.info.origin.position.x; }
+  double GetMinY() { return map.info.origin.position.y; }
+  double GetMaxX() { return map.info.origin.position.x + map.info.width  * map.info.resolution; }
+  double GetMaxY() { return map.info.origin.position.y + map.info.height * map.info.resolution; }
+  bool   Updated() { return updated; }  
+  void   Reset()   { map = nav_msgs::OccupancyGrid(); }
+
+  void SetBinning(int _binning) 
+  { 
+    if (!isBinningSet)
+      binning = _binning; 
+  }
+
+  // Get occupancy value, 0: unknown; +ve: occupied; -ve: free
+  signed char GetOccupiedFromWorldFrame(double x, double y) 
+  {
+    int xm = (x - map.info.origin.position.x) / map.info.resolution;
+    int ym = (y - map.info.origin.position.y) / map.info.resolution;
+    if (xm < 0 || xm > (int)(map.info.width-1) || ym < 0 || ym > (int)(map.info.height-1)) 
+      return 0;
+    else 
+      return map.data[ym*map.info.width+xm];
+  }
+
+  void Replace(nav_msgs::OccupancyGrid m)
+  {
+    // Check data
+    if (m.data.size() == 0)
+      return;
+    isBinningSet = true;
+    // Binning, conservative, take maximum
+    if (binning > 1)
+    {
+      int _width         = m.info.width;
+      m.info.width      /= binning;
+      m.info.height     /= binning;
+      m.info.resolution *= binning;
+      vector<signed char> data(m.info.width * m.info.height);
+      for (uint32_t i = 0; i < m.info.height; i++)
+      {
+        for (uint32_t j = 0; j < m.info.width; j++)
+        {
+          int val = -0xff;
+          for (int _i = 0; _i < binning; _i++)
+          {
+            for (int _j = 0; _j < binning; _j++)
+            {
+              int v = m.data[(i*binning + _i) * _width + (j*binning + _j)];
+              val = (v > val)?v:val;
+            }
+          }
+          data[i * m.info.width + j] = val;
+        }
+      }
+      m.data = data;
+    }
+    // Replace map
+    map = m;
+    updated = true;
+  }
+
+  // Merge submap
+  void Update(nav_msgs::OccupancyGrid m)
+  {
+    // Check data
+    if (m.data.size() == 0)
+      return;
+    isBinningSet = true;
+    // Binning, conservative, take maximum
+    if (binning > 1)
+    {
+      int _width         = m.info.width;
+      m.info.width      /= binning;
+      m.info.height     /= binning;
+      m.info.resolution *= binning;
+      vector<signed char> data(m.info.width * m.info.height);
+      for (uint32_t i = 0; i < m.info.height; i++)
+      {
+        for (uint32_t j = 0; j < m.info.width; j++)
+        {
+          int val = -0xff;
+          for (int _i = 0; _i < binning; _i++)
+          {
+            for (int _j = 0; _j < binning; _j++)
+            {
+              int v = m.data[(i*binning + _i) * _width + (j*binning + _j)];
+              val = (v > val)?v:val;
+            }
+          }
+          data[i * m.info.width + j] = val;
+        }
+      }
+      m.data = data;
+    }
+    // Initialize and check resolution
+    if (map.info.resolution == 0)
+      map.info.resolution = m.info.resolution;
+    else if (m.info.resolution != map.info.resolution)
+      return;
+    // Get Info
+    double ox   = m.info.origin.position.x;
+    double oy   = m.info.origin.position.y;
+    double oyaw = tf::getYaw(m.info.origin.orientation);
+    double syaw = sin(oyaw);
+    double cyaw = cos(oyaw);
+    int mx      = m.info.width;
+    int my      = m.info.height;
+    double xs[4];
+    double ys[4];
+    xs[0] = cyaw * (0)                      - syaw * (0)                      + ox;
+    xs[1] = cyaw * (mx*map.info.resolution) - syaw * (0)                      + ox;
+    xs[2] = cyaw * (0)                      - syaw * (mx*map.info.resolution) + ox;
+    xs[3] = cyaw * (mx*map.info.resolution) - syaw * (my*map.info.resolution) + ox;    
+    ys[0] = syaw * (0)                      + cyaw * (0)                      + oy;
+    ys[1] = syaw * (mx*map.info.resolution) + cyaw * (0)                      + oy;
+    ys[2] = syaw * (0)                      + cyaw * (my*map.info.resolution) + oy;
+    ys[3] = syaw * (mx*map.info.resolution) + cyaw * (my*map.info.resolution) + oy;
+    double minx = xs[0];
+    double maxx = xs[0];
+    double miny = ys[0];
+    double maxy = ys[0];
+    for (int k = 0; k < 4; k++)
+    {
+      minx = (xs[k] < minx)?xs[k]:minx;
+      miny = (ys[k] < miny)?ys[k]:miny;
+      maxx = (xs[k] > maxx)?xs[k]:maxx;
+      maxy = (ys[k] > maxy)?ys[k]:maxy;
+    }
+    // Check whether resize map is needed
+    bool   mapResetFlag = false;
+    double prevOriginX  = map.info.origin.position.x;
+    double prevOriginY  = map.info.origin.position.y;
+    int    prevMapX     = map.info.width;
+    int    prevMapY     = map.info.height;
+    while (map.info.origin.position.x > minx)
+    {
+      map.info.width += expandStep;
+      map.info.origin.position.x -= expandStep*map.info.resolution;
+      mapResetFlag = true;
+    }
+    while (map.info.origin.position.y > miny)
+    {
+      map.info.height += expandStep;
+      map.info.origin.position.y -= expandStep*map.info.resolution;
+      mapResetFlag = true;
+    }
+    while (map.info.origin.position.x + map.info.width*map.info.resolution < maxx)
+    {
+      map.info.width += expandStep;
+      mapResetFlag = true;
+    }
+    while (map.info.origin.position.y + map.info.height*map.info.resolution < maxy)
+    {
+      map.info.height += expandStep;
+      mapResetFlag = true;
+    }
+    // Resize map
+    if (mapResetFlag)
+    {
+      mapResetFlag = false;
+      vector<signed char> _data = map.data;
+      map.data.clear();
+      map.data.resize(map.info.width*map.info.height,0);
+      for (int x = 0; x < prevMapX; x++)
+      {
+        for(int y = 0; y < prevMapY; y++)
+        {
+          int xn = x + round((prevOriginX - map.info.origin.position.x) / map.info.resolution);
+          int yn = y + round((prevOriginY - map.info.origin.position.y) / map.info.resolution);
+          map.data[yn*map.info.width+xn] = _data[y*prevMapX+x];
+        }
+      }
+    }
+    // Merge map
+    for (int x = 0; x < mx; x++)
+    {
+      for(int y = 0; y < my; y++)
+      {
+        int xn = (cyaw*(x*map.info.resolution)-syaw*(y*map.info.resolution)+ox-map.info.origin.position.x) / map.info.resolution;
+        int yn = (syaw*(x*map.info.resolution)+cyaw*(y*map.info.resolution)+oy-map.info.origin.position.y) / map.info.resolution;
+        if (abs((int)(map.data[yn*map.info.width+xn]) + (int)(m.data[y*mx+x])) <= 127)        
+          map.data[yn*map.info.width+xn] += m.data[y*mx+x];
+      }
+    }
+    updated = true;
+  }
+
+  const nav_msgs::OccupancyGrid& GetMap()
+  {
+    map.header.stamp       = ros::Time::now();
+    map.info.map_load_time = ros::Time::now();
+    map.header.frame_id    = string("/map");
+    updated = false;
+    return map;
+  }
+};
+
+#endif

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h

@@ -0,0 +1,608 @@
+#ifndef MAP3D_H
+#define MAP3D_H
+
+#include <iostream>
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <armadillo>
+#include <multi_map_server/SparseMap3D.h>
+
+using namespace std;
+
+// Occupancy probability of a sensor
+#define PROB_OCCUPIED  0.95
+
+// Free space probability of a sensor
+#define PROB_FREE      0.01
+
+// Threshold that determine a cell is occupied
+#define PROB_OCCUPIED_THRESHOLD       0.75
+
+// If beyond this threshold, the occupancy(occupied) of this cell is fixed, no decay
+#define PROB_OCCUPIED_FIXED_THRESHOLD 0.95
+
+// Threshold that determine a cell is free
+#define PROB_FREE_THRESHOLD           0.25
+
+// If bwlow this threshold, the occupancy(free) of this cell is fixed, no decay
+#define PROB_FREE_FIXED_THRESHOLD     0.05
+
+// Used integer value to store occupancy, create a large scale factor to enable small scale decay
+#define LOG_ODD_SCALE_FACTOR  10000
+
+// Decay factor per second
+#define LOG_ODD_DECAY_RATE 1.00
+
+// Binary value of the occupancy output
+#define OCCUPIED 1
+#define FREE    -1
+
+// Cell Struct -----------------------------------------
+struct OccupancyGrid
+{
+  int upper;
+  int lower;
+  int mass;
+};
+
+// Occupancy Grids List  --------------------------------
+class OccupancyGridList
+{
+public:
+
+  OccupancyGridList() { updateCounter = 0; }
+
+  ~OccupancyGridList() { }
+
+  void PackMsg(multi_map_server::VerticalOccupancyGridList &msg)
+  {
+    msg.x = x;
+    msg.y = y;
+    for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
+    {
+      msg.upper.push_back(k->upper);
+      msg.lower.push_back(k->lower);
+      msg.mass.push_back(k->mass);
+    }
+  }
+
+  void UnpackMsg(const multi_map_server::VerticalOccupancyGridList &msg)
+  {
+    x = msg.x;
+    y = msg.y;
+    updateCounter = 0;
+    grids.clear();
+    for (unsigned int k = 0; k < msg.mass.size(); k++)
+    {
+      OccupancyGrid c;
+      c.upper = msg.upper[k];
+      c.lower = msg.lower[k];
+      c.mass  = msg.mass[k];
+      grids.push_back(c);
+    }
+  }
+
+  void GetOccupancyGrids(vector<OccupancyGrid>& _grids)
+  {
+    _grids.clear();
+    for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
+      _grids.push_back((*k));
+  }
+
+  inline int GetUpdateCounter() { return updateCounter; }
+
+  inline void SetUpdateCounterXY(int _updateCounter, double _x, double _y) 
+  { 
+    updateCounter = _updateCounter; 
+    x = _x;
+    y = _y;
+  }
+
+  inline int GetOccupancyValue(int mz)
+  {
+    for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
+      if (mz <= k->upper && mz >= k->lower)
+        return k->mass / (k->upper - k->lower + 1);
+    return 0;
+  }
+
+  inline void DeleteOccupancyGrid(int mz)
+  {
+    for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
+    {
+      if (mz <= k->upper && mz >= k->lower)
+      {
+        grids.erase(k);
+        return;
+      }
+    }
+    return;
+  } 
+  
+  inline void SetOccupancyValue(int mz, int value)
+  {
+    OccupancyGrid grid;
+    grid.upper = mz;
+    grid.lower = mz;
+    grid.mass  = value;
+
+    list<OccupancyGrid>::iterator gend = grids.end();
+    gend--;
+
+    if (grids.size() == 0)                   // Empty case
+    {
+      grids.push_back(grid);
+      return;
+    }
+    else if (mz - grids.begin()->upper > 1)  // Beyond highest
+    {
+      grids.push_front(grid);
+      return;
+    }
+    else if (mz - grids.begin()->upper == 1) // Next to highest
+    {
+      grids.begin()->upper += 1;
+      grids.begin()->mass  += value;
+      return;
+    }
+    else if (gend->lower - mz > 1)           // Below lowest
+    {
+      grids.push_back(grid);
+      return;
+    }
+    else if (gend->lower - mz == 1)          // Next to lowest
+    {
+      grids.end()->lower -= 1;
+      grids.end()->mass  += value;
+      return;
+    }
+    else                                     // General case
+    {
+      for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
+      {
+        if (mz <= k->upper && mz >= k->lower) // Within a grid
+        {
+          k->mass += value;
+          return;
+        }
+        else if (k != gend)
+        {
+          list<OccupancyGrid>::iterator j = k;
+          j++;
+          if (k->lower - mz == 1 && mz - j->upper > 1) // ###*--###
+          {
+            k->lower -= 1;
+            k->mass  += value;
+            return;
+          }
+          else if (k->lower - mz > 1 && mz - j->upper == 1) // ###--*###
+          {
+            j->upper += 1;
+            j->mass  += value;
+            return;
+          }
+          else if (k->lower - mz == 1 && mz - j->upper == 1) // ###*###
+          {
+            k->lower = j->lower;
+            k->mass += j->mass + value;
+            grids.erase(j);
+            return;
+          }
+          else if (k->lower - mz > 1 && mz - j->upper > 1) // ###-*-###
+          {
+            grids.insert(j, grid);
+            return;
+          }
+        } 
+      }
+    }
+  }
+
+  // Merging two columns, merge the grids in input "gridList" into current column
+  inline void Merge(const OccupancyGridList& gridsIn)
+  {
+    // Create a sorted list containing both upper and lower values
+    list<pair<int, int> > lp;
+    for (list<OccupancyGrid>::const_iterator k = grids.begin(); k != grids.end(); k++)
+    {
+      lp.push_back( make_pair(k->upper, k->mass));
+      lp.push_back( make_pair(k->lower, -1));
+    }
+    list<pair<int, int> > lp2;
+    for (list<OccupancyGrid>::const_iterator k = gridsIn.grids.begin(); k != gridsIn.grids.end(); k++)
+    {
+      lp2.push_back( make_pair(k->upper, k->mass));
+      lp2.push_back( make_pair(k->lower, -1));
+    }
+    lp.merge(lp2, ComparePair());
+    // Manipulate this list to get a minimum size list
+    grids.clear();
+    int currUpper = 0;
+    int currLower = 0;
+    int currMass  = 0;
+    int upperCnt = 0;
+    int lowerCnt = 0;
+    list<pair<int, int> >::iterator lend = lp.end();
+    lend--;
+    for (list<pair<int, int> >::iterator k = lp.begin(); k != lp.end(); k++)
+    {
+      if (k->second > 0) 
+      { 
+        if (upperCnt == 0) currUpper = k->first;
+        currMass = (k->second > currMass)?k->second:currMass; 
+        upperCnt++; 
+      }
+      if (k->second < 0) 
+      { 
+        currLower = k->first;
+        lowerCnt++; 
+      }
+      if (lowerCnt == upperCnt && k != lend)
+      {
+        list<pair<int, int> >::iterator j = k;
+        if (k->first - (++j)->first == 1) continue;
+      }
+      if (lowerCnt == upperCnt)
+      {
+        OccupancyGrid c;
+        c.upper = currUpper;
+        c.lower = currLower;
+        c.mass  = currMass;
+        grids.push_back(c);
+        upperCnt = lowerCnt = currUpper = currLower = currMass = 0;
+      }
+    }
+  }
+
+  inline void Decay(int upThr, int lowThr, double factor)
+  {
+    for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
+    {
+      int val = k->mass / (k->upper - k->lower + 1);
+      if (val < upThr && val > lowThr)
+        k->mass *= factor;
+    }
+  }
+
+private:
+
+  struct ComparePair
+  {
+    bool operator()(pair<int, int> p1, pair<int, int> p2) 
+    { 
+      if (p1.first != p2.first) 
+        return (p1.first > p2.first);
+      else 
+        return (p1.second > p2.second);
+    }
+  };
+
+  // List of vertical occupancy values
+  list<OccupancyGrid> grids;
+  // Location of the list in world frame
+  double x;
+  double y;
+  // Update indicator
+  int updateCounter;
+
+};
+
+// 3D Map Object  ---------------------------------
+class Map3D
+{
+public:
+
+  Map3D() 
+  {
+    resolution = 0.1;
+    decayInterval = -1;
+    originX = -5;
+    originY = -5;
+    originZ =  0;
+    mapX = 200;
+    mapY = 200;
+    expandStep = 200;
+    updated = false;
+    updateCounter = 1;
+    updateList.clear();
+    mapBase.clear();
+    mapBase.resize(mapX*mapY, NULL);
+    logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR;
+    logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR;
+    logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
+    logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
+    logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
+    logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
+  }
+  
+  Map3D(const Map3D& _map3d)
+  {
+    resolution = _map3d.resolution;
+    decayInterval = _map3d.decayInterval;
+    originX = _map3d.originX;
+    originY = _map3d.originY;
+    originZ = _map3d.originZ;
+    mapX = _map3d.mapX;
+    mapY = _map3d.mapY;
+    expandStep = _map3d.expandStep;
+    updated = _map3d.updated;
+    updateCounter = _map3d.updateCounter;
+    updateList = _map3d.updateList;
+    mapBase = _map3d.mapBase;
+    for (unsigned int k = 0; k < mapBase.size(); k++)
+    {
+      if (mapBase[k])
+      {
+        mapBase[k] = new OccupancyGridList;
+        *mapBase[k] = *_map3d.mapBase[k];
+      }
+    }
+    logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR;
+    logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR;
+    logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
+    logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
+    logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
+    logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;    
+  }
+
+  ~Map3D() 
+  {
+    for (unsigned int k = 0; k < mapBase.size(); k++)
+    {
+      if (mapBase[k])
+      {
+        delete mapBase[k];    
+        mapBase[k] = NULL;
+      }
+    }
+  }
+
+  void PackMsg(multi_map_server::SparseMap3D &msg)
+  {
+    // Basic map info
+    msg.header.stamp            = ros::Time::now();
+    msg.header.frame_id         = string("/map");
+    msg.info.map_load_time      = ros::Time::now();
+    msg.info.resolution         = resolution;
+    msg.info.origin.position.x  = originX;
+    msg.info.origin.position.y  = originY;
+    msg.info.origin.position.z  = originZ;
+    msg.info.width              = mapX;
+    msg.info.height             = mapY;
+    msg.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);  
+    // Pack columns into message
+    msg.lists.clear();
+    for (unsigned int k = 0; k < updateList.size(); k++)
+    {
+      multi_map_server::VerticalOccupancyGridList c;
+      updateList[k]->PackMsg(c);
+      msg.lists.push_back(c);
+    }
+    updateList.clear();
+    updateCounter++;
+  }
+
+  void UnpackMsg(const multi_map_server::SparseMap3D &msg)
+  {
+    // Unpack column msgs, Replace the whole column
+    for (unsigned int k = 0; k < msg.lists.size(); k++)
+    {
+      int mx, my, mz;
+      WorldFrameToMapFrame(msg.lists[k].x, msg.lists[k].y, 0, mx, my, mz);
+      ResizeMapBase(mx, my);
+      if (mapBase[my*mapX+mx]) delete mapBase[my*mapX+mx];
+      mapBase[my*mapX+mx] = new OccupancyGridList;
+      mapBase[my*mapX+mx]->UnpackMsg(msg.lists[k]);
+    }
+    CheckDecayMap();
+    updated = true;
+  }
+
+  inline void SetOccupancyFromWorldFrame(double x, double y, double z, double p = PROB_OCCUPIED)
+  {
+    // Find occupancy value to be set
+    int value = 0;
+    if (p == PROB_OCCUPIED) value = logOddOccupied;
+    else if (p == PROB_FREE) value = logOddFree;
+    else return;
+    // Update occupancy value, return the column that have been changed
+    int mx, my, mz;
+    WorldFrameToMapFrame(x, y, z, mx, my, mz);
+    ResizeMapBase(mx, my);
+    if (!mapBase[my*mapX+mx]) 
+      mapBase[my*mapX+mx] = new OccupancyGridList;
+    mapBase[my*mapX+mx]->SetOccupancyValue(mz, value);
+    // Also record the column that have been changed in another list, for publish incremental map
+    if (mapBase[my*mapX+mx]->GetUpdateCounter() != updateCounter)
+    {
+      updateList.push_back(mapBase[my*mapX+mx]); 
+      mapBase[my*mapX+mx]->SetUpdateCounterXY(updateCounter, x, y);
+    }
+    updated = true;
+  }
+
+  inline int GetOccupancyFromWorldFrame(double x, double y, double z)
+  {
+    int mx, my, mz;
+    WorldFrameToMapFrame(x, y, z, mx, my, mz);
+    if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
+      return 0;
+    if (!mapBase[my*mapX+mx])
+      return 0;
+    else
+    {
+      if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) > logOddOccupiedThr)
+        return 1;
+      else if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) < logOddFreeThr)
+        return -1;
+      else 
+        return 0;
+    }
+  }
+
+  inline void DeleteFromWorldFrame(double x, double y, double z)
+  {
+    int mx, my, mz;
+    WorldFrameToMapFrame(x, y, z, mx, my, mz);
+    if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
+      return;
+    if (mapBase[my*mapX+mx])
+      mapBase[my*mapX+mx]->DeleteOccupancyGrid(mz);
+  }
+
+  vector<arma::colvec>& GetOccupancyWorldFrame(int type = OCCUPIED)
+  {
+    pts.clear();
+    for (int mx = 0; mx < mapX; mx++)
+    {
+      for (int my = 0; my < mapY; my++)
+      {
+        if (mapBase[my*mapX+mx])
+        {
+          vector<OccupancyGrid> grids;
+          mapBase[my*mapX+mx]->GetOccupancyGrids(grids);
+          for (unsigned int k = 0; k < grids.size(); k++)
+          {
+            if  ( (grids[k].mass / (grids[k].upper - grids[k].lower + 1) > logOddOccupiedThr && type == OCCUPIED) || 
+                  (grids[k].mass / (grids[k].upper - grids[k].lower + 1) < logOddFreeThr     && type == FREE) )
+            {
+              for (int mz = grids[k].lower; mz <= grids[k].upper; mz++)
+              {
+                double x, y, z;
+                MapFrameToWorldFrame(mx, my, mz, x, y, z);
+                arma::colvec pt(3);
+                pt(0) = x;
+                pt(1) = y;
+                pt(2) = z;
+                pts.push_back(pt);
+              }
+            }
+          }
+        }
+      }
+    }
+    return pts;
+  }
+
+  // Do not allow setting parameters if at least one update received
+  void SetResolution(double _resolution) 
+  {
+    if (!updated)
+      resolution = _resolution;
+  }
+  
+  void SetDecayInterval(double _decayInterval) 
+  {
+    if (!updated && _decayInterval > 0)
+      decayInterval = _decayInterval;
+  }  
+
+  inline double GetResolution() { return resolution; }
+  inline double GetMaxX() { return originX + mapX*resolution; }
+  inline double GetMinX() { return originX; }
+  inline double GetMaxY() { return originY + mapY*resolution; }
+  inline double GetMinY() { return originY; }
+  inline bool Updated() { return updated; }
+
+private:
+
+  inline void WorldFrameToMapFrame(double x, double y, double z, int& mx, int& my, int& mz)
+  {
+    mx = (x - originX) / resolution;
+    my = (y - originY) / resolution;
+    mz = (z - originZ) / resolution;
+  }
+
+  inline void MapFrameToWorldFrame(int mx, int my, int mz, double& x, double& y, double& z)
+  {
+    double r = 0.5*resolution;
+    x = mx * resolution + r + originX;
+    y = my * resolution + r + originY;
+    z = mz * resolution + r + originZ;
+  }
+
+  inline void ResizeMapBase(int& mx, int& my)
+  {
+    if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
+    {
+      double prevOriginX = originX;
+      double prevOriginY = originY;
+      int    prevMapX    = mapX;
+      int    prevMapY    = mapY;
+      while(mx < 0)
+      {
+        mapX    += expandStep;
+        mx      += expandStep;
+        originX -= expandStep*resolution;
+      }
+      while(my < 0)
+      {
+        mapY    += expandStep;
+        my      += expandStep;
+        originY -= expandStep*resolution;
+      }
+      while(mx >= mapX)
+      {
+        mapX    += expandStep;
+      } 
+      while(my >= mapY)
+      {
+        mapY    += expandStep;
+      } 
+      vector<OccupancyGridList*> _mapBase = mapBase;
+      mapBase.clear();
+      mapBase.resize(mapX*mapY,NULL);
+      for (int _x = 0; _x < prevMapX; _x++)
+      {
+        for(int _y = 0; _y < prevMapY; _y++)
+        {
+          int x = _x + round((prevOriginX - originX) / resolution);
+          int y = _y + round((prevOriginY - originY) / resolution);
+          mapBase[y*mapX+x] = _mapBase[_y*prevMapX+_x];
+        }
+      }
+    }
+  }
+
+  void CheckDecayMap()
+  {
+    if (decayInterval < 0)
+      return;
+    // Check whether to decay
+    static ros::Time prevDecayT = ros::Time::now();
+    ros::Time t = ros::Time::now();
+    double dt = (t - prevDecayT).toSec();
+    if (dt > decayInterval)
+    {
+      double r = pow(LOG_ODD_DECAY_RATE, dt);
+      for (int mx = 0; mx < mapX; mx++)
+        for (int my = 0; my < mapY; my++)
+          if (mapBase[my*mapX+mx])
+            mapBase[my*mapX+mx]->Decay(logOddOccupiedFixedThr, logOddFreeFixedThr, r);
+      prevDecayT = t;
+    }
+  }
+
+  double resolution;
+  double decayInterval;
+
+  int logOddOccupied;
+  int logOddFree;
+  int logOddOccupiedThr;
+  int logOddOccupiedFixedThr;
+  int logOddFreeThr;
+  int logOddFreeFixedThr;
+
+  bool updated;
+  int updateCounter;
+  vector<OccupancyGridList*> updateList;
+
+  double originX, originY, originZ;
+  int mapX, mapY;
+  int expandStep;
+  vector<OccupancyGridList*> mapBase;
+
+  vector<arma::colvec> pts;
+
+};
+#endif

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b multi_map_server is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg

@@ -0,0 +1,2 @@
+nav_msgs/OccupancyGrid[] maps
+geometry_msgs/Pose[] origins

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg

@@ -0,0 +1,2 @@
+SparseMap3D[] maps
+geometry_msgs/Pose[] origins

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg

@@ -0,0 +1,4 @@
+Header header
+nav_msgs/MapMetaData info
+VerticalOccupancyGridList[] lists
+

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg

@@ -0,0 +1,6 @@
+float32 x
+float32 y
+int32[] upper
+int32[] lower
+int32[] mass
+

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/package.xml

@@ -0,0 +1,38 @@
+<package>
+  <description>multi_map_server</description>
+  <name>multi_map_server</name>
+  <version>0.0.0</version>
+  <maintainer email="eeshaojie@todo.todo">Shaojie Shen</maintainer>
+  <license>BSD</license>
+  
+  <url>http://ros.org/wiki/multi_map_server</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_depend>laser_geometry</build_depend>
+  <build_depend>pose_utils</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>quadrotor_msgs</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>std_srvs</run_depend>
+  <run_depend>laser_geometry</run_depend>
+  <run_depend>pose_utils</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>quadrotor_msgs</run_depend>
+
+  <export>
+    <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
+  </export>
+</package>
+

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py

@@ -0,0 +1 @@
+#autogenerated by ROS python message generators

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py

@@ -0,0 +1,404 @@
+"""autogenerated by genpy from multi_map_server/MultiOccupancyGrid.msg. Do not edit."""
+import sys
+python3 = True if sys.hexversion > 0x03000000 else False
+import genpy
+import struct
+
+import geometry_msgs.msg
+import nav_msgs.msg
+import genpy
+import std_msgs.msg
+
+class MultiOccupancyGrid(genpy.Message):
+  _md5sum = "61e63a291f11a6b1796a1edf79f34f72"
+  _type = "multi_map_server/MultiOccupancyGrid"
+  _has_header = False #flag to mark the presence of a Header object
+  _full_text = """nav_msgs/OccupancyGrid[] maps
+geometry_msgs/Pose[] origins
+
+================================================================================
+MSG: nav_msgs/OccupancyGrid
+# This represents a 2-D grid map, in which each cell represents the probability of
+# occupancy.
+
+Header header 
+
+#MetaData for the map
+MapMetaData info
+
+# The map data, in row-major order, starting with (0,0).  Occupancy
+# probabilities are in the range [0,100].  Unknown is -1.
+int8[] data
+
+================================================================================
+MSG: std_msgs/Header
+# Standard metadata for higher-level stamped data types.
+# This is generally used to communicate timestamped data 
+# in a particular coordinate frame.
+# 
+# sequence ID: consecutively increasing ID 
+uint32 seq
+#Two-integer timestamp that is expressed as:
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
+# time-handling sugar is provided by the client library
+time stamp
+#Frame this data is associated with
+# 0: no frame
+# 1: global frame
+string frame_id
+
+================================================================================
+MSG: nav_msgs/MapMetaData
+# This hold basic information about the characterists of the OccupancyGrid
+
+# The time at which the map was loaded
+time map_load_time
+# The map resolution [m/cell]
+float32 resolution
+# Map width [cells]
+uint32 width
+# Map height [cells]
+uint32 height
+# The origin of the map [m, m, rad].  This is the real-world pose of the
+# cell (0,0) in the map.
+geometry_msgs/Pose origin
+================================================================================
+MSG: geometry_msgs/Pose
+# A representation of pose in free space, composed of postion and orientation. 
+Point position
+Quaternion orientation
+
+================================================================================
+MSG: geometry_msgs/Point
+# This contains the position of a point in free space
+float64 x
+float64 y
+float64 z
+
+================================================================================
+MSG: geometry_msgs/Quaternion
+# This represents an orientation in free space in quaternion form.
+
+float64 x
+float64 y
+float64 z
+float64 w
+
+"""
+  __slots__ = ['maps','origins']
+  _slot_types = ['nav_msgs/OccupancyGrid[]','geometry_msgs/Pose[]']
+
+  def __init__(self, *args, **kwds):
+    """
+    Constructor. Any message fields that are implicitly/explicitly
+    set to None will be assigned a default value. The recommend
+    use is keyword arguments as this is more robust to future message
+    changes.  You cannot mix in-order arguments and keyword arguments.
+
+    The available fields are:
+       maps,origins
+
+    :param args: complete set of field values, in .msg order
+    :param kwds: use keyword arguments corresponding to message field names
+    to set specific fields.
+    """
+    if args or kwds:
+      super(MultiOccupancyGrid, self).__init__(*args, **kwds)
+      #message fields cannot be None, assign default values for those that are
+      if self.maps is None:
+        self.maps = []
+      if self.origins is None:
+        self.origins = []
+    else:
+      self.maps = []
+      self.origins = []
+
+  def _get_types(self):
+    """
+    internal API method
+    """
+    return self._slot_types
+
+  def serialize(self, buff):
+    """
+    serialize message into buffer
+    :param buff: buffer, ``StringIO``
+    """
+    try:
+      length = len(self.maps)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.maps:
+        _v1 = val1.header
+        buff.write(_struct_I.pack(_v1.seq))
+        _v2 = _v1.stamp
+        _x = _v2
+        buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
+        _x = _v1.frame_id
+        length = len(_x)
+        if python3 or type(_x) == unicode:
+          _x = _x.encode('utf-8')
+          length = len(_x)
+        if python3:
+          buff.write(struct.pack('<I%sB'%length, length, *_x))
+        else:
+          buff.write(struct.pack('<I%ss'%length, length, _x))
+        _v3 = val1.info
+        _v4 = _v3.map_load_time
+        _x = _v4
+        buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
+        _x = _v3
+        buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
+        _v5 = _v3.origin
+        _v6 = _v5.position
+        _x = _v6
+        buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
+        _v7 = _v5.orientation
+        _x = _v7
+        buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
+        length = len(val1.data)
+        buff.write(_struct_I.pack(length))
+        pattern = '<%sb'%length
+        buff.write(struct.pack(pattern, *val1.data))
+      length = len(self.origins)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.origins:
+        _v8 = val1.position
+        _x = _v8
+        buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
+        _v9 = val1.orientation
+        _x = _v9
+        buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
+
+  def deserialize(self, str):
+    """
+    unpack serialized message in str into this message instance
+    :param str: byte array of serialized message, ``str``
+    """
+    try:
+      if self.maps is None:
+        self.maps = None
+      if self.origins is None:
+        self.origins = None
+      end = 0
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.maps = []
+      for i in range(0, length):
+        val1 = nav_msgs.msg.OccupancyGrid()
+        _v10 = val1.header
+        start = end
+        end += 4
+        (_v10.seq,) = _struct_I.unpack(str[start:end])
+        _v11 = _v10.stamp
+        _x = _v11
+        start = end
+        end += 8
+        (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        start = end
+        end += length
+        if python3:
+          _v10.frame_id = str[start:end].decode('utf-8')
+        else:
+          _v10.frame_id = str[start:end]
+        _v12 = val1.info
+        _v13 = _v12.map_load_time
+        _x = _v13
+        start = end
+        end += 8
+        (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
+        _x = _v12
+        start = end
+        end += 12
+        (_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
+        _v14 = _v12.origin
+        _v15 = _v14.position
+        _x = _v15
+        start = end
+        end += 24
+        (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
+        _v16 = _v14.orientation
+        _x = _v16
+        start = end
+        end += 32
+        (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        pattern = '<%sb'%length
+        start = end
+        end += struct.calcsize(pattern)
+        val1.data = struct.unpack(pattern, str[start:end])
+        self.maps.append(val1)
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.origins = []
+      for i in range(0, length):
+        val1 = geometry_msgs.msg.Pose()
+        _v17 = val1.position
+        _x = _v17
+        start = end
+        end += 24
+        (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
+        _v18 = val1.orientation
+        _x = _v18
+        start = end
+        end += 32
+        (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
+        self.origins.append(val1)
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e) #most likely buffer underfill
+
+
+  def serialize_numpy(self, buff, numpy):
+    """
+    serialize message with numpy array types into buffer
+    :param buff: buffer, ``StringIO``
+    :param numpy: numpy python module
+    """
+    try:
+      length = len(self.maps)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.maps:
+        _v19 = val1.header
+        buff.write(_struct_I.pack(_v19.seq))
+        _v20 = _v19.stamp
+        _x = _v20
+        buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
+        _x = _v19.frame_id
+        length = len(_x)
+        if python3 or type(_x) == unicode:
+          _x = _x.encode('utf-8')
+          length = len(_x)
+        if python3:
+          buff.write(struct.pack('<I%sB'%length, length, *_x))
+        else:
+          buff.write(struct.pack('<I%ss'%length, length, _x))
+        _v21 = val1.info
+        _v22 = _v21.map_load_time
+        _x = _v22
+        buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
+        _x = _v21
+        buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
+        _v23 = _v21.origin
+        _v24 = _v23.position
+        _x = _v24
+        buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
+        _v25 = _v23.orientation
+        _x = _v25
+        buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
+        length = len(val1.data)
+        buff.write(_struct_I.pack(length))
+        pattern = '<%sb'%length
+        buff.write(val1.data.tostring())
+      length = len(self.origins)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.origins:
+        _v26 = val1.position
+        _x = _v26
+        buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
+        _v27 = val1.orientation
+        _x = _v27
+        buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
+
+  def deserialize_numpy(self, str, numpy):
+    """
+    unpack serialized message in str into this message instance using numpy for array types
+    :param str: byte array of serialized message, ``str``
+    :param numpy: numpy python module
+    """
+    try:
+      if self.maps is None:
+        self.maps = None
+      if self.origins is None:
+        self.origins = None
+      end = 0
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.maps = []
+      for i in range(0, length):
+        val1 = nav_msgs.msg.OccupancyGrid()
+        _v28 = val1.header
+        start = end
+        end += 4
+        (_v28.seq,) = _struct_I.unpack(str[start:end])
+        _v29 = _v28.stamp
+        _x = _v29
+        start = end
+        end += 8
+        (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        start = end
+        end += length
+        if python3:
+          _v28.frame_id = str[start:end].decode('utf-8')
+        else:
+          _v28.frame_id = str[start:end]
+        _v30 = val1.info
+        _v31 = _v30.map_load_time
+        _x = _v31
+        start = end
+        end += 8
+        (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
+        _x = _v30
+        start = end
+        end += 12
+        (_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
+        _v32 = _v30.origin
+        _v33 = _v32.position
+        _x = _v33
+        start = end
+        end += 24
+        (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
+        _v34 = _v32.orientation
+        _x = _v34
+        start = end
+        end += 32
+        (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        pattern = '<%sb'%length
+        start = end
+        end += struct.calcsize(pattern)
+        val1.data = numpy.frombuffer(str[start:end], dtype=numpy.int8, count=length)
+        self.maps.append(val1)
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.origins = []
+      for i in range(0, length):
+        val1 = geometry_msgs.msg.Pose()
+        _v35 = val1.position
+        _x = _v35
+        start = end
+        end += 24
+        (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
+        _v36 = val1.orientation
+        _x = _v36
+        start = end
+        end += 32
+        (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
+        self.origins.append(val1)
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e) #most likely buffer underfill
+
+_struct_I = genpy.struct_I
+_struct_4d = struct.Struct("<4d")
+_struct_f2I = struct.Struct("<f2I")
+_struct_2I = struct.Struct("<2I")
+_struct_3d = struct.Struct("<3d")

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiSparseMap3D.py

@@ -0,0 +1,484 @@
+"""autogenerated by genpy from multi_map_server/MultiSparseMap3D.msg. Do not edit."""
+import sys
+python3 = True if sys.hexversion > 0x03000000 else False
+import genpy
+import struct
+
+import geometry_msgs.msg
+import multi_map_server.msg
+import nav_msgs.msg
+import genpy
+import std_msgs.msg
+
+class MultiSparseMap3D(genpy.Message):
+  _md5sum = "2e3d76c98ee3e2b23a422f64965f6418"
+  _type = "multi_map_server/MultiSparseMap3D"
+  _has_header = False #flag to mark the presence of a Header object
+  _full_text = """SparseMap3D[] maps
+geometry_msgs/Pose[] origins
+
+================================================================================
+MSG: multi_map_server/SparseMap3D
+Header header
+nav_msgs/MapMetaData info
+VerticalOccupancyGridList[] lists
+
+
+================================================================================
+MSG: std_msgs/Header
+# Standard metadata for higher-level stamped data types.
+# This is generally used to communicate timestamped data 
+# in a particular coordinate frame.
+# 
+# sequence ID: consecutively increasing ID 
+uint32 seq
+#Two-integer timestamp that is expressed as:
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
+# time-handling sugar is provided by the client library
+time stamp
+#Frame this data is associated with
+# 0: no frame
+# 1: global frame
+string frame_id
+
+================================================================================
+MSG: nav_msgs/MapMetaData
+# This hold basic information about the characterists of the OccupancyGrid
+
+# The time at which the map was loaded
+time map_load_time
+# The map resolution [m/cell]
+float32 resolution
+# Map width [cells]
+uint32 width
+# Map height [cells]
+uint32 height
+# The origin of the map [m, m, rad].  This is the real-world pose of the
+# cell (0,0) in the map.
+geometry_msgs/Pose origin
+================================================================================
+MSG: geometry_msgs/Pose
+# A representation of pose in free space, composed of postion and orientation. 
+Point position
+Quaternion orientation
+
+================================================================================
+MSG: geometry_msgs/Point
+# This contains the position of a point in free space
+float64 x
+float64 y
+float64 z
+
+================================================================================
+MSG: geometry_msgs/Quaternion
+# This represents an orientation in free space in quaternion form.
+
+float64 x
+float64 y
+float64 z
+float64 w
+
+================================================================================
+MSG: multi_map_server/VerticalOccupancyGridList
+float32 x
+float32 y
+int32[] upper
+int32[] lower
+int32[] mass
+
+
+"""
+  __slots__ = ['maps','origins']
+  _slot_types = ['multi_map_server/SparseMap3D[]','geometry_msgs/Pose[]']
+
+  def __init__(self, *args, **kwds):
+    """
+    Constructor. Any message fields that are implicitly/explicitly
+    set to None will be assigned a default value. The recommend
+    use is keyword arguments as this is more robust to future message
+    changes.  You cannot mix in-order arguments and keyword arguments.
+
+    The available fields are:
+       maps,origins
+
+    :param args: complete set of field values, in .msg order
+    :param kwds: use keyword arguments corresponding to message field names
+    to set specific fields.
+    """
+    if args or kwds:
+      super(MultiSparseMap3D, self).__init__(*args, **kwds)
+      #message fields cannot be None, assign default values for those that are
+      if self.maps is None:
+        self.maps = []
+      if self.origins is None:
+        self.origins = []
+    else:
+      self.maps = []
+      self.origins = []
+
+  def _get_types(self):
+    """
+    internal API method
+    """
+    return self._slot_types
+
+  def serialize(self, buff):
+    """
+    serialize message into buffer
+    :param buff: buffer, ``StringIO``
+    """
+    try:
+      length = len(self.maps)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.maps:
+        _v1 = val1.header
+        buff.write(_struct_I.pack(_v1.seq))
+        _v2 = _v1.stamp
+        _x = _v2
+        buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
+        _x = _v1.frame_id
+        length = len(_x)
+        if python3 or type(_x) == unicode:
+          _x = _x.encode('utf-8')
+          length = len(_x)
+        if python3:
+          buff.write(struct.pack('<I%sB'%length, length, *_x))
+        else:
+          buff.write(struct.pack('<I%ss'%length, length, _x))
+        _v3 = val1.info
+        _v4 = _v3.map_load_time
+        _x = _v4
+        buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
+        _x = _v3
+        buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
+        _v5 = _v3.origin
+        _v6 = _v5.position
+        _x = _v6
+        buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
+        _v7 = _v5.orientation
+        _x = _v7
+        buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
+        length = len(val1.lists)
+        buff.write(_struct_I.pack(length))
+        for val2 in val1.lists:
+          _x = val2
+          buff.write(_struct_2f.pack(_x.x, _x.y))
+          length = len(val2.upper)
+          buff.write(_struct_I.pack(length))
+          pattern = '<%si'%length
+          buff.write(struct.pack(pattern, *val2.upper))
+          length = len(val2.lower)
+          buff.write(_struct_I.pack(length))
+          pattern = '<%si'%length
+          buff.write(struct.pack(pattern, *val2.lower))
+          length = len(val2.mass)
+          buff.write(_struct_I.pack(length))
+          pattern = '<%si'%length
+          buff.write(struct.pack(pattern, *val2.mass))
+      length = len(self.origins)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.origins:
+        _v8 = val1.position
+        _x = _v8
+        buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
+        _v9 = val1.orientation
+        _x = _v9
+        buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
+
+  def deserialize(self, str):
+    """
+    unpack serialized message in str into this message instance
+    :param str: byte array of serialized message, ``str``
+    """
+    try:
+      if self.maps is None:
+        self.maps = None
+      if self.origins is None:
+        self.origins = None
+      end = 0
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.maps = []
+      for i in range(0, length):
+        val1 = multi_map_server.msg.SparseMap3D()
+        _v10 = val1.header
+        start = end
+        end += 4
+        (_v10.seq,) = _struct_I.unpack(str[start:end])
+        _v11 = _v10.stamp
+        _x = _v11
+        start = end
+        end += 8
+        (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        start = end
+        end += length
+        if python3:
+          _v10.frame_id = str[start:end].decode('utf-8')
+        else:
+          _v10.frame_id = str[start:end]
+        _v12 = val1.info
+        _v13 = _v12.map_load_time
+        _x = _v13
+        start = end
+        end += 8
+        (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
+        _x = _v12
+        start = end
+        end += 12
+        (_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
+        _v14 = _v12.origin
+        _v15 = _v14.position
+        _x = _v15
+        start = end
+        end += 24
+        (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
+        _v16 = _v14.orientation
+        _x = _v16
+        start = end
+        end += 32
+        (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        val1.lists = []
+        for i in range(0, length):
+          val2 = multi_map_server.msg.VerticalOccupancyGridList()
+          _x = val2
+          start = end
+          end += 8
+          (_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
+          start = end
+          end += 4
+          (length,) = _struct_I.unpack(str[start:end])
+          pattern = '<%si'%length
+          start = end
+          end += struct.calcsize(pattern)
+          val2.upper = struct.unpack(pattern, str[start:end])
+          start = end
+          end += 4
+          (length,) = _struct_I.unpack(str[start:end])
+          pattern = '<%si'%length
+          start = end
+          end += struct.calcsize(pattern)
+          val2.lower = struct.unpack(pattern, str[start:end])
+          start = end
+          end += 4
+          (length,) = _struct_I.unpack(str[start:end])
+          pattern = '<%si'%length
+          start = end
+          end += struct.calcsize(pattern)
+          val2.mass = struct.unpack(pattern, str[start:end])
+          val1.lists.append(val2)
+        self.maps.append(val1)
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.origins = []
+      for i in range(0, length):
+        val1 = geometry_msgs.msg.Pose()
+        _v17 = val1.position
+        _x = _v17
+        start = end
+        end += 24
+        (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
+        _v18 = val1.orientation
+        _x = _v18
+        start = end
+        end += 32
+        (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
+        self.origins.append(val1)
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e) #most likely buffer underfill
+
+
+  def serialize_numpy(self, buff, numpy):
+    """
+    serialize message with numpy array types into buffer
+    :param buff: buffer, ``StringIO``
+    :param numpy: numpy python module
+    """
+    try:
+      length = len(self.maps)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.maps:
+        _v19 = val1.header
+        buff.write(_struct_I.pack(_v19.seq))
+        _v20 = _v19.stamp
+        _x = _v20
+        buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
+        _x = _v19.frame_id
+        length = len(_x)
+        if python3 or type(_x) == unicode:
+          _x = _x.encode('utf-8')
+          length = len(_x)
+        if python3:
+          buff.write(struct.pack('<I%sB'%length, length, *_x))
+        else:
+          buff.write(struct.pack('<I%ss'%length, length, _x))
+        _v21 = val1.info
+        _v22 = _v21.map_load_time
+        _x = _v22
+        buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
+        _x = _v21
+        buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
+        _v23 = _v21.origin
+        _v24 = _v23.position
+        _x = _v24
+        buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
+        _v25 = _v23.orientation
+        _x = _v25
+        buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
+        length = len(val1.lists)
+        buff.write(_struct_I.pack(length))
+        for val2 in val1.lists:
+          _x = val2
+          buff.write(_struct_2f.pack(_x.x, _x.y))
+          length = len(val2.upper)
+          buff.write(_struct_I.pack(length))
+          pattern = '<%si'%length
+          buff.write(val2.upper.tostring())
+          length = len(val2.lower)
+          buff.write(_struct_I.pack(length))
+          pattern = '<%si'%length
+          buff.write(val2.lower.tostring())
+          length = len(val2.mass)
+          buff.write(_struct_I.pack(length))
+          pattern = '<%si'%length
+          buff.write(val2.mass.tostring())
+      length = len(self.origins)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.origins:
+        _v26 = val1.position
+        _x = _v26
+        buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
+        _v27 = val1.orientation
+        _x = _v27
+        buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
+
+  def deserialize_numpy(self, str, numpy):
+    """
+    unpack serialized message in str into this message instance using numpy for array types
+    :param str: byte array of serialized message, ``str``
+    :param numpy: numpy python module
+    """
+    try:
+      if self.maps is None:
+        self.maps = None
+      if self.origins is None:
+        self.origins = None
+      end = 0
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.maps = []
+      for i in range(0, length):
+        val1 = multi_map_server.msg.SparseMap3D()
+        _v28 = val1.header
+        start = end
+        end += 4
+        (_v28.seq,) = _struct_I.unpack(str[start:end])
+        _v29 = _v28.stamp
+        _x = _v29
+        start = end
+        end += 8
+        (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        start = end
+        end += length
+        if python3:
+          _v28.frame_id = str[start:end].decode('utf-8')
+        else:
+          _v28.frame_id = str[start:end]
+        _v30 = val1.info
+        _v31 = _v30.map_load_time
+        _x = _v31
+        start = end
+        end += 8
+        (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
+        _x = _v30
+        start = end
+        end += 12
+        (_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
+        _v32 = _v30.origin
+        _v33 = _v32.position
+        _x = _v33
+        start = end
+        end += 24
+        (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
+        _v34 = _v32.orientation
+        _x = _v34
+        start = end
+        end += 32
+        (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        val1.lists = []
+        for i in range(0, length):
+          val2 = multi_map_server.msg.VerticalOccupancyGridList()
+          _x = val2
+          start = end
+          end += 8
+          (_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
+          start = end
+          end += 4
+          (length,) = _struct_I.unpack(str[start:end])
+          pattern = '<%si'%length
+          start = end
+          end += struct.calcsize(pattern)
+          val2.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+          start = end
+          end += 4
+          (length,) = _struct_I.unpack(str[start:end])
+          pattern = '<%si'%length
+          start = end
+          end += struct.calcsize(pattern)
+          val2.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+          start = end
+          end += 4
+          (length,) = _struct_I.unpack(str[start:end])
+          pattern = '<%si'%length
+          start = end
+          end += struct.calcsize(pattern)
+          val2.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+          val1.lists.append(val2)
+        self.maps.append(val1)
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.origins = []
+      for i in range(0, length):
+        val1 = geometry_msgs.msg.Pose()
+        _v35 = val1.position
+        _x = _v35
+        start = end
+        end += 24
+        (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
+        _v36 = val1.orientation
+        _x = _v36
+        start = end
+        end += 32
+        (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
+        self.origins.append(val1)
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e) #most likely buffer underfill
+
+_struct_I = genpy.struct_I
+_struct_f2I = struct.Struct("<f2I")
+_struct_2f = struct.Struct("<2f")
+_struct_4d = struct.Struct("<4d")
+_struct_2I = struct.Struct("<2I")
+_struct_3d = struct.Struct("<3d")

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_SparseMap3D.py

@@ -0,0 +1,340 @@
+"""autogenerated by genpy from multi_map_server/SparseMap3D.msg. Do not edit."""
+import sys
+python3 = True if sys.hexversion > 0x03000000 else False
+import genpy
+import struct
+
+import geometry_msgs.msg
+import multi_map_server.msg
+import nav_msgs.msg
+import genpy
+import std_msgs.msg
+
+class SparseMap3D(genpy.Message):
+  _md5sum = "a20102f0b3a02e95070dab4140b78fb5"
+  _type = "multi_map_server/SparseMap3D"
+  _has_header = True #flag to mark the presence of a Header object
+  _full_text = """Header header
+nav_msgs/MapMetaData info
+VerticalOccupancyGridList[] lists
+
+
+================================================================================
+MSG: std_msgs/Header
+# Standard metadata for higher-level stamped data types.
+# This is generally used to communicate timestamped data 
+# in a particular coordinate frame.
+# 
+# sequence ID: consecutively increasing ID 
+uint32 seq
+#Two-integer timestamp that is expressed as:
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
+# time-handling sugar is provided by the client library
+time stamp
+#Frame this data is associated with
+# 0: no frame
+# 1: global frame
+string frame_id
+
+================================================================================
+MSG: nav_msgs/MapMetaData
+# This hold basic information about the characterists of the OccupancyGrid
+
+# The time at which the map was loaded
+time map_load_time
+# The map resolution [m/cell]
+float32 resolution
+# Map width [cells]
+uint32 width
+# Map height [cells]
+uint32 height
+# The origin of the map [m, m, rad].  This is the real-world pose of the
+# cell (0,0) in the map.
+geometry_msgs/Pose origin
+================================================================================
+MSG: geometry_msgs/Pose
+# A representation of pose in free space, composed of postion and orientation. 
+Point position
+Quaternion orientation
+
+================================================================================
+MSG: geometry_msgs/Point
+# This contains the position of a point in free space
+float64 x
+float64 y
+float64 z
+
+================================================================================
+MSG: geometry_msgs/Quaternion
+# This represents an orientation in free space in quaternion form.
+
+float64 x
+float64 y
+float64 z
+float64 w
+
+================================================================================
+MSG: multi_map_server/VerticalOccupancyGridList
+float32 x
+float32 y
+int32[] upper
+int32[] lower
+int32[] mass
+
+
+"""
+  __slots__ = ['header','info','lists']
+  _slot_types = ['std_msgs/Header','nav_msgs/MapMetaData','multi_map_server/VerticalOccupancyGridList[]']
+
+  def __init__(self, *args, **kwds):
+    """
+    Constructor. Any message fields that are implicitly/explicitly
+    set to None will be assigned a default value. The recommend
+    use is keyword arguments as this is more robust to future message
+    changes.  You cannot mix in-order arguments and keyword arguments.
+
+    The available fields are:
+       header,info,lists
+
+    :param args: complete set of field values, in .msg order
+    :param kwds: use keyword arguments corresponding to message field names
+    to set specific fields.
+    """
+    if args or kwds:
+      super(SparseMap3D, self).__init__(*args, **kwds)
+      #message fields cannot be None, assign default values for those that are
+      if self.header is None:
+        self.header = std_msgs.msg.Header()
+      if self.info is None:
+        self.info = nav_msgs.msg.MapMetaData()
+      if self.lists is None:
+        self.lists = []
+    else:
+      self.header = std_msgs.msg.Header()
+      self.info = nav_msgs.msg.MapMetaData()
+      self.lists = []
+
+  def _get_types(self):
+    """
+    internal API method
+    """
+    return self._slot_types
+
+  def serialize(self, buff):
+    """
+    serialize message into buffer
+    :param buff: buffer, ``StringIO``
+    """
+    try:
+      _x = self
+      buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
+      _x = self.header.frame_id
+      length = len(_x)
+      if python3 or type(_x) == unicode:
+        _x = _x.encode('utf-8')
+        length = len(_x)
+      if python3:
+        buff.write(struct.pack('<I%sB'%length, length, *_x))
+      else:
+        buff.write(struct.pack('<I%ss'%length, length, _x))
+      _x = self
+      buff.write(_struct_2If2I7d.pack(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w))
+      length = len(self.lists)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.lists:
+        _x = val1
+        buff.write(_struct_2f.pack(_x.x, _x.y))
+        length = len(val1.upper)
+        buff.write(_struct_I.pack(length))
+        pattern = '<%si'%length
+        buff.write(struct.pack(pattern, *val1.upper))
+        length = len(val1.lower)
+        buff.write(_struct_I.pack(length))
+        pattern = '<%si'%length
+        buff.write(struct.pack(pattern, *val1.lower))
+        length = len(val1.mass)
+        buff.write(_struct_I.pack(length))
+        pattern = '<%si'%length
+        buff.write(struct.pack(pattern, *val1.mass))
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
+
+  def deserialize(self, str):
+    """
+    unpack serialized message in str into this message instance
+    :param str: byte array of serialized message, ``str``
+    """
+    try:
+      if self.header is None:
+        self.header = std_msgs.msg.Header()
+      if self.info is None:
+        self.info = nav_msgs.msg.MapMetaData()
+      if self.lists is None:
+        self.lists = None
+      end = 0
+      _x = self
+      start = end
+      end += 12
+      (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      start = end
+      end += length
+      if python3:
+        self.header.frame_id = str[start:end].decode('utf-8')
+      else:
+        self.header.frame_id = str[start:end]
+      _x = self
+      start = end
+      end += 76
+      (_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w,) = _struct_2If2I7d.unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.lists = []
+      for i in range(0, length):
+        val1 = multi_map_server.msg.VerticalOccupancyGridList()
+        _x = val1
+        start = end
+        end += 8
+        (_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        pattern = '<%si'%length
+        start = end
+        end += struct.calcsize(pattern)
+        val1.upper = struct.unpack(pattern, str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        pattern = '<%si'%length
+        start = end
+        end += struct.calcsize(pattern)
+        val1.lower = struct.unpack(pattern, str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        pattern = '<%si'%length
+        start = end
+        end += struct.calcsize(pattern)
+        val1.mass = struct.unpack(pattern, str[start:end])
+        self.lists.append(val1)
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e) #most likely buffer underfill
+
+
+  def serialize_numpy(self, buff, numpy):
+    """
+    serialize message with numpy array types into buffer
+    :param buff: buffer, ``StringIO``
+    :param numpy: numpy python module
+    """
+    try:
+      _x = self
+      buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
+      _x = self.header.frame_id
+      length = len(_x)
+      if python3 or type(_x) == unicode:
+        _x = _x.encode('utf-8')
+        length = len(_x)
+      if python3:
+        buff.write(struct.pack('<I%sB'%length, length, *_x))
+      else:
+        buff.write(struct.pack('<I%ss'%length, length, _x))
+      _x = self
+      buff.write(_struct_2If2I7d.pack(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w))
+      length = len(self.lists)
+      buff.write(_struct_I.pack(length))
+      for val1 in self.lists:
+        _x = val1
+        buff.write(_struct_2f.pack(_x.x, _x.y))
+        length = len(val1.upper)
+        buff.write(_struct_I.pack(length))
+        pattern = '<%si'%length
+        buff.write(val1.upper.tostring())
+        length = len(val1.lower)
+        buff.write(_struct_I.pack(length))
+        pattern = '<%si'%length
+        buff.write(val1.lower.tostring())
+        length = len(val1.mass)
+        buff.write(_struct_I.pack(length))
+        pattern = '<%si'%length
+        buff.write(val1.mass.tostring())
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
+
+  def deserialize_numpy(self, str, numpy):
+    """
+    unpack serialized message in str into this message instance using numpy for array types
+    :param str: byte array of serialized message, ``str``
+    :param numpy: numpy python module
+    """
+    try:
+      if self.header is None:
+        self.header = std_msgs.msg.Header()
+      if self.info is None:
+        self.info = nav_msgs.msg.MapMetaData()
+      if self.lists is None:
+        self.lists = None
+      end = 0
+      _x = self
+      start = end
+      end += 12
+      (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      start = end
+      end += length
+      if python3:
+        self.header.frame_id = str[start:end].decode('utf-8')
+      else:
+        self.header.frame_id = str[start:end]
+      _x = self
+      start = end
+      end += 76
+      (_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w,) = _struct_2If2I7d.unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      self.lists = []
+      for i in range(0, length):
+        val1 = multi_map_server.msg.VerticalOccupancyGridList()
+        _x = val1
+        start = end
+        end += 8
+        (_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        pattern = '<%si'%length
+        start = end
+        end += struct.calcsize(pattern)
+        val1.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        pattern = '<%si'%length
+        start = end
+        end += struct.calcsize(pattern)
+        val1.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+        start = end
+        end += 4
+        (length,) = _struct_I.unpack(str[start:end])
+        pattern = '<%si'%length
+        start = end
+        end += struct.calcsize(pattern)
+        val1.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+        self.lists.append(val1)
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e) #most likely buffer underfill
+
+_struct_I = genpy.struct_I
+_struct_3I = struct.Struct("<3I")
+_struct_2f = struct.Struct("<2f")
+_struct_2If2I7d = struct.Struct("<2If2I7d")

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_VerticalOccupancyGridList.py

@@ -0,0 +1,185 @@
+"""autogenerated by genpy from multi_map_server/VerticalOccupancyGridList.msg. Do not edit."""
+import sys
+python3 = True if sys.hexversion > 0x03000000 else False
+import genpy
+import struct
+
+
+class VerticalOccupancyGridList(genpy.Message):
+  _md5sum = "7ef85cc95b82747f51eb01a16bd7c795"
+  _type = "multi_map_server/VerticalOccupancyGridList"
+  _has_header = False #flag to mark the presence of a Header object
+  _full_text = """float32 x
+float32 y
+int32[] upper
+int32[] lower
+int32[] mass
+
+
+"""
+  __slots__ = ['x','y','upper','lower','mass']
+  _slot_types = ['float32','float32','int32[]','int32[]','int32[]']
+
+  def __init__(self, *args, **kwds):
+    """
+    Constructor. Any message fields that are implicitly/explicitly
+    set to None will be assigned a default value. The recommend
+    use is keyword arguments as this is more robust to future message
+    changes.  You cannot mix in-order arguments and keyword arguments.
+
+    The available fields are:
+       x,y,upper,lower,mass
+
+    :param args: complete set of field values, in .msg order
+    :param kwds: use keyword arguments corresponding to message field names
+    to set specific fields.
+    """
+    if args or kwds:
+      super(VerticalOccupancyGridList, self).__init__(*args, **kwds)
+      #message fields cannot be None, assign default values for those that are
+      if self.x is None:
+        self.x = 0.
+      if self.y is None:
+        self.y = 0.
+      if self.upper is None:
+        self.upper = []
+      if self.lower is None:
+        self.lower = []
+      if self.mass is None:
+        self.mass = []
+    else:
+      self.x = 0.
+      self.y = 0.
+      self.upper = []
+      self.lower = []
+      self.mass = []
+
+  def _get_types(self):
+    """
+    internal API method
+    """
+    return self._slot_types
+
+  def serialize(self, buff):
+    """
+    serialize message into buffer
+    :param buff: buffer, ``StringIO``
+    """
+    try:
+      _x = self
+      buff.write(_struct_2f.pack(_x.x, _x.y))
+      length = len(self.upper)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%si'%length
+      buff.write(struct.pack(pattern, *self.upper))
+      length = len(self.lower)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%si'%length
+      buff.write(struct.pack(pattern, *self.lower))
+      length = len(self.mass)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%si'%length
+      buff.write(struct.pack(pattern, *self.mass))
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
+
+  def deserialize(self, str):
+    """
+    unpack serialized message in str into this message instance
+    :param str: byte array of serialized message, ``str``
+    """
+    try:
+      end = 0
+      _x = self
+      start = end
+      end += 8
+      (_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%si'%length
+      start = end
+      end += struct.calcsize(pattern)
+      self.upper = struct.unpack(pattern, str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%si'%length
+      start = end
+      end += struct.calcsize(pattern)
+      self.lower = struct.unpack(pattern, str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%si'%length
+      start = end
+      end += struct.calcsize(pattern)
+      self.mass = struct.unpack(pattern, str[start:end])
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e) #most likely buffer underfill
+
+
+  def serialize_numpy(self, buff, numpy):
+    """
+    serialize message with numpy array types into buffer
+    :param buff: buffer, ``StringIO``
+    :param numpy: numpy python module
+    """
+    try:
+      _x = self
+      buff.write(_struct_2f.pack(_x.x, _x.y))
+      length = len(self.upper)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%si'%length
+      buff.write(self.upper.tostring())
+      length = len(self.lower)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%si'%length
+      buff.write(self.lower.tostring())
+      length = len(self.mass)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%si'%length
+      buff.write(self.mass.tostring())
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
+
+  def deserialize_numpy(self, str, numpy):
+    """
+    unpack serialized message in str into this message instance using numpy for array types
+    :param str: byte array of serialized message, ``str``
+    :param numpy: numpy python module
+    """
+    try:
+      end = 0
+      _x = self
+      start = end
+      end += 8
+      (_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%si'%length
+      start = end
+      end += struct.calcsize(pattern)
+      self.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%si'%length
+      start = end
+      end += struct.calcsize(pattern)
+      self.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%si'%length
+      start = end
+      end += struct.calcsize(pattern)
+      self.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e) #most likely buffer underfill
+
+_struct_I = genpy.struct_I
+_struct_2f = struct.Struct("<2f")

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py

@@ -0,0 +1,4 @@
+from ._SparseMap3D import *
+from ._MultiOccupancyGrid import *
+from ._MultiSparseMap3D import *
+from ._VerticalOccupancyGridList import *

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc

@@ -0,0 +1,90 @@
+#include <iostream>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud.h>
+#include <pose_utils.h>
+#include <multi_map_server/MultiOccupancyGrid.h>
+#include <multi_map_server/MultiSparseMap3D.h>
+#include <multi_map_server/Map2D.h>
+#include <multi_map_server/Map3D.h>
+
+ros::Publisher pub1;
+ros::Publisher pub2;
+
+vector<Map2D> maps2d;
+vector<geometry_msgs::Pose> origins2d;
+vector<Map3D> maps3d;
+vector<geometry_msgs::Pose> origins3d;
+
+void maps2d_callback(const multi_map_server::MultiOccupancyGrid::ConstPtr &msg)
+{
+  // Merge map
+  maps2d.resize(msg->maps.size(), Map2D(4));
+  for (unsigned int k = 0; k < msg->maps.size(); k++)
+    maps2d[k].Replace(msg->maps[k]);
+  origins2d = msg->origins;    
+  // Assemble and publish map
+  multi_map_server::MultiOccupancyGrid m;
+  m.maps.resize(maps2d.size());
+  m.origins.resize(maps2d.size());
+  for (unsigned int k = 0; k < maps2d.size(); k++)
+  {
+    m.maps[k]    = maps2d[k].GetMap();
+    m.origins[k] = origins2d[k];
+  }
+  pub1.publish(m);
+}
+
+void maps3d_callback(const multi_map_server::MultiSparseMap3D::ConstPtr &msg)
+{
+  // Update incremental map
+  maps3d.resize(msg->maps.size());  
+  for (unsigned int k = 0; k < msg->maps.size(); k++)
+    maps3d[k].UnpackMsg(msg->maps[k]);
+  origins3d = msg->origins;
+  // Publish
+  sensor_msgs::PointCloud m;
+  for (unsigned int k = 0; k < msg->maps.size(); k++)
+  {
+    colvec po(6);
+    po(0) = origins3d[k].position.x;
+    po(1) = origins3d[k].position.y;
+    po(2) = origins3d[k].position.z;
+    colvec poq(4);
+    poq(0) = origins3d[k].orientation.w;
+    poq(1) = origins3d[k].orientation.x;
+    poq(2) = origins3d[k].orientation.y;
+    poq(3) = origins3d[k].orientation.z;
+    po.rows(3,5) = R_to_ypr(quaternion_to_R(poq));
+    colvec tpo = po.rows(0,2);
+    mat    Rpo = ypr_to_R(po.rows(3,5));
+    vector<colvec> pts = maps3d[k].GetOccupancyWorldFrame(OCCUPIED);
+    for (unsigned int i = 0; i < pts.size(); i++)
+    {
+      colvec pt = Rpo * pts[i] + tpo;
+      geometry_msgs::Point32 _pt;
+      _pt.x = pt(0);
+      _pt.y = pt(1);
+      _pt.z = pt(2);
+      m.points.push_back(_pt);
+    }
+  }
+  // Publish
+  m.header.stamp    = ros::Time::now();
+  m.header.frame_id = string("/map");
+  pub2.publish(m);
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "multi_map_visualization");
+  ros::NodeHandle n("~");
+
+  ros::Subscriber sub1 = n.subscribe("dmaps2d", 1, maps2d_callback);
+  ros::Subscriber sub2 = n.subscribe("dmaps3d", 1, maps3d_callback);
+  pub1 = n.advertise<multi_map_server::MultiOccupancyGrid>("maps2d", 1, true);
+  pub2 = n.advertise<sensor_msgs::PointCloud>("map3d", 1, true);
+
+  ros::spin();
+  return 0;
+}
+

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc

@@ -0,0 +1,192 @@
+#include <iostream>
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud.h>
+#include <geometry_msgs/PoseArray.h>
+#include <laser_geometry/laser_geometry.h>
+#include <pose_utils.h>
+#include <multi_map_server/MultiOccupancyGrid.h>
+#include <multi_map_server/MultiSparseMap3D.h>
+#include <multi_map_server/Map2D.h>
+#include <multi_map_server/Map3D.h>
+
+using namespace arma;
+using namespace std;
+#define MAX_MAP_CNT 25
+
+ros::Publisher pub1;
+ros::Publisher pub2;
+
+// 2D Map
+int maps2dCnt = 0;
+Map2D maps2d[MAX_MAP_CNT];
+map_server_3d_new::MultiOccupancyGrid grids2d;
+// 3D Map
+int maps3dCnt = 0;
+Map3D maps3d[MAX_MAP_CNT];
+// Map origin from UKF
+vector<geometry_msgs::Pose> maps_origin;   
+
+void map2d_callback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
+{
+  // Update msg and publish
+  if (grids2d.maps.size() == 0)
+  {
+    // Init msg
+    grids2d.maps.push_back(*msg);
+    maps2dCnt++;
+  }
+  else if (grids2d.maps.back().info.map_load_time != msg->info.map_load_time)
+  {
+    // Add Costmap
+    nav_msgs::OccupancyGrid m;
+    maps2d[maps2dCnt-1].get_map(m);
+    mapMatcher.AddCostMap(m);
+    // Increase msg size
+    grids2d.maps.back().data.clear();
+    grids2d.maps.push_back(*msg);
+    maps2dCnt++;
+  }
+  else
+  {
+    grids2d.maps.back() = *msg;
+  }
+  pub1.publish(grids2d);
+  // Update internval map
+  maps2d[maps2dCnt-1].update(*msg);
+}
+
+void scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
+{
+  // Get Map Status
+  static bool isSLAMValid = false;
+  if (msg->intensities[10])
+  {
+    if (!isSLAMValid)
+    {
+      maps3dCnt++;
+      mapLinks.push_back(zeros<colvec>(3));
+    }
+    isSLAMValid = true;
+  }
+  else
+  {
+    isSLAMValid = false;
+    return;
+  }
+  // Scan cnt
+  mapLinks.back()(2)++;
+  // Get Current Pose
+  colvec pose(6);
+  pose(0) = msg->intensities[0];
+  pose(1) = msg->intensities[1];
+  pose(2) = msg->intensities[2];
+  pose(3) = msg->intensities[3];
+  pose(4) = msg->intensities[4];
+  pose(5) = msg->intensities[5];
+  colvec pose2D(3);
+  pose2D(0) = msg->intensities[0];
+  pose2D(1) = msg->intensities[1];
+  pose2D(2) = msg->intensities[3];
+  double currFloor  = msg->intensities[7];
+  double currLaserH = msg->intensities[8];
+  // Horizontal laser scans
+  sensor_msgs::LaserScan scan = *msg;
+  scan.intensities.clear();
+  laser_geometry::LaserProjection projector;
+  sensor_msgs::PointCloud scanCloud;
+  projector.projectLaser(scan, scanCloud);
+  mat scanMat(3, scanCloud.points.size());
+  // Get scan in body frame
+  for (int k = 0; k < scanCloud.points.size(); k++)
+  {
+    scanMat(0,k) = scanCloud.points[k].x;
+    scanMat(1,k) = scanCloud.points[k].y;
+    scanMat(2,k) = 0.215 - 0.09;
+  }
+  // Downsample Laser scan to map resolution
+  double resolution = maps3d[maps3dCnt-1].GetResolution();
+  double px = NUM_INF;
+  double py = NUM_INF;
+  int cnt = 0;
+  mat scanMatDown = zeros<mat>(3, scanMat.n_cols);
+  for (int k = 0; k < scanMat.n_cols; k++)
+  {
+    double x = scanMat(0,k);
+    double y = scanMat(1,k);
+    double dist = (x-px)*(x-px) + (y-py)*(y-py); 
+    if (dist > resolution * resolution)
+    {
+      scanMatDown.col(cnt) = scanMat.col(k);
+      px = x;
+      py = y;
+      cnt++;
+    }
+  }
+  if (cnt > 0)
+    scanMat = scanMatDown.cols(0, cnt-1);
+  else
+    scanMat = scanMatDown.cols(0,cnt);
+  // Transform to map local frame
+  scanMat = ypr_to_R(pose.rows(3,5)) * scanMat + pose.rows(0,2)*ones<rowvec>(scanMat.n_cols);
+  // Update map
+  for (int k = 0; k < scanMat.n_cols; k++)
+    maps3d[maps3dCnt-1].SetOccupancyFromWorldFrame(scanMat(0,k), scanMat(1,k), scanMat(2,k), PROB_LASER_OCCUPIED);
+  // downward facing laser scans
+  if (currLaserH > 0)
+  {
+    colvec pt(3);
+    pt(0) = 0;
+    pt(1) = 0;
+    pt(2) = -currLaserH;
+    pt =  ypr_to_R(pose.rows(3,5)) * pt + pose.rows(0,2);
+    double resolution = maps3d[maps3dCnt-1].GetResolution();
+    for (double x = -0.1; x <= 0.1; x+=resolution)
+      for (double y = -0.1; y <= 0.1; y+=resolution)
+        maps3d[maps3dCnt-1].SetOccupancyFromWorldFrame(pt(0)+x, pt(1)+y, pt(2), PROB_LASER_OCCUPIED);          
+  }
+  // Floor Levels
+  maps3d[maps3dCnt-1].SetFloor(currFloor);
+}
+
+void maps_origin_callback(const geometry_msgs::PoseArray::ConstPtr &msg)
+{
+  maps_origin = msg->poses;
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "multi_map_server_3d");
+  ros::NodeHandle n("~");
+
+  ros::Subscriber sub1 = n.subscribe("dmap2d",       100, map2d_callback);
+  ros::Subscriber sub2 = n.subscribe("scan",         100, scan_callback);
+  ros::Subscriber sub3 = n.subscribe("/maps_origin", 100, maps_origin_callback);
+  pub1 = n.advertise<multi_map_server::MultiOccupancyGrid>("dmaps2d", 10, true); 
+  pub2 = n.advertise<multi_map_server::MultiSparseMap3D>(  "dmaps3d", 10, true); 
+
+  ros::Rate r(100.0);  
+  int cnt = 0;
+  while (n.ok())
+  {
+    cnt++;
+    if (cnt > 100)
+    {
+      cnt = 0;
+      map_server_3d_new::MultiSparseMap3D msg;
+      msg.maps_origin = maps_origin;
+      for (int k = 0; k < maps3dCnt; k++)
+      {
+        msg.maps_active.push_back((bool)(!mapLinks[k](0)) && mapLinks[k](2) > 50);
+        map_server_3d_new::SparseMap3D m;
+        maps3d[k].GetSparseMap3DMsg(m);
+        m.header.seq = k;
+        msg.maps.push_back(m);
+      }
+      pub2.publish(msg);
+    }
+    ros::spinOnce();
+    r.sleep();
+  }
+  return 0;
+}

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/CMakeLists.txt

@@ -0,0 +1,211 @@
+#cmake_minimum_required(VERSION 2.4.6)
+#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+#rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+
+#rosbuild_add_executable(odom_visualization src/odom_visualization.cpp)
+#target_link_libraries(odom_visualization pose_utils)
+
+#----------------------------------
+cmake_minimum_required(VERSION 2.8.3)
+project(odom_visualization)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-O3 -Wall -g")
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+  nav_msgs
+  visualization_msgs
+  quadrotor_msgs
+  tf
+  pose_utils
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+#add_message_files(
+#   FILES
+#   MultiOccupancyGrid.msg
+#   MultiSparseMap3D.msg
+#   SparseMap3D.msg
+#   VerticalOccupancyGridList.msg
+   #plan_cmd.msg
+#)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+#generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs
+#   nav_msgs
+#)
+
+###################################
+## 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 you 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
+#  LIBRARIES irobot_msgs
+#  CATKIN_DEPENDS geometry_msgs nav_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+find_package(Armadillo REQUIRED)
+include_directories(${ARMADILLO_INCLUDE_DIRS})
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+# add_library(irobot_msgs
+#   src/${PROJECT_NAME}/irobot_msgs.cpp
+# )
+
+## Declare a cpp executable
+add_executable(odom_visualization src/odom_visualization.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(multi_map_visualization multi_map_server_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(odom_visualization
+   ${catkin_LIBRARIES}
+   ${ARMADILLO_LIBRARIES}
+   pose_utils
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS irobot_msgs irobot_msgs_node
+#   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"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b odom_visualization is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/package.xml

@@ -0,0 +1,31 @@
+<package>
+  <version>0.0.0</version>
+  <name>odom_visualization</name>
+  <description>
+
+     odom_visualization
+
+  </description>
+  <maintainer email="eeshaojie@todo.todo">Shaojie Shen</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/odom_visualization</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>pose_utils</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>pose_utils</run_depend>
+
+</package>
+
+

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp

@@ -0,0 +1,472 @@
+#include <iostream>
+#include <string.h>
+#include "ros/ros.h"
+#include "tf/transform_broadcaster.h"
+#include "nav_msgs/Odometry.h"
+#include "geometry_msgs/PoseWithCovarianceStamped.h"
+#include "geometry_msgs/PoseStamped.h"
+#include "nav_msgs/Path.h"
+#include "sensor_msgs/Range.h"
+#include "visualization_msgs/Marker.h"
+#include "armadillo"
+#include "pose_utils.h"
+#include "quadrotor_msgs/PositionCommand.h"
+
+using namespace arma;
+using namespace std;
+
+static  string mesh_resource;
+static double color_r, color_g, color_b, color_a, cov_scale, scale;
+
+bool   cross_config = false;
+bool   tf45       = false;
+bool   cov_pos    = false;
+bool   cov_vel    = false;
+bool   cov_color  = false;
+bool   origin       = false;
+bool   isOriginSet  = false;
+colvec poseOrigin(6);
+ros::Publisher posePub;
+ros::Publisher pathPub;
+ros::Publisher velPub;
+ros::Publisher covPub;
+ros::Publisher covVelPub;
+ros::Publisher trajPub;
+ros::Publisher sensorPub;
+ros::Publisher meshPub;
+ros::Publisher heightPub;
+tf::TransformBroadcaster* broadcaster;
+geometry_msgs::PoseStamped poseROS;
+nav_msgs::Path             pathROS;
+visualization_msgs::Marker velROS;
+visualization_msgs::Marker covROS;
+visualization_msgs::Marker covVelROS;
+visualization_msgs::Marker trajROS;
+visualization_msgs::Marker sensorROS;
+visualization_msgs::Marker meshROS;
+sensor_msgs::Range         heightROS;
+string _frame_id;
+
+void odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+  if (msg->header.frame_id == string("null"))
+    return;
+  colvec pose(6);  
+  pose(0) = msg->pose.pose.position.x;
+  pose(1) = msg->pose.pose.position.y;
+  pose(2) = msg->pose.pose.position.z;
+  colvec q(4);
+  q(0)    = msg->pose.pose.orientation.w;
+  q(1)    = msg->pose.pose.orientation.x;
+  q(2)    = msg->pose.pose.orientation.y;
+  q(3)    = msg->pose.pose.orientation.z;
+  pose.rows(3,5) = R_to_ypr(quaternion_to_R(q));
+  colvec vel(3);
+  vel(0) = msg->twist.twist.linear.x;
+  vel(1) = msg->twist.twist.linear.y;
+  vel(2) = msg->twist.twist.linear.z;  
+  
+  if (origin && !isOriginSet)
+  {
+    isOriginSet = true;
+    poseOrigin  = pose;
+  }
+  if (origin)
+  {
+    vel  = trans(ypr_to_R(pose.rows(3,5))) * vel;  
+    pose = pose_update(pose_inverse(poseOrigin), pose);
+    vel  = ypr_to_R(pose.rows(3,5)) * vel;  
+  }
+
+  // Pose
+  poseROS.header = msg->header;
+  poseROS.header.stamp = msg->header.stamp;
+  poseROS.header.frame_id = string("world");
+  poseROS.pose.position.x = pose(0);
+  poseROS.pose.position.y = pose(1);
+  poseROS.pose.position.z = pose(2);
+  q = R_to_quaternion(ypr_to_R(pose.rows(3,5)));
+  poseROS.pose.orientation.w = q(0);
+  poseROS.pose.orientation.x = q(1);
+  poseROS.pose.orientation.y = q(2);
+  poseROS.pose.orientation.z = q(3);      
+  posePub.publish(poseROS);
+  
+  // Velocity
+  colvec yprVel(3);
+  yprVel(0) =  atan2(vel(1), vel(0));
+  yprVel(1) = -atan2(vel(2), norm(vel.rows(0,1),2));
+  yprVel(2) = 0;
+  q = R_to_quaternion(ypr_to_R(yprVel));    
+  velROS.header.frame_id = string("world");
+  velROS.header.stamp = msg->header.stamp;
+  velROS.ns = string("velocity");
+  velROS.id = 0;
+  velROS.type = visualization_msgs::Marker::ARROW;
+  velROS.action = visualization_msgs::Marker::ADD;
+  velROS.pose.position.x = pose(0);
+  velROS.pose.position.y = pose(1);
+  velROS.pose.position.z = pose(2);
+  velROS.pose.orientation.w = q(0);
+  velROS.pose.orientation.x = q(1);
+  velROS.pose.orientation.y = q(2);
+  velROS.pose.orientation.z = q(3);      
+  velROS.scale.x = norm(vel, 2); 
+  velROS.scale.y = 0.05;
+  velROS.scale.z = 0.05;
+  velROS.color.a = 1.0;
+  velROS.color.r = color_r;
+  velROS.color.g = color_g;
+  velROS.color.b = color_b;
+  velPub.publish(velROS);
+
+  // Path
+  static ros::Time prevt = msg->header.stamp;
+  if ((msg->header.stamp - prevt).toSec() > 0.1)
+  {
+    prevt = msg->header.stamp;
+    pathROS.header = poseROS.header;
+    pathROS.poses.push_back(poseROS);
+    pathPub.publish(pathROS);
+  }
+
+  // Covariance color
+  double r = 1;
+  double g = 1;
+  double b = 1;
+  bool G = msg->twist.covariance[33];
+  bool V = msg->twist.covariance[34];
+  bool L = msg->twist.covariance[35];
+  if (cov_color)
+  {
+    r = G;
+    g = V;
+    b = L;
+  }
+  
+  // Covariance Position
+  if (cov_pos)
+  {
+    mat P(6,6);
+    for (int j = 0; j < 6; j++)
+      for (int i = 0; i < 6; i++)
+        P(i,j) = msg->pose.covariance[i+j*6];
+    colvec eigVal;
+    mat    eigVec;
+    eig_sym(eigVal, eigVec, P.submat(0,0,2,2));
+    if (det(eigVec) < 0)
+    {
+      for (int k = 0; k < 3; k++)
+      {
+        mat eigVecRev = eigVec;
+        eigVecRev.col(k) *= -1;
+        if (det(eigVecRev) > 0)
+        {
+          eigVec = eigVecRev;
+          break;
+        }
+      }
+    }
+    covROS.header.frame_id = string("world");
+    covROS.header.stamp = msg->header.stamp;
+    covROS.ns = string("covariance");
+    covROS.id = 0;
+    covROS.type = visualization_msgs::Marker::SPHERE;
+    covROS.action = visualization_msgs::Marker::ADD;
+    covROS.pose.position.x = pose(0);
+    covROS.pose.position.y = pose(1);
+    covROS.pose.position.z = pose(2);
+    q = R_to_quaternion(eigVec);
+    covROS.pose.orientation.w = q(0);
+    covROS.pose.orientation.x = q(1);
+    covROS.pose.orientation.y = q(2);
+    covROS.pose.orientation.z = q(3);      
+    covROS.scale.x = sqrt(eigVal(0))*cov_scale;
+    covROS.scale.y = sqrt(eigVal(1))*cov_scale;
+    covROS.scale.z = sqrt(eigVal(2))*cov_scale;
+    covROS.color.a = 0.4;
+    covROS.color.r = r * 0.5;
+    covROS.color.g = g * 0.5;
+    covROS.color.b = b * 0.5;
+    covPub.publish(covROS);
+  }
+
+  // Covariance Velocity
+  if (cov_vel)
+  {
+    mat P(3,3);
+    for (int j = 0; j < 3; j++)
+      for (int i = 0; i < 3; i++)
+        P(i,j) = msg->twist.covariance[i+j*6];
+    mat R = ypr_to_R(pose.rows(3,5));
+    P = R * P * trans(R);
+    colvec eigVal;
+    mat    eigVec;
+    eig_sym(eigVal, eigVec, P);
+    if (det(eigVec) < 0)
+    {
+      for (int k = 0; k < 3; k++)
+      {
+        mat eigVecRev = eigVec;
+        eigVecRev.col(k) *= -1;
+        if (det(eigVecRev) > 0)
+        {
+          eigVec = eigVecRev;
+          break;
+        }
+      }
+    }
+    covVelROS.header.frame_id = string("world");
+    covVelROS.header.stamp = msg->header.stamp;
+    covVelROS.ns = string("covariance_velocity");
+    covVelROS.id = 0;
+    covVelROS.type = visualization_msgs::Marker::SPHERE;
+    covVelROS.action = visualization_msgs::Marker::ADD;
+    covVelROS.pose.position.x = pose(0);
+    covVelROS.pose.position.y = pose(1);
+    covVelROS.pose.position.z = pose(2);
+    q = R_to_quaternion(eigVec);
+    covVelROS.pose.orientation.w = q(0);
+    covVelROS.pose.orientation.x = q(1);
+    covVelROS.pose.orientation.y = q(2);
+    covVelROS.pose.orientation.z = q(3);      
+    covVelROS.scale.x = sqrt(eigVal(0))*cov_scale;
+    covVelROS.scale.y = sqrt(eigVal(1))*cov_scale;
+    covVelROS.scale.z = sqrt(eigVal(2))*cov_scale;
+    covVelROS.color.a = 0.4;
+    covVelROS.color.r = r;
+    covVelROS.color.g = g;
+    covVelROS.color.b = b;
+    covVelPub.publish(covVelROS);
+  }
+
+  // Color Coded Trajectory
+  static colvec ppose = pose;
+  static ros::Time pt = msg->header.stamp;
+  ros::Time t = msg->header.stamp;
+  if ((t - pt).toSec() > 0.5)
+  {
+    trajROS.header.frame_id = string("world");
+    trajROS.header.stamp    = ros::Time::now();
+    trajROS.ns              = string("trajectory");
+    trajROS.type            = visualization_msgs::Marker::LINE_LIST;
+    trajROS.action          = visualization_msgs::Marker::ADD;  
+    trajROS.pose.position.x    = 0;
+    trajROS.pose.position.y    = 0;
+    trajROS.pose.position.z    = 0;  
+    trajROS.pose.orientation.w = 1;  
+    trajROS.pose.orientation.x = 0;  
+    trajROS.pose.orientation.y = 0;  
+    trajROS.pose.orientation.z = 0;
+    trajROS.scale.x = 0.1;
+    trajROS.scale.y = 0;
+    trajROS.scale.z = 0;
+    trajROS.color.r = 0.0;
+    trajROS.color.g = 1.0;
+    trajROS.color.b = 0.0;
+    trajROS.color.a = 0.8;  
+    geometry_msgs::Point p;          
+    p.x = ppose(0);
+    p.y = ppose(1);
+    p.z = ppose(2);
+    trajROS.points.push_back(p);   
+    p.x = pose(0);
+    p.y = pose(1);
+    p.z = pose(2);
+    trajROS.points.push_back(p);    
+    std_msgs::ColorRGBA color;
+    color.r = r;
+    color.g = g;
+    color.b = b;
+    color.a = 1;
+    trajROS.colors.push_back(color);
+    trajROS.colors.push_back(color);
+    ppose = pose;
+    pt = t;
+    trajPub.publish(trajROS);
+  }
+
+  // Sensor availability
+  sensorROS.header.frame_id = string("world");
+  sensorROS.header.stamp    = msg->header.stamp;
+  sensorROS.ns              = string("sensor");
+  sensorROS.type            = visualization_msgs::Marker::TEXT_VIEW_FACING;
+  sensorROS.action          = visualization_msgs::Marker::ADD;  
+  sensorROS.pose.position.x = pose(0);
+  sensorROS.pose.position.y = pose(1);
+  sensorROS.pose.position.z = pose(2) + 1.0;
+  sensorROS.pose.orientation.w = q(0);
+  sensorROS.pose.orientation.x = q(1);
+  sensorROS.pose.orientation.y = q(2);
+  sensorROS.pose.orientation.z = q(3);      
+  string strG = G?string(" GPS "):string("");
+  string strV = V?string(" Vision "):string("");
+  string strL = L?string(" Laser "):string("");
+  sensorROS.text = "| " + strG + strV + strL + " |";
+  sensorROS.color.a = 1.0;
+  sensorROS.color.r = 1.0;
+  sensorROS.color.g = 1.0;
+  sensorROS.color.b = 1.0;
+  sensorROS.scale.z = 0.5;
+  sensorPub.publish(sensorROS);
+
+  // Laser height measurement
+  double H = msg->twist.covariance[32];
+  heightROS.header.frame_id = string("height");
+  heightROS.header.stamp = msg->header.stamp;
+  heightROS.radiation_type = sensor_msgs::Range::ULTRASOUND;
+  heightROS.field_of_view = 5.0 * M_PI / 180.0;
+  heightROS.min_range = -100;
+  heightROS.max_range =  100;
+  heightROS.range     =  H; 
+  heightPub.publish(heightROS);
+
+  // Mesh model                                                  
+  meshROS.header.frame_id = _frame_id;
+  meshROS.header.stamp = msg->header.stamp; 
+  meshROS.ns = "mesh";
+  meshROS.id = 0;
+  meshROS.type = visualization_msgs::Marker::MESH_RESOURCE;
+  meshROS.action = visualization_msgs::Marker::ADD;
+  meshROS.pose.position.x = msg->pose.pose.position.x;
+  meshROS.pose.position.y = msg->pose.pose.position.y;
+  meshROS.pose.position.z = msg->pose.pose.position.z;
+  q(0) = msg->pose.pose.orientation.w;
+  q(1) = msg->pose.pose.orientation.x;
+  q(2) = msg->pose.pose.orientation.y;
+  q(3) = msg->pose.pose.orientation.z;
+  if (cross_config)
+  {
+    colvec ypr = R_to_ypr(quaternion_to_R(q));
+    ypr(0)    += 45.0*PI/180.0;
+    q          = R_to_quaternion(ypr_to_R(ypr)); 
+  }  
+  meshROS.pose.orientation.w = q(0);
+  meshROS.pose.orientation.x = q(1);
+  meshROS.pose.orientation.y = q(2);
+  meshROS.pose.orientation.z = q(3);
+  meshROS.scale.x = scale;
+  meshROS.scale.y = scale;
+  meshROS.scale.z = scale;
+  meshROS.color.a = color_a;
+  meshROS.color.r = color_r;
+  meshROS.color.g = color_g;
+  meshROS.color.b = color_b;
+  meshROS.mesh_resource = mesh_resource;
+  meshPub.publish(meshROS);                                                  
+
+  // TF for raw sensor visualization
+  if (tf45)
+  {
+    tf::Transform transform;
+    transform.setOrigin(tf::Vector3(pose(0), pose(1), pose(2)));
+    transform.setRotation(tf::Quaternion(q(1), q(2), q(3), q(0)));
+
+    tf::Transform transform45;
+    transform45.setOrigin(tf::Vector3(0, 0, 0));
+    colvec y45 = zeros<colvec>(3);
+    y45(0) = 45.0 * M_PI/180;
+    colvec q45 = R_to_quaternion(ypr_to_R(y45));  
+    transform45.setRotation(tf::Quaternion(q45(1), q45(2), q45(3), q45(0)));  
+
+    tf::Transform transform90;
+    transform90.setOrigin(tf::Vector3(0, 0, 0));
+    colvec p90 = zeros<colvec>(3);
+    p90(1) = 90.0 * M_PI/180;
+    colvec q90 = R_to_quaternion(ypr_to_R(p90));  
+    transform90.setRotation(tf::Quaternion(q90(1), q90(2), q90(3), q90(0)));  
+
+    broadcaster->sendTransform(tf::StampedTransform(transform,   msg->header.stamp, string("world"),  string("base")));      
+    broadcaster->sendTransform(tf::StampedTransform(transform45, msg->header.stamp, string("base"), string("laser")));          
+    broadcaster->sendTransform(tf::StampedTransform(transform45, msg->header.stamp, string("base"), string("vision")));          
+    broadcaster->sendTransform(tf::StampedTransform(transform90, msg->header.stamp, string("base"), string("height")));          
+  } 
+}
+
+void cmd_callback(const quadrotor_msgs::PositionCommand cmd)
+{   
+  if (cmd.header.frame_id == string("null"))
+    return;
+  
+  colvec pose(6);  
+  pose(0) = cmd.position.x;
+  pose(1) = cmd.position.y;
+  pose(2) = cmd.position.z;
+  colvec q(4);
+  q(0)    = 1.0;
+  q(1)    = 0.0;
+  q(2)    = 0.0;
+  q(3)    = 0.0;
+  pose.rows(3,5) = R_to_ypr(quaternion_to_R(q));
+  
+  // Mesh model                                                  
+  meshROS.header.frame_id = _frame_id;
+  meshROS.header.stamp = cmd.header.stamp; 
+  meshROS.ns = "mesh";
+  meshROS.id = 0;
+  meshROS.type = visualization_msgs::Marker::MESH_RESOURCE;
+  meshROS.action = visualization_msgs::Marker::ADD;
+  meshROS.pose.position.x = cmd.position.x;
+  meshROS.pose.position.y = cmd.position.y;
+  meshROS.pose.position.z = cmd.position.z;
+ 
+  if (cross_config)
+  {
+    colvec ypr = R_to_ypr(quaternion_to_R(q));
+    ypr(0)    += 45.0*PI/180.0;
+    q          = R_to_quaternion(ypr_to_R(ypr)); 
+  }  
+  meshROS.pose.orientation.w = q(0);
+  meshROS.pose.orientation.x = q(1);
+  meshROS.pose.orientation.y = q(2);
+  meshROS.pose.orientation.z = q(3);
+  meshROS.scale.x = 2.0;
+  meshROS.scale.y = 2.0;
+  meshROS.scale.z = 2.0;
+  meshROS.color.a = color_a;
+  meshROS.color.r = color_r;
+  meshROS.color.g = color_g;
+  meshROS.color.b = color_b;
+  meshROS.mesh_resource = mesh_resource;
+  meshPub.publish(meshROS);                                                  
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "odom_visualization");
+  ros::NodeHandle n("~");
+
+  n.param("mesh_resource", mesh_resource, std::string("package://odom_visualization/meshes/hummingbird.mesh"));
+  n.param("color/r", color_r, 1.0);
+  n.param("color/g", color_g, 0.0);
+  n.param("color/b", color_b, 0.0);
+  n.param("color/a", color_a, 1.0);
+  n.param("origin", origin, false);  
+  n.param("robot_scale", scale, 2.0);    
+  n.param("frame_id",   _frame_id, string("world") );    
+ 
+  n.param("cross_config", cross_config, false);    
+  n.param("tf45", tf45, false);    
+  n.param("covariance_scale",    cov_scale,  100.0);
+  n.param("covariance_position", cov_pos,    false);    
+  n.param("covariance_velocity", cov_vel,    false);    
+  n.param("covariance_color",    cov_color,  false);    
+  
+  ros::Subscriber sub_odom = n.subscribe("odom", 100,  odom_callback);
+  ros::Subscriber sub_cmd  = n.subscribe("cmd",  100,  cmd_callback);
+  posePub   = n.advertise<geometry_msgs::PoseStamped>("pose",                100, true);
+  pathPub   = n.advertise<nav_msgs::Path>(            "path",                100, true);
+  velPub    = n.advertise<visualization_msgs::Marker>("velocity",            100, true);
+  covPub    = n.advertise<visualization_msgs::Marker>("covariance",          100, true);
+  covVelPub = n.advertise<visualization_msgs::Marker>("covariance_velocity", 100, true);
+  trajPub   = n.advertise<visualization_msgs::Marker>("trajectory",          100, true);
+  sensorPub = n.advertise<visualization_msgs::Marker>("sensor",              100, true);
+  meshPub   = n.advertise<visualization_msgs::Marker>("robot",               100, true);  
+  heightPub = n.advertise<sensor_msgs::Range>(        "height",              100, true);  
+  tf::TransformBroadcaster b;
+  broadcaster = &b;
+
+  ros::spin();
+
+  return 0;
+}

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(pose_utils)
+
+find_package(catkin REQUIRED COMPONENTS
+  #armadillo
+  roscpp
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES pose_utils
+#  CATKIN_DEPENDS geometry_msgs nav_msgs
+#  DEPENDS system_lib
+)
+
+find_package(Armadillo REQUIRED)
+
+include_directories(
+    ${catkin_INCLUDE_DIRS}
+    ${ARMADILLO_INCLUDE_DIRS}
+    include
+    )
+
+add_library(pose_utils 
+   ${ARMADILLO_LIBRARIES}
+   src/pose_utils.cpp)

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/include/pose_utils.h

@@ -0,0 +1,53 @@
+#ifndef POSE_UTILS_H
+#define POSE_UTILS_H
+
+#include <iostream>
+#include "armadillo"
+
+#define PI 3.14159265359
+#define NUM_INF 999999.9
+
+using namespace arma;
+using namespace std;
+
+// Rotation ---------------------
+mat ypr_to_R(const colvec& ypr);
+
+mat yaw_to_R(double yaw);
+
+colvec R_to_ypr(const mat& R);
+
+mat quaternion_to_R(const colvec& q);
+
+colvec R_to_quaternion(const mat& R);
+
+colvec quaternion_mul(const colvec& q1, const colvec& q2);
+
+colvec quaternion_inv(const colvec& q);
+
+// General Pose Update ----------
+colvec pose_update(const colvec& X1, const colvec& X2);
+
+colvec pose_inverse(const colvec& X);
+
+colvec pose_update_2d(const colvec& X1, const colvec& X2);
+
+colvec pose_inverse_2d(const colvec& X);
+
+// For Pose EKF -----------------
+mat Jplus1(const colvec& X1, const colvec& X2);
+
+mat Jplus2(const colvec& X1, const colvec& X2);
+
+// For IMU EKF ------------------
+colvec state_update(const colvec& X, const colvec& U, double dt);
+
+mat jacobianF(const colvec& X, const colvec& U, double dt);
+
+mat jacobianU(const colvec& X, const colvec& U, double dt);
+
+colvec state_measure(const colvec& X);
+
+mat jacobianH();
+
+#endif