基于ROS的A*算法代码学习
算法小白一枚,本文是在学习路径规划算法中的一篇学习记录,由于对c++掌握粗浅,所以边查边学,将整个代码进行了详细的梳理,当然里面也还有些问题受限于相关知识不能解决,希望该文章能解决您的一些问题,同时也希望本人的遗留问题能得到您的解答。
(代码注释中英夹杂,英文注释是原来作者的注释,中文注释是本人后来添加的)
过程演示
1.编译功能包.
2.Ctrl+Shift+t新建窗口打开roscore.
3.Ctrl+Shift+t新建窗口打开rviz.
记得要source devel/setup.sh,告诉这个窗口你功能包的位置,否则3d Nav Goal插件会找不到.
4.在rviz中打开demo.rviz(路径src/grid_path_searcher/launch/rviz_config).
5.Ctrl+Shift+t新建窗口加载地图.
记得source.
6.A*路径搜索
下面是结果
这是终端里的信息
ROS包
├── grid_path_searcher--------------------------------------------------------------路径搜索包名称
│ ├── CMakeLists.txt
│ ├── include
│ │ ├── Astar_searcher.h------------------------------------------------------------A代码头文件
│ │ ├── backward.hpp
│ │ ├── JPS_searcher.h
│ │ ├── JPS_utils.h
│ │ └── node.h-------------------------------------------------------------------------A代码头文件
│ ├── launch
│ │ ├── demo.launch-----------------------------------------------------------------加载地图的launch文件
│ │ └── rviz_config
│ │ ├── demo.rviz---------------------------------------------------------------------rviz的环境文件
│ │ └── jps_demo.rviz
│ ├── package.xml
│ ├── README.md
│ └── src
│ ├── Astar_searcher.cpp-----------------------------------------------------------A*算法代码文件
│ ├── CMakeLists.txt
│ ├── demo_node.cpp---------------------------------------------------------------主函数文件
│ ├── graph_searcher.cpp
│ ├── random_complex_generator.cpp-----------------------------------------地图生成障碍物
│ └── read_only
│ ├── JPS_searcher.cpp
│ └── JPS_utils.cpp
│ └── waypoint_generator
│ ├── CMakeLists.txt
│ ├── package.xml
│ └── src
│ ├── sample_waypoints.h
│ └── waypoint_generator.cpp-----------------------------------------------------发布目标点信息
以上所有带注释的就是ROS下的A*算法代码实现相关文件.
node.h
#ifndef _NODE_H_
#define _NODE_H_
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include "backward.hpp"
#define inf 1>>20
struct GridNode;
typedef GridNode* GridNodePtr;
struct GridNode
{
// 1--> open set, -1 --> closed set, 0 --> 未被访问过
int id;
// 实际坐标
Eigen::Vector3d coord;
// direction of expanding
Eigen::Vector3i dir;
// 栅格地图坐标
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感觉在代码中没用到,有什么用呢???
dir = Eigen::Vector3i::Zero();
gScore = inf;
fScore = inf;
cameFrom = NULL;
}
GridNode(){};
~GridNode(){};
};
#endif
Astar_searcher.h
#ifndef _ASTART_SEARCHER_H
#define _ASTART_SEARCHER_H
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include "backward.hpp"
#include "node.h"
class AstarPathFinder
{
private:
protected:
uint8_t * data;
// 地图指针
GridNodePtr *** GridNodeMap;
// 网格地图坐标形式的目标点
Eigen::Vector3i goalIdx;
// 定义的地图的长宽高
int GLX_SIZE, GLY_SIZE, GLZ_SIZE;
int GLXYZ_SIZE, GLYZ_SIZE;
// resolution表示栅格地图精度,inv_resolution=1/resolution
double resolution, inv_resolution;
// gl_x表示地图边界,l->low(下边界),u->up(上边界)
double gl_xl, gl_yl, gl_zl;
double gl_xu, gl_yu, gl_zu;
// 终点指针
GridNodePtr terminatePtr;
// openSet容器,用于存放规划中确定下来的路径点
std::multimap<double, GridNodePtr> openSet;
// 返回两点之间的距离(曼哈顿距离/欧式距离/对角线距离)
double getHeu(GridNodePtr node1, GridNodePtr node2);
// 返回当前指针指向坐标的所有相邻节点,以及当前节点与相邻节点的cost
void AstarGetSucc(GridNodePtr currentPtr, std::vector<GridNodePtr> & neighborPtrSets, std::vector<double> & edgeCostSets);
// 判断节点是否是障碍物
bool isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const;
bool isOccupied(const Eigen::Vector3i & index) const;
// 判断节点是不是为空
bool isFree(const int & idx_x, const int & idx_y, const int & idx_z) const;
bool isFree(const Eigen::Vector3i & index) const;
//栅格地图坐标转实际坐标
Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index);
//实际坐标转栅格地图坐标
Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt);
public:
AstarPathFinder(){};
~AstarPathFinder(){};
// A*路径搜索
void AstarGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
// 将所有点的属性设置为未访问过的状态下
void resetGrid(GridNodePtr ptr);
// 通过循环遍历重置每一个点
void resetUsedGrids();
// 初始化地图
void initGridMap(double _resolution, Eigen::Vector3d global_xyz_l, Eigen::Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id);
// 设置障碍物
void setObs(const double coord_x, const double coord_y, const double coord_z);
Eigen::Vector3d coordRounding(const Eigen::Vector3d & coord);
// 获得A*搜索到的完整路径
std::vector<Eigen::Vector3d> getPath();
// 获得访问过的所有节点
std::vector<Eigen::Vector3d> getVisitedNodes();
};
#endif
Astar_searcher.cpp
#include "Astar_searcher.h"
using namespace std;
using namespace Eigen;
bool tie_break = false;
// 初始化地图
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_x表示地图边界,l->low(下边界),u->up(上边界)
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;
GLYZ_SIZE = GLY_SIZE * GLZ_SIZE;
GLXYZ_SIZE = GLX_SIZE * GLYZ_SIZE;
resolution = _resolution;
inv_resolution = 1.0 / _resolution;
// void *memset(void *s, int ch, size_t n);将s中当前位置后面的n个字节(typedef unsigned int size_t)用ch替换并返回 s 。
data = new uint8_t[GLXYZ_SIZE];
memset(data, 0, GLXYZ_SIZE * sizeof(uint8_t));
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 tmpIdx(i,j,k);
Vector3d pos = gridIndex2coord(tmpIdx);
// GridNodeMap-三维指针;传入网格地图坐标和实际坐标,初始化每个节点的属性
GridNodeMap[i][j][k] = new GridNode(tmpIdx, pos);
}
}
}
}
// 将所有点的属性设置为未访问过的状态下
void AstarPathFinder::resetGrid(GridNodePtr ptr)
{
ptr->id = 0;
ptr->cameFrom = NULL;
ptr->gScore = inf;
ptr->fScore = inf;
}
// 通过循环遍历重置每一个点
void AstarPathFinder::resetUsedGrids()
{
for(int i=0; i < GLX_SIZE ; i++)
for(int j=0; j < GLY_SIZE ; j++)
for(int k=0; k < GLZ_SIZE ; k++)
resetGrid(GridNodeMap[i][j][k]);
}
// set obstalces into grid map for path planning
void AstarPathFinder::setObs(const double coord_x, const double coord_y, const double coord_z)
{
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);
data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] = 1;
}
// 获得访问过的所有节点
vector<Vector3d> AstarPathFinder::getVisitedNodes()
{
vector<Vector3d> visited_nodes;
for(int i = 0; i < GLX_SIZE; i++)
for(int j = 0; j < GLY_SIZE; j++)
for(int k = 0; k < GLZ_SIZE; k++)
{
if(GridNodeMap[i][j][k]->id != 0) // visualize all nodes in open and close list
// if(GridNodeMap[i][j][k]->id == -1) // visualize nodes in close list only TODO: careful
visited_nodes.push_back(GridNodeMap[i][j][k]->coord);
}
ROS_WARN("visited_nodes size : %d", visited_nodes.size());
return visited_nodes;
}
// 网格地图坐标转换为实际坐标
Vector3d AstarPathFinder::gridIndex2coord(const Vector3i & index)
{
Vector3d pt;
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;
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是C++关键字，在函数声明或定义中，函数返回类型前加上关键字inline，即可以把函数指定为内联函数。这样可以解决一些频繁调用的函数大量消耗栈空间（栈内存）的问题。
// 用网格地图坐标判断是否是障碍物点(在函数内部调用另一个用实际坐标判断的函数)
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));
}
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));
}
// 获取该点周围的所有节点和周围点的edgeCostSets(edgeCostSets:该点到目标的的距离)
inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> & neighborPtrSets, vector<double> & edgeCostSets)
{
neighborPtrSets.clear(); // Note: the pointers in this set copy pointers to GridNodeMap
edgeCostSets.clear();
/*
*
STEP 4: finish AstarPathFinder::AstarGetSucc yourself
please write your code below
*
*/
// idea index -> coordinate -> edgecost
if(currentPtr == nullptr)
std::cout << "Error: Current pointer is null!" << endl;
Eigen::Vector3i thisNode = currentPtr -> index;
int this_x = thisNode[0];
int this_y = thisNode[1];
int this_z = thisNode[2];
auto this_coord = currentPtr -> coord;
int n_x, n_y, n_z;
double dist;
GridNodePtr temp_ptr = nullptr;
Eigen::Vector3d n_coord;
// 遍历周围点,获取周围点的edgeCostSets
for(int i = -1;i <= 1;++i ){
for(int j = -1;j <= 1;++j ){
for(int k = -1;k <= 1;++k){
if( i == 0 && j == 0 && k == 0)
continue; // to avoid this node
n_x = this_x + i;
n_y = this_y + j;
n_z = this_z + k;
if( (n_x < 0) || (n_x > (GLX_SIZE - 1)) || (n_y < 0) || (n_y > (GLY_SIZE - 1) ) || (n_z < 0) || (n_z > (GLZ_SIZE - 1)))
continue; // to avoid index problem
if(isOccupied(n_x, n_y, n_z))
continue; // to avoid obstacles
// put the pointer into neighborPtrSets
temp_ptr = GridNodeMap[n_x][n_y][n_z];
if(temp_ptr->id == -1)
continue; // todo to check this; why the node can transversing the obstacles
n_coord = temp_ptr->coord;
if(temp_ptr == currentPtr){
std::cout << "Error: temp_ptr == currentPtr)" << std::endl;
}
if( (std::abs(n_coord[0] - this_coord[0]) < 1e-6) and (std::abs(n_coord[1] - this_coord[1]) < 1e-6) and (std::abs(n_coord[2] - this_coord[2]) < 1e-6 )){
std::cout << "Error: Not expanding correctly!" << std::endl;
std::cout << "n_coord:" << n_coord[0] << " "<<n_coord[1]<<" "<<n_coord[2] << std::endl;
std::cout << "this_coord:" << this_coord[0] << " "<<this_coord[1]<<" "<<this_coord[2] << std::endl;
std::cout << "current node index:" << this_x << " "<< this_y<<" "<< this_z << std::endl;
std::cout << "neighbor node index:" << n_x << " "<< n_y<<" "<< n_z << std::endl;
}
dist = std::sqrt( (n_coord[0] - this_coord[0]) * (n_coord[0] - this_coord[0])+
(n_coord[1] - this_coord[1]) * (n_coord[1] - this_coord[1])+
(n_coord[2] - this_coord[2]) * (n_coord[2] - this_coord[2]));
neighborPtrSets.push_back(temp_ptr); // calculate the cost in edgeCostSets: inf means that is not unexpanded
edgeCostSets.push_back(dist); // put the cost inot edgeCostSets
}
}
}
}
// 启发试函数,获得h值
double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2)
{
/*
choose possible heuristic function you want
Manhattan, Euclidean, Diagonal, or 0 (Dijkstra)
Remember tie_breaker learned in lecture, add it here ?
*
*
*
STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function
please write your code below
*
*
*/
double h;
auto node1_coord = node1->coord;
auto node2_coord = node2->coord;
// Heuristics 1: Manhattan
// h = std::abs(node1_coord(0) - node2_coord(0) ) +
// std::abs(node1_coord(1) - node2_coord(1) ) +
// std::abs(node1_coord(2) - node2_coord(2) );
// Heuristics 2: Euclidean
// h = std::sqrt(std::pow((node1_coord(0) - node2_coord(0)), 2 ) +
// std::pow((node1_coord(1) - node2_coord(1)), 2 ) +
// std::pow((node1_coord(2) - node2_coord(2)), 2 ));
// Heuristics 3: Diagnol distance
double dx = std::abs(node1_coord(0) - node2_coord(0) );
double dy = std::abs(node1_coord(1) - node2_coord(1) );
double dz = std::abs(node1_coord(2) - node2_coord(2) );
double min_xyz = std::min({dx, dy, dz});
h = dx + dy + dz + (std::sqrt(3.0) -3) * min_xyz; // idea: diagnol is a short-cut, find out how many short-cuts can be realized
//这个tie_break目前还没搞懂什么意思
if(tie_break){
double p = 1.0 / 25.0;
h *= (1.0 + p);
//std::cout << "Tie Break!" << std::endl;
}
return h;
}
// A*路径搜索
void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt)
{
ros::Time time_1 = ros::Time::now();
//index of start_point and end_point
Vector3i start_idx = coord2gridIndex(start_pt);
Vector3i end_idx = coord2gridIndex(end_pt);
goalIdx = end_idx;
//position of start_point and end_point
start_pt = gridIndex2coord(start_idx);
end_pt = gridIndex2coord(end_idx);
//Initialize the pointers of struct GridNode which represent start node and goal node
GridNodePtr startPtr = new GridNode(start_idx, start_pt);
GridNodePtr endPtr = new GridNode(end_idx, end_pt);
//openSet is the open_list implemented through multimap in STL library
openSet.clear();
//currentPtr represents the node with lowest f(n) in the open_list
GridNodePtr currentPtr = NULL;
GridNodePtr neighborPtr = NULL;
//put start node in open set
startPtr -> gScore = 0;
startPtr -> fScore = getHeu(startPtr,endPtr);
//STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function
startPtr -> id = 1;
startPtr -> coord = start_pt;
// make_pair的用法:无需写出型别,就可以生成一个pair对象;比如std::make_pair(42, '@'),而不必费力写成：std::pair<int, char>(42, '@')
//todo Note: modified, insert the pointer GridNodeMap[i][j][k] to the start node in grid map
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
*
*
*/
// three dimension pointer GridNodeMap[i][j][k] is pointed to a struct GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord);
// assign g(xs) = 0, g(n) = inf (already done in initialzation of struct)
// mark start point as visited(expanded) (id 0: no operation, id: 1 in OPEN, id -1: in CLOSE )
// 这个到底需不需要???测试后发现不需要也可以实现功能
GridNodeMap[start_idx[0]][start_idx[1]][start_idx[2]] -> id = 1;
vector<GridNodePtr> neighborPtrSets;
vector<double> edgeCostSets;
Eigen::Vector3i current_idx; // record the current index
// 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
*
*
*/
// openset:待访问节点容器;closed set:访问过节点容器
int x = openSet.begin()->second->index(0);
int y = openSet.begin()->second->index(1);
int z = openSet.begin()->second->index(2);
openSet.erase(openSet.begin());
currentPtr = GridNodeMap[x][y][z];
// 如果节点被访问过;则返回
if(currentPtr->id == -1)
continue;
// 标记id为-1,表示节点已被扩展
currentPtr->id = -1;
// currentPtr = openSet.begin() -> second; // first T1, second T2
// openSet.erase(openSet.begin()); // remove the node with minimal f value
// current_idx = currentPtr->index;
// GridNodeMap[current_idx[0]][current_idx[1]][current_idx[2]] -> id = -1;// update the id in grid node map
// if the current node is the goal
if( currentPtr->index == goalIdx )
{
ros::Time time_2 = ros::Time::now();
terminatePtr = currentPtr;
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;
}
//get the succetion
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];
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
*
*/
// shall update: gScore = inf; fScore = inf; cameFrom = NULL, id, mayby direction
neighborPtr->gScore = currentPtr->gScore + edgeCostSets[i];
neighborPtr->fScore = neighborPtr->gScore + getHeu(neighborPtr,endPtr);
neighborPtr->cameFrom = currentPtr; // todo shallow copy or deep copy
// push node "m" into OPEN
openSet.insert(make_pair(neighborPtr -> fScore, neighborPtr));
neighborPtr -> id = 1;
continue;
}
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
*
*/
// shall update: gScore; fScore; cameFrom, mayby direction
if( neighborPtr->gScore > (currentPtr->gScore+edgeCostSets[i]))
{
neighborPtr -> gScore = currentPtr -> gScore + edgeCostSets[i];
neighborPtr -> fScore = neighborPtr -> gScore + getHeu(neighborPtr,endPtr);
neighborPtr -> cameFrom = currentPtr;
}
continue;
}
else{//this node is in closed set
/*
*
please write your code below
*
*/
// todo nothing to do here?
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() );
}
// 获得A*搜索到的完整路径
vector<Vector3d> AstarPathFinder::getPath()
{
vector<Vector3d> path;
vector<GridNodePtr> gridPath;
/*
*
*
STEP 8: trace back from the curretnt nodePtr to get all nodes along the path
please write your code below
*
*/
auto ptr = terminatePtr;
while(ptr -> cameFrom != NULL){
gridPath.push_back(ptr);
ptr = ptr->cameFrom;
}
for (auto ptr: gridPath)
path.push_back(ptr->coord);
reverse(path.begin(),path.end());
return path;
}
// if the difference of f is trivial, then choose then prefer the path along the straight line from start to goal
// discared!!!
// 目前这个函数我没有用到.
GridNodePtr & TieBreaker(const std::multimap<double, GridNodePtr> & openSet, const GridNodePtr & endPtr)
{
// todo do I have to update the f in openSet??
std::multimap<double, GridNodePtr> local_set;
auto f_min = openSet.begin()->first;
auto f_max = f_min + 1e-2;
auto itlow = openSet.lower_bound (f_min);
auto itup = openSet.upper_bound(f_max);
double cross, f_new;
for (auto it=itlow; it!=itup; ++it)
{
std::cout << "f value is:" << (*it).first << " pointer is: " << (*it).second << '\n';
cross = std::abs(endPtr->coord(0) - (*it).second->coord(0)) +
std::abs(endPtr->coord(1) - (*it).second->coord(1)) +
std::abs(endPtr->coord(2) - (*it).second->coord(2));
f_new = (*it).second->fScore + 0.001 * cross;
local_set.insert( make_pair(f_new, (*it).second) ); // todo what is iterator, is this way correct?
}
return local_set.begin()->second;
}
demo_node.cpp
#include <iostream>
#include <fstream>
#include <math.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include "Astar_searcher.h"
#include "JPS_searcher.h"
#include "backward.hpp"
using namespace std;
using namespace Eigen;
namespace backward {
backward::SignalHandling sh;
}
// simulation param from launch file
// 精度,精度倒数,点云间隙
double _resolution, _inv_resolution, _cloud_margin;
// 地图x,y,z尺寸
double _x_size, _y_size, _z_size;
// useful global variables
bool _has_map = false;
// 起始点指针
Vector3d _start_pt;
// 地图上下界
Vector3d _map_lower, _map_upper;
// 地图x,y,z最大id
int _max_x_id, _max_y_id, _max_z_id;
// ros related
ros::Subscriber _map_sub, _pts_sub;
ros::Publisher _grid_path_vis_pub, _visited_nodes_vis_pub, _grid_map_vis_pub;
AstarPathFinder * _astar_path_finder = new AstarPathFinder();
JPSPathFinder * _jps_path_finder = new JPSPathFinder();
// 终点信息的回调函数
void rcvWaypointsCallback(const nav_msgs::Path & wp);
// 地图信息的回调函数
void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map);
// 实体化搜索到的路径
void visGridPath( vector<Vector3d> nodes, bool is_use_jps );
// 实体化显示所有访问过的节点
void visVisitedNode( vector<Vector3d> nodes );
// 路径规划函数
void pathFinding(const Vector3d start_pt, const Vector3d target_pt);
// 终点信息的回调函数
void rcvWaypointsCallback(const nav_msgs::Path & wp)
{
// 如果终点信息的z值小于0,或者没有地图信息,则直接返回
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);
}
// 地图信息的回调函数
void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map)
{
// 如果已经有地图信息,则返回
if( _has_map ) return;
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_vis;
sensor_msgs::PointCloud2 map_vis;
// 将点云格式为sensor_msgs/PointCloud2 格式转为 pcl/PointCloud(在RVIZ中显示的点云的数据格式sensor_msgs::PointCloud2)
pcl::fromROSMsg(pointcloud_map, cloud);
// 如果还没有点云数据,则返回
if( (int)cloud.points.size() == 0 ) return;
pcl::PointXYZ pt;
for (int idx = 0; idx < (int)cloud.points.size(); idx++)
{
pt = cloud.points[idx];
// set obstalces into grid map for path planning
_astar_path_finder->setObs(pt.x, pt.y, pt.z);
// jps才能用得到,Astar永不到
// _jps_path_finder->setObs(pt.x, pt.y, pt.z);
// for visualize only
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.width = cloud_vis.points.size();
cloud_vis.height = 1;
cloud_vis.is_dense = true;
// pcl::toROSMsg (pcl::PointCloud<pcl::PointXYZ>,sensor_msgs::PointCloud2);
pcl::toROSMsg(cloud_vis, map_vis);
map_vis.header.frame_id = "/world";
//发布地图信息
_grid_map_vis_pub.publish(map_vis);
_has_map = true;
}
//路径规划函数
void pathFinding(const Vector3d start_pt, const Vector3d target_pt)
{
//Call A* to search for a path
_astar_path_finder->AstarGraphSearch(start_pt, target_pt);
//Retrieve the path
auto grid_path = _astar_path_finder->getPath();
auto visited_nodes = _astar_path_finder->getVisitedNodes();
//Visualize the result
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
//you just need to change the #define value of _use_jps
#define _use_jps 0
#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
}
//主函数
int main(int argc, char** argv)
{
// 初始化demo_node节点
ros::init(argc, argv, "demo_node");
ros::NodeHandle nh("~");
_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);
// param是从参数服务器中获取第一个参数,将值保存到第二个参数中,如果没获取到第一个参数的值,则使用默认值(第三个参数)
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 = 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);
// rate中的参数100，代表在while循环中，每秒钟运行100次。
ros::Rate rate(100);
bool status = ros::ok();
while(status)
{
// 监听反馈函数（callback）。只能监听反馈，不能循环。
ros::spinOnce();
status = ros::ok();
rate.sleep();
}
delete _astar_path_finder;
delete _jps_path_finder;
return 0;
}
// 实体化搜索到的路径
void visGridPath( vector<Vector3d> nodes, bool is_use_jps )
{
visualization_msgs::Marker node_vis;
node_vis.header.frame_id = "world";
node_vis.header.stamp = ros::Time::now();
// is_use_jps是用于判断用的是不是jps算法
if(is_use_jps)
node_vis.ns = "demo_node/jps_path";
else
node_vis.ns = "demo_node/astar_path";
node_vis.type = visualization_msgs::Marker::CUBE_LIST;
node_vis.action = visualization_msgs::Marker::ADD;
node_vis.id = 0;
//----不明白这里的pose是什么作用???
node_vis.pose.orientation.x = 0.0;
node_vis.pose.orientation.y = 0.0;
node_vis.pose.orientation.z = 0.0;
node_vis.pose.orientation.w = 1.0;
//路径的颜色
if(is_use_jps){
node_vis.color.a = 1.0;
node_vis.color.r = 1.0;
node_vis.color.g = 0.0;
node_vis.color.b = 0.0;
}
else{
node_vis.color.a = 1.0;
node_vis.color.r = 0.0;
node_vis.color.g = 1.0;
node_vis.color.b = 0.0;
}
node_vis.scale.x = _resolution;
node_vis.scale.y = _resolution;
node_vis.scale.z = _resolution;
// 将所有路径中的点压入堆栈
geometry_msgs::Point pt;
for(int i = 0; i < int(nodes.size()); i++)
{
Vector3d coord = nodes[i];
pt.x = coord(0);
pt.y = coord(1);
pt.z = coord(2);
node_vis.points.push_back(pt);
}
// 发布显示所有路径节点
_grid_path_vis_pub.publish(node_vis);
}
//按理说这个函数是实体化显示所有访问过得点,但是在地图中却没有显示出来
void visVisitedNode( vector<Vector3d> nodes )
{
visualization_msgs::Marker node_vis;
node_vis.header.frame_id = "world";
node_vis.header.stamp = ros::Time::now();
node_vis.ns = "demo_node/expanded_nodes";
node_vis.type = visualization_msgs::Marker::CUBE_LIST;
node_vis.action = visualization_msgs::Marker::ADD;
node_vis.id = 0;
node_vis.pose.orientation.x = 0.0;
node_vis.pose.orientation.y = 0.0;
node_vis.pose.orientation.z = 0.0;
node_vis.pose.orientation.w = 1.0;
node_vis.color.a = 0.5;
node_vis.color.r = 0.0;
node_vis.color.g = 0.0;
node_vis.color.b = 1.0;
node_vis.scale.x = _resolution;
node_vis.scale.y = _resolution;
node_vis.scale.z = _resolution;
geometry_msgs::Point pt;
for(int i = 0; i < int(nodes.size()); i++)
{
Vector3d coord = nodes[i];
pt.x = coord(0);
pt.y = coord(1);
pt.z = coord(2);
node_vis.points.push_back(pt);
}
_visited_nodes_vis_pub.publish(node_vis);
}
waypoint_generator.cpp
#include <iostream>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Vector3.h>
#include <nav_msgs/Path.h>
#include "sample_waypoints.h"
#include <vector>
#include <deque>
#include <boost/format.hpp>
#include <eigen3/Eigen/Dense>
using namespace std;
using bfmt = boost::format;
ros::Publisher pub1;
ros::Publisher pub2;
ros::Publisher pub3;
string waypoint_type = string("manual");
bool is_odom_ready;
nav_msgs::Odometry odom;
nav_msgs::Path waypoints;
// series waypoint needed
std::deque<nav_msgs::Path> waypointSegments;
ros::Time trigged_time;
void load_seg(ros::NodeHandle& nh, int segid, const ros::Time& time_base) {
std::string seg_str = boost::str(bfmt("seg%d/") % segid);
double yaw;
double time_from_start;
ROS_INFO("Getting segment %d", segid);
ROS_ASSERT(nh.getParam(seg_str + "yaw", yaw));
ROS_ASSERT_MSG((yaw > -3.1499999) && (yaw < 3.14999999), "yaw=%.3f", yaw);
ROS_ASSERT(nh.getParam(seg_str + "time_from_start", time_from_start));
ROS_ASSERT(time_from_start >= 0.0);
std::vector<double> ptx;
std::vector<double> pty;
std::vector<double> ptz;
ROS_ASSERT(nh.getParam(seg_str + "x", ptx));
ROS_ASSERT(nh.getParam(seg_str + "y", pty));
ROS_ASSERT(nh.getParam(seg_str + "z", ptz));
ROS_ASSERT(ptx.size());
ROS_ASSERT(ptx.size() == pty.size() && ptx.size() == ptz.size());
nav_msgs::Path path_msg;
path_msg.header.stamp = time_base + ros::Duration(time_from_start);
double baseyaw = tf::getYaw(odom.pose.pose.orientation);
for (size_t k = 0; k < ptx.size(); ++k) {
geometry_msgs::PoseStamped pt;
pt.pose.orientation = tf::createQuaternionMsgFromYaw(baseyaw + yaw);
Eigen::Vector2d dp(ptx.at(k), pty.at(k));
Eigen::Vector2d rdp;
rdp.x() = std::cos(-baseyaw-yaw)*dp.x() + std::sin(-baseyaw-yaw)*dp.y();
rdp.y() =-std::sin(-baseyaw-yaw)*dp.x() + std::cos(-baseyaw-yaw)*dp.y();
pt.pose.position.x = rdp.x() + odom.pose.pose.position.x;
pt.pose.position.y = rdp.y() + odom.pose.pose.position.y;
pt.pose.position.z = ptz.at(k) + odom.pose.pose.position.z;
path_msg.poses.push_back(pt);
}
waypointSegments.push_back(path_msg);
}
void load_waypoints(ros::NodeHandle& nh, const ros::Time& time_base) {
int seg_cnt = 0;
waypointSegments.clear();
ROS_ASSERT(nh.getParam("segment_cnt", seg_cnt));
for (int i = 0; i < seg_cnt; ++i) {
load_seg(nh, i, time_base);
if (i > 0) {
ROS_ASSERT(waypointSegments[i - 1].header.stamp < waypointSegments[i].header.stamp);
}
}
ROS_INFO("Overall load %zu segments", waypointSegments.size());
}
// 发布目标点信息,并清空waypoints
void publish_waypoints() {
waypoints.header.frame_id = std::string("world");
waypoints.header.stamp = ros::Time::now();
pub1.publish(waypoints);
// 下面这四行代码无用,因为我们并没有odom坐标系,可能是原代码作者需要用到
geometry_msgs::PoseStamped init_pose;
init_pose.header = odom.header;
init_pose.pose = odom.pose.pose;
waypoints.poses.insert(waypoints.poses.begin(), init_pose);
// pub2.publish(waypoints);
waypoints.poses.clear();
}
void publish_waypoints_vis() {
nav_msgs::Path wp_vis = waypoints;
geometry_msgs::PoseArray poseArray;
poseArray.header.frame_id = std::string("world");
poseArray.header.stamp = ros::Time::now();
{
geometry_msgs::Pose init_pose;
init_pose = odom.pose.pose;
poseArray.poses.push_back(init_pose);
}
for (auto it = waypoints.poses.begin(); it != waypoints.poses.end(); ++it) {
geometry_msgs::Pose p;
p = it->pose;
poseArray.poses.push_back(p);
}
pub2.publish(poseArray);
}
// odom回调函数,我们并没有odom坐标系,无用
void odom_callback(const nav_msgs::Odometry::ConstPtr& msg) {
is_odom_ready = true;
odom = *msg;
if (waypointSegments.size()) {
ros::Time expected_time = waypointSegments.front().header.stamp;
if (odom.header.stamp >= expected_time) {
waypoints = waypointSegments.front();
std::stringstream ss;
ss << bfmt("Series send %.3f from start:\n") % trigged_time.toSec();
for (auto& pose_stamped : waypoints.poses) {
ss << bfmt("P[%.2f, %.2f, %.2f] q(%.2f,%.2f,%.2f,%.2f)") %
pose_stamped.pose.position.x % pose_stamped.pose.position.y %
pose_stamped.pose.position.z % pose_stamped.pose.orientation.w %
pose_stamped.pose.orientation.x % pose_stamped.pose.orientation.y %
pose_stamped.pose.orientation.z << std::endl;
}
ROS_INFO_STREAM(ss.str());
publish_waypoints_vis();
publish_waypoints();
waypointSegments.pop_front();
}
}
}
// 目标点回调函数
void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
// 原代码这部分就是被注释掉的,所以从这部分可以得出
/* if (!is_odom_ready) {
ROS_ERROR("[waypoint_generator] No odom!");
return;
}*/
trigged_time = ros::Time::now(); //odom.header.stamp;
//ROS_ASSERT(trigged_time > ros::Time(0));
ros::NodeHandle n("~");
n.param("waypoint_type", waypoint_type, string("manual"));
if (waypoint_type == string("circle")) {
waypoints = circle();
publish_waypoints_vis();
publish_waypoints();
} else if (waypoint_type == string("eight")) {
waypoints = eight();
publish_waypoints_vis();
publish_waypoints();
} else if (waypoint_type == string("point")) {
waypoints = point();
publish_waypoints_vis();
publish_waypoints();
} else if (waypoint_type == string("series")) {
load_waypoints(n, trigged_time);
// 我们只用看这部分,这部分才是我们能用到的
} else if (waypoint_type == string("manual-lonely-waypoint")) {
// if height >= 0, it's a valid goal;
if (msg->pose.position.z >= 0) {
geometry_msgs::PoseStamped pt = *msg;
waypoints.poses.clear();
waypoints.poses.push_back(pt);
// publish_waypoints_vis();
publish_waypoints();
} else {
ROS_WARN("[waypoint_generator] invalid goal in manual-lonely-waypoint mode.");
}
} else {
if (msg->pose.position.z > 0) {
// if height > 0, it's a normal goal;
geometry_msgs::PoseStamped pt = *msg;
if (waypoint_type == string("noyaw")) {
double yaw = tf::getYaw(odom.pose.pose.orientation);
pt.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
}
waypoints.poses.push_back(pt);
publish_waypoints_vis();
} else if (msg->pose.position.z > -1.0) {
// if 0 > height > -1.0, remove last goal;
if (waypoints.poses.size() >= 1) {
waypoints.poses.erase(std::prev(waypoints.poses.end()));
}
publish_waypoints_vis();
} else {
// if -1.0 > height, end of input
if (waypoints.poses.size() >= 1) {
publish_waypoints_vis();
publish_waypoints();
}
}
}
}
void traj_start_trigger_callback(const geometry_msgs::PoseStamped& msg) {
if (!is_odom_ready) {
ROS_ERROR("[waypoint_generator] No odom!");
return;
}
ROS_WARN("[waypoint_generator] Trigger!");
trigged_time = odom.header.stamp;
ROS_ASSERT(trigged_time > ros::Time(0));
ros::NodeHandle n("~");
n.param("waypoint_type", waypoint_type, string("manual"));
ROS_ERROR_STREAM("Pattern " << waypoint_type << " generated!");
if (waypoint_type == string("free")) {
waypoints = point();
publish_waypoints_vis();
publish_waypoints();
} else if (waypoint_type == string("circle")) {
waypoints = circle();
publish_waypoints_vis();
publish_waypoints();
} else if (waypoint_type == string("eight")) {
waypoints = eight();
publish_waypoints_vis();
publish_waypoints();
} else if (waypoint_type == string("point")) {
waypoints = point();
publish_waypoints_vis();
publish_waypoints();
} else if (waypoint_type == string("series")) {
load_waypoints(n, trigged_time);
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "waypoint_generator");
ros::NodeHandle n("~");
n.param("waypoint_type", waypoint_type, string("manual"));
ros::Subscriber sub1 = n.subscribe("odom", 10, odom_callback);
ros::Subscriber sub2 = n.subscribe("goal", 10, goal_callback);
// 没用到
ros::Subscriber sub3 = n.subscribe("traj_start_trigger", 10, traj_start_trigger_callback);
pub1 = n.advertise<nav_msgs::Path>("waypoints", 50);
pub2 = n.advertise<geometry_msgs::PoseArray>("waypoints_vis", 10);
trigged_time = ros::Time(0);
ros::spin();
return 0;
}
参考引用
代码来源:https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/tree/master/hw_2.
相关知识来源:深蓝学院<<移动机器人运动规划>>
扫一扫
8849
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
