ROS:move_base自定义全局规划器
如果看过古月居的视频应该很熟悉下面几个文件夹
其中move_base.launch在下方路径中
一.编写路径规划器
在工作空间的src下（我的工作空间名是cat_ws）创建全局规划器包
catkin_create_pkg rrt_astar_global_planner nav_core roscpp rospy std_msgs
rrt_astart.h放在新建包的include/rrt_astar_global_planner下
rrt_astar.cpp 放入刚建立的文件包的src下
rrt_astart.h如下：
#include
#include
/** for global path planner interface **/
#include
#include
#include
#include
#include
#include
#include
#include
#include
/** include standard libraries **/
#include
#include
#include
#include
#include
#include
#include
using std::string;
#ifndef RRTSTAR_ROS_CPP
#define RRTSTAR_ROS_CPP
/**
* @brief Node struct
*
*/
struct Node {
float x;
float y;
int node_id;
int parent_id;
float cost;
bool operator ==(const Node& node)
{
return (x == node.x) && (y == node.y) && (node_id == node.node_id) && (parent_id == node.parent_id) && (cost == node.cost) ;
}
bool operator !=(const Node& node)
{
if((x != node.x) || (y != node.y) || (node_id != node.node_id) || (parent_id != node.parent_id) || (cost != node.cost))
return true;
else
return false;
}
};
namespace RRTstar_planner
{
class RRTstarPlannerROS : public nav_core::BaseGlobalPlanner
{
public:
/**
* @brief Default constructor of the plugin
*/
RRTstarPlannerROS();
RRTstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Initialization function for the PlannerCore object
* @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
*/
void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
* @param goal The goal pose
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector& plan);
void publishPlan(const std::vector& path);
/*
* @brief Compute the euclidean distance (straight-line distance) between two points
* @param px1 point 1 x
* @param py1 point 1 y
* @param px2 point 2 x
* @param py2 point 2 y
* @return the distance computed
*/
float distance(float px1, float py1, float px2, float py2);
/**
* @brief it randomly samples a point in the free space of the plan
* @return the a random point in the free space of the cartesian plane considered
*/
std::pair sampleFree();
/**
* @brief Check if there is a collision at the world point (wx, wy)
* @param wx world x coordinate (cartesian system)
* @param wy world y coordinate (cartesian system)
* @return True is the point collides and false otherwise
*/
bool collision(float wx, float wy);
/**
* @brief Given the nodes set and an point the function returns the closest node of the node
* @param nodes the set of nodes
* @param p_rand the random point (x,y) in the plane
* return the closest node
*/
Node getNearest(std::vector nodes, std::pair p_rand);
/**
* @brief Select the best parent. Check if there is any node around the newnode with cost less than its parent node cost.
* If yes choose this less cost node as the new parent of the newnode.
* @param nn the parent of the newnode
* @param newnode the node that will checked if there is a better parent for it
* @param nodes the set of nodes
* @return the same newnode with the best parent node
*
*/
Node chooseParent(Node nn, Node newnode, std::vector nodes);
/*
* @brief The function checks if the cost of the parents of all nodes around is still less than the newnode.
* If there is a node with parent with higher cost the new parent of this node is the newnode now.
*
* @param nodes the set of nodes
* @param newnode the newnode
* @return the nodes set rewired
*/
std::vector rewire(std::vector nodes, Node newnode);
/*
* @brief The function generate the new point between the epsilon_min and epsilon_max along the line p_rand and nearest node.
* This new point is a node candidate. It will a node if there is no obstacles between its nearest node and itself.
* @param px1 point 1 x
* @param py1 point 1 y
* @param px2 point 2 x
* @param py2 point 2 y
* @return the new point
*/
std::pair steer(float x1, float y1, float x2, float y2);
bool obstacleFree(Node node_nearest, float px, float py);
/**
* @brief Check if the distance between the goal and the newnode is less than the GOAL_RADIUS. If yes the newnode is the goal.
* @param px1 point 1 x
* @param py1 point 1 y
* @param px2 point 2 x
* @param py2 point 2 y
* *@return True if distance is less than the xy tolerance (GOAL_RADIUS), False otherwise
*/
bool pointCircleCollision(float x1, float y1, float x2, float y2, float radius);
float XDIM;
float YDIM;
int MAX_NUM_NODES;
float RADIUS;
float GOAL_RADIUS;
float epsilon_min;
float epsilon_max;
protected:
/**
* @brief Store a copy of the current costmap in \a costmap. Called by makePlan.
*/
costmap_2d::Costmap2D* costmap_;
costmap_2d::Costmap2DROS* costmap_ros_;
std::string frame_id_;
ros::Publisher plan_pub_;
ros::Publisher pubPathNodes;
// TODO
//allow_unknown_;
private:
/**
* @brief Convert from Map (matrix type) coordinates (mx = column and my = row) to World
*/
void mapToWorld(int mx, int my, float& wx, float& wy);
/**
* @brief Convert from Map (matrix type) coordinates (mx = column and my = row) to World
*/
void worldToMap(float wx, float wy, int& mx, int& my);
float originX;
float originY;
float resolution;
//double step_size_, min_dist_from_robot_;
//base_local_planner::WorldModel* world_model_;
bool initialized_;
int width;
int height;
visualization_msgs::MarkerArray pathNodes;
};
}; // RRTstar_planner namespace
#endif rrt_astart.cpp如下:
#include
#include
#include
#include
std::random_device rd;
static std::default_random_engine generator ( rd() );
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(RRTstar_planner::RRTstarPlannerROS, nav_core::BaseGlobalPlanner)
namespace RRTstar_planner
{
RRTstarPlannerROS::RRTstarPlannerROS()
: costmap_(nullptr), initialized_(false) { }
RRTstarPlannerROS::RRTstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
: costmap_ros_(costmap_ros)
{
//initialize the planner
initialize(name, costmap_ros);
}
void RRTstarPlannerROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{
if (!initialized_)
{
// Initialize map
costmap_ros_ = costmap_ros;
costmap_ = costmap_ros->getCostmap();
ros::NodeHandle private_nh("~/" + name);
originX = costmap_->getOriginX();
originY = costmap_->getOriginY();
width = costmap_->getSizeInCellsX();
height = costmap_->getSizeInCellsY();
resolution = costmap_->getResolution();
frame_id_ = costmap_ros->getGlobalFrameID();
RADIUS = 1.0;
GOAL_RADIUS = 0.5;
epsilon_min = 0.05;
epsilon_max = 0.1;
ROS_INFO("RRT* planner initialized successfully");
initialized_ = true;
plan_pub_ = private_nh.advertise("plan", 1);
pubPathNodes = private_nh.advertise("path_point", 1);
}
else
ROS_WARN("This planner has already been initialized... doing nothing");
}
bool RRTstarPlannerROS::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector& plan)
{
//clear the plan, just in case
plan.clear();
std::vector> path;
ros::NodeHandle n;
std::string global_frame = frame_id_;
std::vector nodes;
MAX_NUM_NODES = 20000;
Node start_node;
start_node.x = start.pose.position.x;
start_node.y = start.pose.position.y;
start_node.node_id = 0;
start_node.parent_id = -1; // None parent node
start_node.cost = 0.0;
nodes.push_back(start_node);
std::pair p_rand;
std::pair p_new;
Node node_nearest;
while (nodes.size() < MAX_NUM_NODES)
{
bool found_next = false;
while (found_next == false)
{
p_rand = sampleFree(); // random point in the free space
node_nearest = getNearest(nodes, p_rand); // The nearest node of the random point
p_new = steer(node_nearest.x, node_nearest.y, p_rand.first, p_rand.second); // new point and node candidate.
if (obstacleFree(node_nearest, p_new.first, p_new.second))
{
Node newnode;
newnode.x = p_new.first;
newnode.y = p_new.second;
newnode.node_id = nodes.size(); // index of the last element after the push_bask below
newnode.parent_id = node_nearest.node_id;
newnode.cost = 0.0;
// Optimize
newnode = chooseParent(node_nearest, newnode, nodes); // Select the best parent
nodes.push_back(newnode);
nodes = rewire(nodes, newnode);
found_next = true;
}
}
// Check if the distance between the goal and the new node is less than the GOAL_RADIUS
if (pointCircleCollision(p_new.first, p_new.second, goal.pose.position.x , goal.pose.position.y, GOAL_RADIUS) && nodes.size() > 10000)
{
ROS_INFO("RRT* Global Planner: Path found!!!!");
std::pair point;
// New goal inside of the goal tolerance
Node new_goal_node = nodes[nodes.size() - 1];
Node current_node = new_goal_node;
current_node = new_goal_node;
// Final Path
while (current_node.parent_id != -1)
{
point.first = current_node.x;
point.second = current_node.y;
path.insert(path.begin(), point);
current_node = nodes[current_node.parent_id];
}
//std::cout << "Path size: " << path.size() << std::endl;
//if the global planner find a path
if (path.size() > 0)
{
plan.push_back(start);
ros::Time plan_time = ros::Time::now();
// convert the points to poses
for (int i = 0; i < path.size(); i++)
{
visualization_msgs::Marker pathNode;
pathNode.header.frame_id = costmap_ros_->getGlobalFrameID();
pathNode.header.stamp = plan_time;
pathNode.id = i;
pathNode.type = visualization_msgs::Marker::SPHERE;
pathNode.scale.x = 0.1;
pathNode.scale.y = 0.1;
pathNode.scale.z = 0.1;
pathNode.color.a = 1.0;
pathNode.color.r = 1.0;
pathNode.pose.position.x = path[i].first;
pathNode.pose.position.y = path[i].second;
pathNodes.markers.push_back(pathNode);
//std::cout << path[i].first << " " << path[i].second << std::endl;
geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_ros_->getGlobalFrameID();
pose.pose.position.x = path[i].first;
pose.pose.position.y = path[i].second;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
plan.push_back(pose);
}
// plan.push_back(goal);
pubPathNodes.publish(pathNodes);
publishPlan(plan);
return true;
}
else
{
ROS_WARN("The planner failed to find a path, choose other goal position");
return false;
}
}
}
ROS_WARN("The planner failed to find a path, choose other goal position");
return false;
}
bool RRTstarPlannerROS::pointCircleCollision(float x1, float y1, float x2, float y2, float radius)
{
float dist = distance(x1, y1, x2, y2);
if (dist < radius)
return true;
else
return false;
}
float RRTstarPlannerROS::distance(float px1, float py1, float px2, float py2)
{
float dist = sqrt((px1 - px2)*(px1 - px2) + (py1 - py2)*(py1 - py2));
return dist;
}
std::pair RRTstarPlannerROS::sampleFree()
{
std::pair random_point;
for (int i = 0; i < 10000; i++)
{
// generate random x and y coords within map bounds
std::random_device rd;
std::mt19937 gen(rd());
//float map_width = costmap_->getSizeInMetersX();
//float map_height = costmap_->getSizeInMetersY();
// Using the clearpath Husky World I know that the dimensions are
float map_width = 10.0;
float map_height = 10.0;
std::uniform_real_distribution<> x(-map_width, map_width);
std::uniform_real_distribution<> y(-map_height, map_height);
random_point.first = x(gen);
random_point.second = y(gen);
if (!collision(random_point.first, random_point.second))
return random_point;
//TODO check collision
//TODO check collision
//TODO check collision
//TODO check collision
//TODO check collision
//TODO check collision
}
return random_point;
}
void RRTstarPlannerROS::publishPlan(const std::vector& path) {
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return;
}
//create a message for the plan
nav_msgs::Path gui_path;
gui_path.poses.resize(path.size());
gui_path.header.frame_id = frame_id_;
gui_path.header.stamp = ros::Time::now();
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
for (unsigned int i = 0; i < path.size(); i++) {
gui_path.poses[i] = path[i];
}
plan_pub_.publish(gui_path);
}
void RRTstarPlannerROS::mapToWorld(int mx, int my, float& wx, float& wy)
{
wx = costmap_->getOriginX() + mx * costmap_->getResolution();
wy = costmap_->getOriginY() + my * costmap_->getResolution();
}
void RRTstarPlannerROS::worldToMap(float wx, float wy, int& mx, int& my) {
float origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
float resolution = costmap_->getResolution();
mx = (wx - origin_x) / resolution ;
my = (wy - origin_y) / resolution ;
}
//check if point collides with the obstacle
bool RRTstarPlannerROS::collision(float wx, float wy)
{
int mx, my;
worldToMap(wx, wy, mx, my);
if ((mx < 0) || (my < 0) || (mx >= costmap_->getSizeInCellsX()) || (my >= costmap_->getSizeInCellsY()))
return true;
// grid[row][column] = vector[row*WIDTH + column]
//if (costmap_[my*width + mx] > 0)
// return true;
unsigned int cost = static_cast(costmap_ -> getCost(mx, my));
if (cost > 0)
return true;
return false;
}
Node RRTstarPlannerROS::getNearest(std::vector nodes, std::pair p_rand)
{
Node node = nodes[0];
for (int i = 1; i < nodes.size(); i++)
{
if (distance(nodes[i].x, nodes[i].y, p_rand.first, p_rand.second) < distance(node.x, node.y, p_rand.first, p_rand.second))
node = nodes[i];
}
return node;
}
Node RRTstarPlannerROS::chooseParent(Node nn, Node newnode, std::vector nodes)
{
for (int i = 0; i < nodes.size(); i++)
{
if (distance(nodes[i].x, nodes[i].y, newnode.x, newnode.y) < RADIUS &&
nodes[i].cost + distance(nodes[i].x, nodes[i].y, newnode.x, newnode.y) < nn.cost + distance(nn.x, nn.y, newnode.x, newnode.y) &&
obstacleFree(nodes[i], nn.x, nn.y))
{
nn = nodes[i];
}
}
newnode.cost = nn.cost + distance(nn.x, nn.y, newnode.x, newnode.y);
newnode.parent_id = nn.node_id;
return newnode;
}
std::vector RRTstarPlannerROS::rewire(std::vector nodes, Node newnode)
{
Node node;
for (int i = 0; i < nodes.size(); i++)
{
node = nodes[i];
if (node != nodes[newnode.parent_id] && distance(node.x, node.y, newnode.x, newnode.y) < RADIUS &&
newnode.cost + distance(node.x, node.y, newnode.x, newnode.y) < node.cost && obstacleFree(node, newnode.x, newnode.y))
{
node.parent_id = newnode.node_id;
node.cost = newnode.cost + distance(node.x, node.y, newnode.x, newnode.y);
}
}
return nodes;
}
std::pair RRTstarPlannerROS::steer(float x1, float y1, float x2, float y2)
{
std::pair p_new;
float dist = distance(x1, y1, x2, y2);
if (dist < epsilon_max && dist > epsilon_min)
{
p_new.first = x1;
p_new.second = y1;
return p_new;
}
else
{
float theta = atan2(y2-y1, x2-x1);
p_new.first = x1 + epsilon_max*cos(theta);
p_new.second = y1 + epsilon_max*sin(theta);
return p_new;
}
}
bool RRTstarPlannerROS::obstacleFree(Node node_nearest, float px, float py)
{
int n = 1;
float theta;
std::pair p_n;
p_n.first = 0.0;
p_n.second = 0.0;
float dist = distance(node_nearest.x, node_nearest.y, px, py);
if (dist < resolution)
{
if (collision(px, py))
return false;
else
return true;
}
else
{
int value = int(floor(dist/resolution));
float theta;
for (int i = 0;i < value; i++)
{
theta = atan2(node_nearest.y - py, node_nearest.x - px);
p_n.first = node_nearest.x + n*resolution*cos(theta);
p_n.second = node_nearest.y + n*resolution*sin(theta);
if (collision(p_n.first, p_n.second))
return false;
n++;
}
return true;
}
}
}; // RRTstar_planner namespace 此时对cmake.list添加依赖库 Declare a C++ library处
并且取消两处注释，否则头文件你用不了，编译不会通过
再次编译
catkin_make
此时devel/lib发现生成librrt_astar_global_planner_lib .so库文件
二.编写插件
1.注册插件类（此一中代码），即
2.创建插件描述文件
在rrt_astar_global_planner创建xml描述文件
touch rrt_astar_global_planner_plugin.xml代码内容如下:
This is RRT Astar Global Planner Plugin by Rafael Barreto.
3.在package.xml添加rrt_astar_global_planner_plugin.xml
4.再次编译 catkin_make
三.move_base使用
参考：b站up 小巨同学zz