Merge branch 'xulingxin_branch'

src/.vs/ProjectSettings.json

@@ -0,0 +1,3 @@
+{
+  "CurrentProjectSetting": "无配置"
+}

src/.vs/VSWorkspaceState.json

@@ -0,0 +1,7 @@
+{
+  "ExpandedNodes": [
+    ""
+  ],
+  "SelectedNode": "\\避障",
+  "PreviewInSolutionExplorer": false
+}

src/Qt尝试预测/Qt_0.cpp

@@ -0,0 +1,19 @@
+QPixmap pixmap("image.png");  // 加载图像
+QPainter painter(&pixmap);    // 创建绘图对象
+
+// 指定矩形范围（这里以左上角为起点，宽高为100像素的矩形为例）
+int x = 0;
+int y = 0;
+int width = 100;
+int height = 100;
+
+// 创建颜色对象（这里使用Qt中预定义的红色）
+QColor color(Qt::red);
+
+// 填充指定范围内的像素
+painter.fillRect(x, y, width, height, color);
+
+// 显示图像
+QLabel label;
+label.setPixmap(pixmap);
+label.show();

src/Qt尝试预测/Qt_1.cpp

@@ -0,0 +1,43 @@
+QPixmap pixmap("image.png");  // 加载图像
+QPainter painter(&pixmap);    // 创建绘图对象
+
+// 指定矩形范围（这里以左上角为起点，宽高为100像素的矩形为例）
+int x = 0;
+int y = 0;
+int width = 100;
+int height = 100;
+
+// 创建画笔对象（这里使用Qt中预定义的红色）
+QPen pen(Qt::red);
+pen.setWidth(2);  // 设置边框宽度
+
+// 绘制矩形框
+painter.setPen(pen);
+painter.drawRect(x, y, width, height);
+
+// 显示图像
+QLabel label;
+label.setPixmap(pixmap);
+label.show();
+
+
+
+
+// 指定矩形范围（这里以左上角为起点，宽高为100像素的矩形为例）
+int x = 0;
+int y = 0;
+int width = 100;
+int height = 100;
+
+// 获取像素大小
+QSize size = pixmap.size();        // 获取图像尺寸
+double pixelSizeX = 1.0 / size.width();   // 计算x方向每个像素在实际场景中所代表的长度
+double pixelSizeY = 1.0 / size.height();  // 计算y方向每个像素在实际场景中所代表的长度
+
+// 计算实际场景中的长度
+int pixelCount = width * height;           // 计算指定范围内的像素数量
+double lengthX = pixelCount * pixelSizeX;  // 计算x方向实际场景中的长度
+double lengthY = pixelCount * pixelSizeY;  // 计算y方向实际场景中的长度
+
+qDebug() << "实际场景中的长度（x方向）：" << lengthX;
+qDebug() << "实际场景中的长度（y方向）：" << lengthY;

src/Qt预测_封装/估算距离.cpp

@@ -0,0 +1,42 @@
+#include <QCoreApplication>
+#include <QDebug>
+#include <cmath>
+
+double calculateRegionSize(int pixel_size, int top_left_pixel_x, int top_left_pixel_y, int bottom_right_pixel_x, int bottom_right_pixel_y, int h, int size)
+{
+    // 计算选择区域内像素数
+    int pixels_in_region = (bottom_right_pixel_x - top_left_pixel_x) * (bottom_right_pixel_y - top_left_pixel_y);
+
+    // 计算像素对应实际大小
+    double real_size = (h * 1000.0) * pixel_size / size; // 单位：mm/像素
+
+    // 计算选定区域的实际大小（单位：米）
+    double region_size = std::sqrt(pixels_in_region * real_size * real_size) / 1000.0; // 单位：m
+
+    return region_size;
+}
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    // 相机像素大小 像素与像素距离
+    int pixel_size = 5;
+
+    // 指定区域范围，左上角和右下角像素坐标
+    int top_left_pixel_x = 100;
+    int top_left_pixel_y = 150;
+    int bottom_right_pixel_x = 200;
+    int bottom_right_pixel_y = 250;
+
+    // 估计参数
+    int h = 10;
+    int size = 100;
+
+    // 计算选定区域的实际大小（单位：米）
+    double region_size = calculateRegionSize(pixel_size, top_left_pixel_x, top_left_pixel_y, bottom_right_pixel_x, bottom_right_pixel_y, h, size);
+
+    qDebug() << "The selected region size is:" << region_size << "m.";
+
+    return a.exec();
+}

src/Qt预测_封装/标记火源.cpp

@@ -0,0 +1,36 @@
+#include <QApplication>
+#include <QWidget>
+#include <QLabel>
+#include <QPixmap>
+#include <QPainter>
+
+// 将矩形范围内绘制为指定颜色的矩形，并添加透明度
+void drawRectWithTransparency(QPixmap& pixmap, const QRect& rect, const QColor& color, int transparency)
+{
+    QPainter painter(&pixmap);
+    painter.setCompositionMode(QPainter::CompositionMode_Clear);
+    painter.fillRect(rect, QColor(0, 0, 0, 0));
+    painter.setCompositionMode(QPainter::CompositionMode_SourceOver);
+    painter.fillRect(rect, QColor(color.red(), color.green(), color.blue(), transparency));
+}
+
+int main(int argc, char *argv[])
+{
+    QApplication app(argc, argv);
+
+    // 创建窗口和标签
+    QWidget window;
+    QLabel label(&window);
+
+    // 加载图片
+    QPixmap pixmap(":1.png");
+    QRect rect(360, 360, 300, 250);
+    drawRectWithTransparency(pixmap, rect, QColor(255, 0, 0), 100);
+    label.setPixmap(pixmap);
+
+    // 显示窗口和标签
+    window.show();
+    label.show();
+
+    return app.exec();
+}

src/Qt预测_封装/火势预测.cpp

@@ -0,0 +1,47 @@
+#include <QApplication>
+#include <QWidget>
+#include <QLabel>
+#include <QPixmap>
+#include <QPainter>
+#include <QVBoxLayout>
+
+void drawRectOnImage(QString imagePath, QRect rect1, QRect rect2, QColor color1, QColor color2)
+{
+    // 加载图片
+    QPixmap pixmap(imagePath);
+
+    // 指定范围并绘制透明红色和黑色矩形
+    QPainter painter(&pixmap);
+    painter.setCompositionMode(QPainter::CompositionMode_Clear);
+    painter.fillRect(rect1, QColor(0, 0, 0, 0));
+    painter.fillRect(rect2, color2); // 黑色矩形
+    painter.setCompositionMode(QPainter::CompositionMode_SourceOver);
+    painter.fillRect(rect1, color1);
+
+    // 显示图片
+    QWidget *mainWindow = new QWidget();
+    QVBoxLayout *layout = new QVBoxLayout(mainWindow);
+    QLabel *label = new QLabel(mainWindow);
+    layout->addWidget(label);
+    label->setFixedSize(pixmap.size());
+    label->setPixmap(pixmap);
+    mainWindow->show();
+
+    // 进入主事件循环
+    QApplication::exec();
+    delete mainWindow;
+}
+
+int main(int argc, char *argv[])
+{
+    QApplication app(argc, argv);
+
+    // 绘制矩形到图片中，并在标签上显示图片
+    QRect rect1(360, 360, 300, 250);
+    QRect rect2(400, 400, 300, 250);
+    QColor color1(255, 0, 0, 100);
+    QColor color2(0, 0, 255, 100);
+    drawRectOnImage(":1.png", rect1, rect2, color1, color2);
+
+    return 0;
+}

src/航道/A_star.cpp

@@ -10,7 +10,6 @@ Astar::~Astar()
 {
   for (int i = 0; i < max_search_num; i++)
   {
-    // delete表示释放堆内存
     delete path_node_pool_[i];
   }
 }
@@ -134,7 +133,7 @@ int Astar::search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt)
     cur_node->node_state = IN_CLOSE_SET;  // in expand set
     iter_num_ += 1;
 
-    /* ---------- init neighbor expansion ---------- */
+    // init neighbor expansion 
     Eigen::Vector3d cur_pos = cur_node->position;
     Eigen::Vector3d expand_node_pos;
 
@@ -162,9 +161,7 @@ int Astar::search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt)
           {
             continue;
           }
-          
+          expand_node_pos = cur_pos + d_pos;// 扩展节点的位置
-          // 扩展节点的位置
-          expand_node_pos = cur_pos + d_pos;
 
           // 确认该点在地图范围内
           if(!Occupy_map_ptr->isInMap(expand_node_pos))
@@ -236,8 +233,7 @@ int Astar::search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt)
   
   }
 
-  // 搜索完所有可行点，即使没达到最大搜索次数，也没有找到路径
+  //无可通行路径
-  // 这种一般是因为无人机周围被占据，或者无人机与目标点之间无可通行路径造成的
   pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "max_search_num: open set empty.");
   return NO_PATH;
 }
@@ -253,7 +249,6 @@ void Astar::retrievePath(NodePtr end_node)
     cur_node = cur_node->parent;
     path_nodes_.push_back(cur_node);
   }
-
   // 反转顺序
   reverse(path_nodes_.begin(), path_nodes_.end());
   // 生成路径

src/航道/occupy_map.cpp

@@ -26,10 +26,6 @@ void Occupy_map::init(ros::NodeHandle& nh)
     global_pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/prometheus/planning/global_pcl",  10); 
     // 发布膨胀后的点云
     inflate_pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/prometheus/planning/global_inflate_pcl", 1);
- 
-    // 发布二维占据图？
-    // 发布膨胀后的二维占据图？
-
     this->inv_resolution_ = 1.0 / resolution_;
     for (int i = 0; i < 3; ++i)
     {
@@ -44,7 +40,7 @@ void Occupy_map::init(ros::NodeHandle& nh)
     min_range_ = origin_;
     max_range_ = origin_ + map_size_3d_;   
 
-    // 对于二维情况，重新限制点云高度
+    // 对于二维重新限制点云高度
     if(is_2D == true)
     {
         min_range_(2) = fly_height_2D - resolution_;
@@ -62,19 +58,17 @@ void Occupy_map::map_update_gpcl(const sensor_msgs::PointCloud2ConstPtr & global
 // 地图更新函数 - 输入：局部点云
 void Occupy_map::map_update_lpcl(const sensor_msgs::PointCloud2ConstPtr & local_point, const nav_msgs::Odometry & odom)
 {
-    has_global_point = true;
+    has_global_point = true;// 局部点云转为全局点云
-// 将传递过来的局部点云转为全局点云
 }
 
 // 地图更新函数 - 输入：laser
 void Occupy_map::map_update_laser(const sensor_msgs::LaserScanConstPtr & local_point, const nav_msgs::Odometry & odom)
 {
-    has_global_point = true;
+    has_global_point = true;//转为全局点云
-// 将传递过来的数据转为全局点云
 }
 
 // 当global_planning节点接收到点云消息更新时，进行设置点云指针并膨胀
-// Astar规划路径时，采用的是此处膨胀后的点云（setOccupancy只在本函数中使用）
+// Astar规划路径时，采用膨胀点云
 void Occupy_map::inflate_point_cloud(void)
 {
     if(!has_global_point)
@@ -82,19 +76,15 @@ void Occupy_map::inflate_point_cloud(void)
         pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Occupy_map [inflate point cloud]: don't have global point, can't inflate!\n");
         return;
     }
-
     // 发布未膨胀点云
     global_pcl_pub.publish(*global_env_);
-
+    //开始时间
-    //记录开始时间
     ros::Time time_start = ros::Time::now();
 
     // 转化为PCL的格式进行处理
     pcl::PointCloud<pcl::PointXYZ> latest_global_cloud_;
     pcl::fromROSMsg(*global_env_, latest_global_cloud_);
-
     //printf("time 1 take %f [s].\n",   (ros::Time::now()-time_start).toSec());
-
     if ((int)latest_global_cloud_.points.size() == 0)  
     {return;}
 
@@ -121,13 +111,12 @@ void Occupy_map::inflate_point_cloud(void)
         {
             continue;
         }
-
         // 根据膨胀距离，膨胀该点
         for (int x = -ifn; x <= ifn; ++x)
             for (int y = -ifn; y <= ifn; ++y)
                 for (int z = -ifn; z <= ifn; ++z) 
                 {
-                    // 为什么Z轴膨胀一半呢？ z 轴其实可以不膨胀
+                    // z 轴不膨胀
                     p3d_inf(0) = pt_inf.x = p3d(0) + x * resolution_;
                     p3d_inf(1) = pt_inf.y = p3d(1) + y * resolution_;
                     p3d_inf(2) = pt_inf.z = p3d(2) + 0.5 * z * resolution_;
@@ -156,10 +145,9 @@ void Occupy_map::inflate_point_cloud(void)
     static int exec_num=0;
     exec_num++;
 
-    // 此处改为根据循环时间计算的数值
+    // 根据循环时间计算数值
     if(exec_num == 20)
     {
-        // 膨胀地图效率与地图大小有关（有点久，Astar更新频率是多久呢？ 怎么才能提高膨胀效率呢？）
         printf("inflate global point take %f [s].\n",   (ros::Time::now()-time_start).toSec());
         exec_num=0;
     }  

src/避障/A_star.cpp

@@ -0,0 +1,370 @@
+#include "A_star.h"
+
+using namespace std;
+using namespace Eigen;
+
+namespace Global_Planning
+{
+
+Astar::~Astar()
+{
+  for (int i = 0; i < max_search_num; i++)
+  {
+    delete path_node_pool_[i];
+  }
+}
+
+void Astar::init(ros::NodeHandle& nh)
+{
+  // 2d参数
+  nh.param("global_planner/is_2D", is_2D, 0);  // 1代表2D平面规划及搜索,0代表3D
+  nh.param("global_planner/2D_fly_height", fly_height, 1.5);  // 2D规划时,定高高度
+  // 规划搜索相关参数
+  nh.param("astar/lambda_heu", lambda_heu_, 2.0);  // 加速引导参数
+  nh.param("astar/allocate_num", max_search_num, 100000); //最大搜索节点数
+  // 地图参数
+  nh.param("map/resolution", resolution_, 0.2);  // 地图分辨率
+
+  tie_breaker_ = 1.0 + 1.0 / max_search_num;
+
+  this->inv_resolution_ = 1.0 / resolution_;
+
+  has_global_point = false;
+  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))
+  {
+    pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Astar can't find path: goal point is occupied.");
+    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 == 1)
+          {
+            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);
+
+          // 如果扩展的当前节点为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)
+            {
+                pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Astar can't find path: reach the max_search_num.\n");
+                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;
+            }
+          }
+        }
+      }
+    }       
+  
+  }
+
+  //无可通行路径
+  pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "max_search_num: open set empty.");
+  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/避障/A_star.h

@@ -0,0 +1,191 @@
+#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 <boost/functional/hash.hpp>
+// #include <map>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Path.h>
+
+#include "occupy_map.h"
+#include "tools.h"
+#include "message_utils.h"
+
+#define NODE_NAME "Swarm_Planner [Astar]"
+
+
+namespace Swarm_Planning
+{
+
+#define IN_CLOSE_SET 'a'
+#define IN_OPEN_SET 'b'
+#define NOT_EXPAND 'c'
+#define inf 1 >> 30
+
+extern ros::Publisher message_pub;
+
+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_;
+        // 最大搜索次数
+        int max_search_num;
+        // tie breaker
+        double tie_breaker_;
+        int is_2D;
+        double fly_height;
+
+        /* ---------- record data ---------- */
+        // 目标点
+        Eigen::Vector3d goal_pos;
+
+        // 地图相关
+        std::vector<int> occupancy_buffer_;  
+        double resolution_, inv_resolution_;
+        Eigen::Vector3d origin_, map_size_3d_;
+        bool has_global_point;
+
+        // 辅助函数
+        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/避障/occupy_map.cpp

@@ -0,0 +1,280 @@
+#include <occupy_map.h>
+
+namespace Global_Planning
+{
+// 初始化函数
+void Occupy_map::init(ros::NodeHandle& nh)
+{
+    // TRUE代表2D平面规划及搜索,FALSE代表3D 
+    nh.param("global_planner/is_2D", is_2D, true); 
+    // 2D规划时,定高高度
+    nh.param("global_planner/fly_height_2D", fly_height_2D, 1.0);
+    // 地图原点
+    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.0);
+    // 地图实际尺寸，单位：米
+    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), 5.0);
+    // 地图分辨率，单位：米
+    nh.param("map/resolution", resolution_,  0.2);
+    // 地图膨胀距离，单位：米
+    nh.param("map/inflate", inflate_,  0.3);
+
+    // 发布 地图rviz显示
+    global_pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/prometheus/planning/global_pcl",  10); 
+    // 发布膨胀后的点云
+    inflate_pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/prometheus/planning/global_inflate_pcl", 1);
+    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));
+    fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0);
+
+    min_range_ = origin_;
+    max_range_ = origin_ + map_size_3d_;   
+
+    // 对于二维重新限制点云高度
+    if(is_2D == true)
+    {
+        min_range_(2) = fly_height_2D - resolution_;
+        max_range_(2) = fly_height_2D + resolution_;
+    }
+}
+
+// 地图更新函数 - 输入：全局点云
+void Occupy_map::map_update_gpcl(const sensor_msgs::PointCloud2ConstPtr & global_point)
+{
+    has_global_point = true;
+    global_env_ = global_point;
+}
+
+// 地图更新函数 - 输入：局部点云
+void Occupy_map::map_update_lpcl(const sensor_msgs::PointCloud2ConstPtr & local_point, const nav_msgs::Odometry & odom)
+{
+    has_global_point = true;// 局部点云转为全局点云
+}
+
+// 地图更新函数 - 输入：laser
+void Occupy_map::map_update_laser(const sensor_msgs::LaserScanConstPtr & local_point, const nav_msgs::Odometry & odom)
+{
+    has_global_point = true;//转为全局点云
+}
+
+// 当global_planning节点接收到点云消息更新时，进行设置点云指针并膨胀
+// Astar规划路径时，采用膨胀点云
+void Occupy_map::inflate_point_cloud(void)
+{
+    if(!has_global_point)
+    { 
+        pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Occupy_map [inflate point cloud]: don't have global point, can't inflate!\n");
+        return;
+    }
+    // 发布未膨胀点云
+    global_pcl_pub.publish(*global_env_);
+    //开始时间
+    ros::Time time_start = ros::Time::now();
+
+    // 转化为PCL的格式进行处理
+    pcl::PointCloud<pcl::PointXYZ> latest_global_cloud_;
+    pcl::fromROSMsg(*global_env_, latest_global_cloud_);
+    //printf("time 1 take %f [s].\n",   (ros::Time::now()-time_start).toSec());
+    if ((int)latest_global_cloud_.points.size() == 0)  
+    {return;}
+
+    pcl::PointCloud<pcl::PointXYZ> cloud_inflate_vis_;
+    cloud_inflate_vis_.clear();
+
+    // 膨胀格子数 = 膨胀距离/分辨率
+    // ceil返回大于或者等于指定表达式的最小整数
+    const int ifn = ceil(inflate_ * inv_resolution_);
+
+    pcl::PointXYZ pt_inf;
+    Eigen::Vector3d p3d, p3d_inf;
+
+    // 遍历全局点云中的所有点
+    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;
+        }
+        // 根据膨胀距离，膨胀该点
+        for (int x = -ifn; x <= ifn; ++x)
+            for (int y = -ifn; y <= ifn; ++y)
+                for (int z = -ifn; z <= ifn; ++z) 
+                {
+                    // z 轴不膨胀
+                    p3d_inf(0) = pt_inf.x = p3d(0) + x * resolution_;
+                    p3d_inf(1) = pt_inf.y = p3d(1) + y * resolution_;
+                    p3d_inf(2) = pt_inf.z = p3d(2) + 0.5 * z * resolution_;
+
+                    // 若膨胀的点不在地图内（膨胀时只考虑地图范围内的点）
+                    if(!isInMap(p3d_inf))
+                    {
+                        continue;
+                    }
+
+                    cloud_inflate_vis_.push_back(pt_inf);
+
+                    // 设置膨胀后的点被占据
+                    this->setOccupancy(p3d_inf, 1);
+                }
+    }
+
+    cloud_inflate_vis_.header.frame_id = "world";
+
+    // 转化为ros msg发布
+    sensor_msgs::PointCloud2 map_inflate_vis;
+    pcl::toROSMsg(cloud_inflate_vis_, map_inflate_vis);
+
+    inflate_pcl_pub.publish(map_inflate_vis);
+
+    static int exec_num=0;
+    exec_num++;
+
+    // 根据循环时间计算数值
+    if(exec_num == 20)
+    {
+        printf("inflate global point take %f [s].\n",   (ros::Time::now()-time_start).toSec());
+        exec_num=0;
+    }  
+
+}
+
+void Occupy_map::setOccupancy(Eigen::Vector3d pos, int occ) 
+{
+    if (occ != 1 && occ != 0) 
+    {
+        pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "occ value error!\n");
+        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;
+}
+
+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))
+    {
+        // 当前位置点不在地图内
+        pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "[check_safety]: the odom point is not in map\n");
+        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)){
+                    // printf("[check_safety]: current odom is near the boundary of the map\n");
+                    // pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "[check_safety]: current odom is near the boundary of the map\n");
+                    return 0;
+                }
+                if(getOccupancy(id_occ)){
+                    // printf("[check_safety]: current state is dagerous, the pos [%d, %d, %d], is occupied\n", ix, iy, iz);
+                    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)];
+}
+
+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/避障/occupy_map.h

@@ -0,0 +1,100 @@
+#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 "tools.h"
+#include "message_utils.h"
+
+#define NODE_NAME "Swarm_Planner [map]"
+
+namespace Swarm_Planning
+{
+
+extern ros::Publisher message_pub;
+
+class Occupy_map
+{
+    public:
+        Occupy_map(){}
+
+        // 全局点云指针
+        sensor_msgs::PointCloud2ConstPtr global_env_;
+        // 地图是否占据容器， 从编程角度来讲，这就是地图变为单一序列化后的索引
+        std::vector<int> occupancy_buffer_;  // 0 is free, 1 is occupied
+        string uav_name;
+        // 地图分辨率
+        double resolution_, inv_resolution_;
+        // 膨胀参数
+        double inflate_;
+        //是否2D规划
+        bool is_2D;
+        double fly_height_2D;
+        bool debug_mode;
+        // 地图原点,地图尺寸
+        Eigen::Vector3d origin_, map_size_3d_, min_range_, max_range_;
+        // 占据图尺寸 = 地图尺寸 / 分辨率
+        Eigen::Vector3i grid_size_;
+
+        bool has_global_point;
+
+        pcl::PointXYZ nei_pos1;
+        pcl::PointXYZ nei_pos2;
+           
+        // 显示相关
+        void show_gpcl_marker(visualization_msgs::Marker &m, int id, Eigen::Vector4d color);
+
+        // 发布点云用于rviz显示
+        ros::Publisher global_pcl_pub, inflate_pcl_pub;
+
+        //初始化
+        void init(ros::NodeHandle& nh);
+        // 地图更新函数 - 输入：全局点云
+        void map_update_gpcl(const sensor_msgs::PointCloud2ConstPtr & global_point);
+        // 地图更新函数 - 输入：局部点云
+        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 setNeiPos(Eigen::Vector3d pos,  int uav_id);
+        // 地图膨胀
+        void inflate_point_cloud(void);
+        // 判断当前点是否在地图内
+        bool isInMap(Eigen::Vector3d pos);
+        // 设置占据
+        void setOccupancy(Eigen::Vector3d pos, int occ);
+        // 由位置计算索引
+        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);
+        // 检查安全
+        bool check_safety(Eigen::Vector3d& pos, double check_distance/*, Eigen::Vector3d& map_point*/);
+        // 定义该类的指针
+        typedef std::shared_ptr<Occupy_map> Ptr;
+};
+
+}
+
+
+
+#endif