p5akopqw4
/
senjing
1
0
Fork 0
Code Issues Pull Requests 2 Projects Releases Wiki Activity

避障及Qt图像简单处理“

Browse Source
xulingxin_branch
7799 2 years ago
parent fb107a2cbc
commit dba2ffa1a7
4 changed files with 0 additions and 729 deletions
Show Stats Download Patch File Download Diff File

375
src/79xlx/A_star.cpp
Unescape View File

@ -1,375 +0,0 @@
#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表示释放堆内存
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;
}
}

19
src/79xlx/Qt_0.cpp
Unescape View File

@ -1,19 +0,0 @@
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();

43
src/79xlx/Qt_1.cpp
Unescape View File

@ -1,43 +0,0 @@
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;

292
src/79xlx/occupy_map.cpp
Unescape View File

@ -1,292 +0,0 @@
#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规划路径时，采用的是此处膨胀后的点云（setOccupancy只在本函数中使用）
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轴膨胀一半呢？ 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)
{
// 膨胀地图效率与地图大小有关（有点久，Astar更新频率是多久呢？ 怎么才能提高膨胀效率呢？）
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)];
}
}
Write Preview
Loading…
Cancel
Save