概述
代码来源:https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/tree/master/hw_2.
文章目录
- 前言
- 一、获取代码
- 二、过程演示
- 1.启动roscore
- 2.打开rviz
- 3.打开rviz文件
- 4.新建终端加载地图
- 5.进行路径搜索
- 三、ROS包
- node.h
- Astar_searcher.h
- Astar_searcher.cpp
- demo_node.cpp
- waypoint_generator.cpp
- 声明
前言
1968年发明的A*算法就是把启发式方法（heuristic approaches）如BFS，和常规方法如Dijsktra算法结合在一起的算法。
A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。
具体原理见朋友写的自动驾驶路径规划——A*（Astar）算法
一、获取代码
在开头的网站中下载ros文件夹下的工作空间catkin_ws1，然后将其中的四个功能包复制到自己的工作空间。（直接复制工作空间遇到了本人无法解决的错误，所以做此选择）
编译后发现代码有错误，将字母‘g’删掉就好
编译完成
二、过程演示
1.启动roscore
roscore
2.打开rviz
rviz
3.打开rviz文件
在rviz中打开demo.rviz(路径src/grid_path_searcher/launch/rviz_config)。
4.新建终端加载地图
roslaunch grid_path_searcher demo.launch
5.进行路径搜索
规划前后对比
画红圈处即为规划的路径
在终端中还有提示信息
三、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
{
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(){};
};
#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;
double resolution, inv_resolution;
double gl_xl, gl_yl, gl_zl;
double gl_xu, gl_yu, gl_zu;
GridNodePtr terminatePtr;
std::multimap<double, GridNodePtr> openSet;
double getHeu(GridNodePtr node1, GridNodePtr node2);
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(){};
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);
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_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;
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[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]);
}
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 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));
}
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;
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
}
}
}
}
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
if(tie_break){
double p = 1.0 / 25.0;
h *= (1.0 + p);
//std::cout << "Tie Break!" << std::endl;
}
// std::cout <<"h is: "<< h << std::endl;
return h;
}
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;
openSet.insert( make_pair(startPtr -> fScore, startPtr) ); // todo Note: modified, insert the pointer GridNodeMap[i][j][k] to the start node in grid map
/*
*
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
*
*
*/
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() );
}
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;
double _x_size, _y_size, _z_size;
// useful global variables
bool _has_map = false;
Vector3d _start_pt;
Vector3d _map_lower, _map_upper;
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)
{
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;
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_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(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)
{
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);
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);
ros::Rate rate(100);
bool status = ros::ok();
while(status)
{
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();
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;
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());
}
void publish_waypoints() {
waypoints.header.frame_id = std::string("world");
waypoints.header.stamp = ros::Time::now();
pub1.publish(waypoints);
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);
}
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 (msg->pose.position.z >= 0) {
// if height >= 0, it's a valid goal;
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;
}
声明
本人所有文章均为个人学习记录，如有侵权，联系立删。
相关代码可以在个人主页
最后
以上就是疯狂砖头为你收集整理的ROS学习记录（四）基于ROS的A*算法仿真前言一、获取代码二、过程演示三、ROS包声明的全部内容，希望文章能够帮你解决ROS学习记录（四）基于ROS的A*算法仿真前言一、获取代码二、过程演示三、ROS包声明所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复