demo_node.cpp
main函数解读
int main(int argc, char** argv)
{
ros::init(argc, argv, "demo_node");
ros::NodeHandle nh("~");
//每秒接收一次点云数据，并调用rcvPointCloudCallBack回调函数进行处理
_map_sub = nh.subscribe( "map", 1, rcvPointCloudCallBack );
//接收目标点，并进行路径搜索
_pts_sub = nh.subscribe( "waypoints", 1, rcvWaypointsCallback );
//创建三个发布数据
//发布网格地图可视化数据。
_grid_map_vis_pub = nh.advertise<sensor_msgs::PointCloud2>("grid_map_vis", 1);
//发布寻路路径可视化数据
_grid_path_vis_pub = nh.advertise<visualization_msgs::Marker>("grid_path_vis", 1);
//发布已访问节点的可视化数据
_visited_nodes_vis_pub = nh.advertise<visualization_msgs::Marker>("visited_nodes_vis",1);
//配置参数
//从接收的点云数据中读取地图相关参数
nh.param("map/cloud_margin", _cloud_margin, 0.0);
//分辨率
nh.param("map/resolution", _resolution, 0.2);
//尺寸
nh.param("map/x_size", _x_size, 50.0);
nh.param("map/y_size", _y_size, 50.0);
nh.param("map/z_size", _z_size, 5.0 );
//起始点坐标
nh.param("planning/start_x", _start_pt(0), 0.0);
nh.param("planning/start_y", _start_pt(1), 0.0);
nh.param("planning/start_z", _start_pt(2), 0.0);
//地图最小点，定义为正方体左下角
_map_lower << - _x_size/2.0, - _y_size/2.0, 0.0;
//地图最大点，定义为正方体右上角
_map_upper << + _x_size/2.0, + _y_size/2.0, _z_size;
//分辨率的倒数，_inv_resolution的值可以用于将像素坐标转换为世界坐标
//例如，如果 _resolution 的值为 10，则 _inv_resolution 的值为 0.1
//世界坐标为 (x坐标 * _inv_resolution, y坐标 * _inv_resolution)
//如果有点的像素坐标为 (100, 200)，则世界坐标或栅格地图id(10，20)
_inv_resolution = 1.0 / _resolution;
//像素坐标最大值转换为世界坐标。
_max_x_id = (int)(_x_size * _inv_resolution);
_max_y_id = (int)(_y_size * _inv_resolution);
_max_z_id = (int)(_z_size * _inv_resolution);
//初始化地图
_astar_path_finder = new AstarPathFinder();
_astar_path_finder -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id);
_jps_path_finder = new JPSPathFinder();
_jps_path_finder -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id);
ros::Rate rate(100);
bool status = ros::ok();
while(status)
{
ros::spinOnce();
//检查 ROS 系统是否应该继续运行
status = ros::ok();
rate.sleep();
}
delete _astar_path_finder;
delete _jps_path_finder;
return 0;
}
简要过程：程序接收地图基本信息后进行地图初始化，利用ROS subscriber回调函数进行障碍物设置（_map_sub）和路径查找（_pts_sub）。
rcvPointCloudCallBack填充障碍物
//点云数据处理
void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map)
{
//检查地图状态：首先检查变量_has_map的值，如果地图已经建立完毕（_has_map为true），则直接返回，不再处理新的点云数据。
if(_has_map ) return;
// cloud存储点云数据，用于建立网格地图的障碍物信息
pcl::PointCloud<pcl::PointXYZ> cloud;
//cloud_vis存储经过处理的点云数据，用于可视化网格地图中的障碍物
pcl::PointCloud<pcl::PointXYZ> cloud_vis;
sensor_msgs::PointCloud2 map_vis;
//使用pcl::fromROSMsg将ROS点云消息pointcloud_map转换为PCL点云cloud
pcl::fromROSMsg(pointcloud_map, cloud);
//检查cloud.points.size()，如果点云为空，则直接返回，不进行后续处理。
if( (int)cloud.points.size() == 0 ) return;
//遍历点云数据：使用循环遍历点云中的每个点pt
pcl::PointXYZ pt;
for (int idx = 0; idx < (int)cloud.points.size(); idx++)
{
pt = cloud.points[idx];
//将点pt设置为障碍物，调用A*和JPS路径规划算法的setObs函数将其添加到对应的网格地图中，标记为不可通行区域。
_astar_path_finder->setObs(pt.x, pt.y, pt.z);
_jps_path_finder->setObs(pt.x, pt.y, pt.z);
//将原始点云中的点在网格中进行四舍五入，保证落在网格整数坐标上
//coordRounding 函数，该函数接受一个三维坐标 Vector3d 作为输入，并返回一个四舍五入后的坐标 Vector3d
Vector3d cor_round = _astar_path_finder->coordRounding(Vector3d(pt.x, pt.y, pt.z));
pt.x = cor_round(0);
pt.y = cor_round(1);
pt.z = cor_round(2);
//将元素添加到向量末尾
cloud_vis.points.push_back(pt);
}
//更新点云信息，设置cloud_vis的宽度、高度和深度
cloud_vis.width = cloud_vis.points.size();
cloud_vis.height = 1;
cloud_vis.is_dense = true;
//转换点云格式，将处理后的点云cloud_vis转换为ROS点云消息map_vis
pcl::toROSMsg(cloud_vis, map_vis);
//设置map_vis的头文件frame_id为"/world"
map_vis.header.frame_id = "/world";
_grid_map_vis_pub.publish(map_vis);
_has_map = true;
}
rcvWaypointsCallback回调函数发现路径
void rcvWaypointsCallback(const nav_msgs::Path & wp)
{
if( wp.poses[0].pose.position.z < 0.0 || _has_map == false )
return;
Vector3d target_pt;
//目标点坐标变换
target_pt << wp.poses[0].pose.position.x,
wp.poses[0].pose.position.y,
wp.poses[0].pose.position.z;
ROS_INFO("[node] receive the planning target");
//路径搜索
pathFinding(_start_pt, target_pt);
}
pathFinding函数
void pathFinding(const Vector3d start_pt, const Vector3d target_pt)
{
//调用A*算法，找到路径
_astar_path_finder->AstarGraphSearch(start_pt, target_pt);
//从终点开始回溯获得路径点集合
auto grid_path = _astar_path_finder->getPath();
auto visited_nodes = _astar_path_finder->getVisitedNodes();
//路径可视化
visGridPath (grid_path, false);
visVisitedNode(visited_nodes);
//Reset map for next call
_astar_path_finder->resetUsedGrids();
//_use_jps = 0 -> Do not use JPS
//_use_jps = 1 -> Use JPS
//是否使用JPS算法
#define _use_jps 1
#if _use_jps
{
//Call JPS to search for a path
_jps_path_finder -> JPSGraphSearch(start_pt, target_pt);
//Retrieve the path
auto grid_path = _jps_path_finder->getPath();
auto visited_nodes = _jps_path_finder->getVisitedNodes();
//Visualize the result
visGridPath (grid_path, _use_jps);
visVisitedNode(visited_nodes);
//Reset map for next call
_jps_path_finder->resetUsedGrids();
}
#endif
}
Astar_searcher.cpp
地图初始化
void AstarPathFinder::initGridMap(double _resolution, Vector3d global_xyz_l, Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id)
{
//地图最小边界坐标
gl_xl = global_xyz_l(0);
gl_yl = global_xyz_l(1);
gl_zl = global_xyz_l(2);
地图最大边界坐标
gl_xu = global_xyz_u(0);
gl_yu = global_xyz_u(1);
gl_zu = global_xyz_u(2);
//方向网格单元编号的最大值
GLX_SIZE = max_x_id;
GLY_SIZE = max_y_id;
GLZ_SIZE = max_z_id;
//地图在yz平面的网格单元数
GLYZ_SIZE = GLY_SIZE * GLZ_SIZE;
//地图的总网格单元数
GLXYZ_SIZE = GLX_SIZE * GLYZ_SIZE;
//分辨率
resolution = _resolution;
inv_resolution = 1.0 / _resolution;
//初始化地图数据存储内存
data = new uint8_t[GLXYZ_SIZE];
//data: 这是要被初始化的内存块的指针
//0: 这是要设置的字节值
//GLXYZ_SIZE * sizeof(uint8_t): 这是要被初始化的字节数
memset(data, 0, GLXYZ_SIZE * sizeof(uint8_t));
//地图使用三重指针，存储地图信息
//分配一个长度为GLX_SIZE的指针数组
GridNodeMap = new GridNodePtr ** [GLX_SIZE];
for(int i = 0; i < GLX_SIZE; i++){
GridNodeMap[i] = new GridNodePtr * [GLY_SIZE];
for(int j = 0; j < GLY_SIZE; j++){
GridNodeMap[i][j] = new GridNodePtr [GLZ_SIZE];
for( int k = 0; k < GLZ_SIZE;k++){
//Vector3i是Eigen库中一个三维整型向量类型，可以存储三个整数值，即栅格id
Vector3i tmpIdx(i,j,k);
//gridIndex2coord函数的功能是将网格单元索引转换为实际坐标
//pos是一个Vector3d类型的变量，存储了该网格单元的真实世界坐标
Vector3d pos = gridIndex2coord(tmpIdx);
GridNodeMap[i][j][k] = new GridNode(tmpIdx, pos);
}
}
}
}
地图信息存储类型
typedef GridNode* GridNodePtr;
struct GridNode
{
//表示网格单元的状态
int id; // 1--> open set, -1 --> closed set
//存储网格单元的真实世界坐标
Eigen::Vector3d coord;
//存储当前网格单元扩展方向的索引。
Eigen::Vector3i dir; // direction of expanding
//存储网格单元在三维地图中的索引
Eigen::Vector3i index;
//代价函数
double gScore, fScore;
//父节点
GridNodePtr cameFrom;
std::multimap<double, GridNodePtr>::iterator nodeMapIt;
GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord){
id = 0;
index = _index;
coord = _coord;
dir = Eigen::Vector3i::Zero();
gScore = inf;
fScore = inf;
cameFrom = NULL;
}
GridNode(){};
~GridNode(){};
};
障碍物设置函数setObs
void AstarPathFinder::setObs(const double coord_x, const double coord_y, const double coord_z)
{
//检查传入的障碍物坐标 (coord_x, coord_y, coord_z) 是否位于地图范围内
//地图最小边界 (gl_xl, gl_yl, gl_zl) 或大于地图最大边界 (gl_xu, gl_yu, gl_zu)
if( coord_x < gl_xl || coord_y < gl_yl || coord_z < gl_zl ||
coord_x >= gl_xu || coord_y >= gl_yu || coord_z >= gl_zu )
return;
int idx_x = static_cast<int>( (coord_x - gl_xl) * inv_resolution);
int idx_y = static_cast<int>( (coord_y - gl_yl) * inv_resolution);
int idx_z = static_cast<int>( (coord_z - gl_zl) * inv_resolution);
//内存中地图存储方式为切分存储，从z轴开始，再看y轴，最后看x轴
data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] = 1;
}
网格索引与世界坐标相互转换
Vector3d AstarPathFinder::gridIndex2coord(const Vector3i & index)
{
Vector3d pt;
//索引的值加上0.5，再将网格单元格索引转换回坐标，就会将坐标向下取整到网格单元格的中心位置
pt(0) = ((double)index(0) + 0.5) * resolution + gl_xl;
pt(1) = ((double)index(1) + 0.5) * resolution + gl_yl;
pt(2) = ((double)index(2) + 0.5) * resolution + gl_zl;
return pt;
}
Vector3i AstarPathFinder::coord2gridIndex(const Vector3d & pt)
{
Vector3i idx;
//将地图内的真实坐标pt转换为网格单元格索引
idx << min( max( int( (pt(0) - gl_xl) * inv_resolution), 0), GLX_SIZE - 1),
min( max( int( (pt(1) - gl_yl) * inv_resolution), 0), GLY_SIZE - 1),
min( max( int( (pt(2) - gl_zl) * inv_resolution), 0), GLZ_SIZE - 1);
return idx;
}
Eigen::Vector3d AstarPathFinder::coordRounding(const Eigen::Vector3d & coord)
{
//先将坐标转换为网格单元格索引。然后，再将将网格单元格索引转换成坐标，达到四舍五入的效果。
return gridIndex2coord(coord2gridIndex(coord));
}
障碍物检测
//是否在地图边界内，且是障碍物
inline bool AstarPathFinder::isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const
{
return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE &&
(data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] == 1));
}
//是否在地图边界内，且不是障碍物
inline bool AstarPathFinder::isFree(const int & idx_x, const int & idx_y, const int & idx_z) const
{
return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE &&
(data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] < 1));
}
//函数重载
inline bool AstarPathFinder::isOccupied(const Eigen::Vector3i & index) const
{
return isOccupied(index(0), index(1), index(2));
}
inline bool AstarPathFinder::isFree(const Eigen::Vector3i & index) const
{
return isFree(index(0), index(1), index(2));
}
算法主体函数
void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt)
{
ros::Time time_1 = ros::Time::now();
//起始点和目标点的索引
Vector3i start_idx = coord2gridIndex(start_pt);
Vector3i end_idx = coord2gridIndex(end_pt);
goalIdx = end_idx;
//起始点和目标点的坐标
start_pt = gridIndex2coord(start_idx);
end_pt = gridIndex2coord(end_idx);
//初始化GridNode结构体的指针，分别代表起始节点和目标节点
GridNodePtr startPtr = new GridNode(start_idx, start_pt);
GridNodePtr endPtr = new GridNode(end_idx, end_pt);
//openSet是STL库中通过multimap实现的open_list
openSet.clear();
// currentPtr represents the node with lowest f(n) in the open_list
GridNodePtr currentPtr = NULL;
GridNodePtr neighborPtr = NULL;
//将起始节点放入open list
startPtr -> gScore = 0; //g(n)
startPtr -> fScore = getHeu(startPtr,endPtr); //f(n)
startPtr -> id = 1; // 1--> open set, -1 --> closed set
startPtr -> coord = start_pt; //起始点坐标
openSet.insert( make_pair(startPtr -> fScore, startPtr) );
/*
*
STEP 2 : some else preparatory works which should be done before while loop
please write your code below
*
*
*/
vector<GridNodePtr> neighborPtrSets;
vector<double> edgeCostSets;
//初始化地图
//resetUsedGrids();
//将地图初始节点指向开始节点
GridNodeMap[start_idx[0]][start_idx[1]][start_idx[2]] = startPtr;
// this is the main loop
while ( !openSet.empty() ){
/*
*
*
step 3: Remove the node with lowest cost function from open set to closed set
please write your code below
IMPORTANT NOTE!!!
This part you should use the C++ STL: multimap, more details can be find in Homework description
*
*
*/
//弹出f(n)最小的节点
auto iter = openSet.begin();
currentPtr = iter->second;
openSet.erase(iter->first);
Vector3i idx = currentPtr -> index;
GridNodeMap[idx(0)][idx(1)][idx(2)] -> id = -1;
// 检查是否到达目标
if( currentPtr->index == goalIdx ){
ros::Time time_2 = ros::Time::now();
terminatePtr = currentPtr;
//terminatePtr = GridNodeMap[idx(0)][idx(1)][idx(2)];
ROS_WARN("[A*]{sucess} Time in A* is %f ms, path cost if %f m", (time_2 - time_1).toSec() * 1000.0, currentPtr->gScore * resolution );
return;
}
//拓展节点函数
AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets); //STEP 4: finish AstarPathFinder::AstarGetSucc yourself
/*
*
*
STEP 5: For all unexpanded neigbors "m" of node "n", please finish this for loop
please write your code below
*
*/
for(int i = 0; i < (int)neighborPtrSets.size(); i++){
/*
*
*
Judge if the neigbors have been expanded
please write your code below
IMPORTANT NOTE!!!
neighborPtrSets[i]->id = -1 : expanded, equal to this node is in close set
neighborPtrSets[i]->id = 1 : unexpanded, equal to this node is in open set
*
*/
//获取一个另据节点
neighborPtr = neighborPtrSets[i];
Vector3i neighbor_idx = neighborPtr -> index;
double cross = getCross(startPtr, neighborPtr, endPtr);
//如果该节点未被拓展
if(neighborPtr -> id == 0){ //discover a new node, which is not in the closed set and open set
/*
*
*
STEP 6: As for a new node, do what you need do ,and then put neighbor in open set and record it
please write your code below
*
*/
// 该节点不是障碍物
if(isFree(neighbor_idx)){
//节点更新并塞入open list
neighborPtr -> id = 1;
neighborPtr -> cameFrom = GridNodeMap[currentPtr -> index(0)][currentPtr -> index(1)][currentPtr -> index(2)];
neighborPtr -> gScore = edgeCostSets[i] + currentPtr -> gScore;
neighborPtr -> fScore = neighborPtr -> gScore + getHeu(neighborPtr, endPtr);
neighborPtr -> fScore = neighborPtr -> fScore * (1 + cross);
openSet.insert( make_pair(neighborPtr->fScore, neighborPtr) );
}else{
// 放入close list作为障碍物
neighborPtr -> id = -1;
}
}
//如果节点另一条路径累计成本更小
else if(neighborPtr -> id == 1){ //this node is in open set and need to judge if it needs to update, the "0" should be deleted when you are coding
/*
*
*
STEP 7: As for a node in open set, update it , maintain the openset ,and then put neighbor in open set and record it
please write your code below
*
*/
//覆盖节点信息
if(edgeCostSets[i] + currentPtr -> gScore < neighborPtr -> gScore){
neighborPtr -> cameFrom = GridNodeMap[currentPtr -> index(0)][currentPtr -> index(1)][currentPtr -> index(2)];
neighborPtr -> gScore = edgeCostSets[i] +currentPtr -> gScore;
neighborPtr -> fScore = neighborPtr -> gScore + getHeu(neighborPtr, endPtr);
}
continue;
}
else{//this node is in closed set
/*
*
please write your code below
*
*/
continue;
}
}
}
//if search fails
ros::Time time_2 = ros::Time::now();
if((time_2 - time_1).toSec() > 0.1)
ROS_WARN("Time consume in Astar path finding is %f", (time_2 - time_1).toSec() );
}
启发式函数
double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2)
{
double node1_x = node1 -> coord(0);
double node1_y = node1 -> coord(1);
double node1_z = node1 -> coord(2);
double node2_x = node2 -> coord(0);
double node2_y = node2 -> coord(1);
double node2_z = node2 -> coord(2);
double dx = abs(node2_x - node1_x);
double dy = abs(node2_y - node1_y);
double dz = abs(node2_z - node1_z);
// 三种启发式函数
// Euclidean
double distance = sqrt(dx * dx + dy * dy + dz * dz);
/// Manhattan
// double distance = dx + dy + dz;
// Diagonal
// double distance = (dx + dy + dz) + (sqrt(3) - 3) * min(min(dx, dy), dz);
return distance;
}
邻居节点搜索
inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> & neighborPtrSets, vector<double> & edgeCostSets)
{
neighborPtrSets.clear();
edgeCostSets.clear();
GridNodePtr neighbor = NULL;
Vector3i neighbor_idx;
for (int i = 1; i >= -1; i--){
for (int j = 1; j >= -1; j--){
for (int k = 1; k >= -1; k--){
if (i != j || i != k || i != 0){
neighbor_idx << currentPtr -> index(0) + i,
currentPtr -> index(1) + j,
currentPtr -> index(2) + k;
if(neighbor_idx(0) >= 0 && neighbor_idx(0) < GLX_SIZE &&
neighbor_idx(1) >= 0 && neighbor_idx(1) < GLY_SIZE &&
neighbor_idx(2) >= 0 && neighbor_idx(2) < GLZ_SIZE)
{
neighbor = GridNodeMap[neighbor_idx(0)][neighbor_idx(1)][neighbor_idx(2)];
neighbor -> dir = Vector3i(i,j,k);
neighborPtrSets.push_back(neighbor);
edgeCostSets.push_back(getHeu(currentPtr, neighbor));
}else continue;
}
}
}
}
}
节点回溯
vector<Vector3d> AstarPathFinder::getPath()
{
vector<Vector3d> path;
vector<GridNodePtr> gridPath;
GridNodePtr gridNode = terminatePtr -> cameFrom;
gridPath.push_back(terminatePtr);
while ( gridNode != NULL ){
gridPath.push_back(gridNode);
gridNode = gridNode->cameFrom;
}
for (auto ptr: gridPath)
path.push_back(ptr->coord);
reverse(path.begin(),path.end());
return path;
}
Tie Breaker函数
double AstarPathFinder::getCross(GridNodePtr startNode, GridNodePtr currentNode, GridNodePtr endNode)
{
double node_x = currentNode -> coord(0);
double node_y = currentNode -> coord(1);
double node_z = currentNode -> coord(2);
double start_x = startNode -> coord(0);
double start_y = startNode -> coord(1);
double start_z = startNode -> coord(2);
double end_x = endNode -> coord(0);
double end_y = endNode -> coord(1);
double end_z = endNode -> coord(2);
double dx1 = abs(node_x - end_x);
double dy1 = abs(node_y - end_y);
double dz1 = abs(node_z - end_z);
double dx2 = abs(start_x - node_x);
double dy2 = abs(start_y - node_y);
double dz2 = abs(start_z - node_z);
double C_x = abs(dy1*dz2 - dy2*dz1);
double C_y = abs(dz1*dx2 - dz2*dx1);
double C_z = abs(dx1*dy2 - dx2*dy1);
double cross = sqrt(C_x * C_x + C_y * C_y + C_z * C_z);
return cross;
}
使用了叉积公式：
代码运行结果
2854
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
