System overview
-
move_base 主要有以下兩部分
注意上面灰色的模塊是可選的，地圖也是可有可無的，藍色是根據不同的機器模塊進行
配置的，白色模塊是必須的
move_base
源碼
namespace move_base{
enum MoveBaseState {
PLANNING,
CONTROLLING,
CLEARING
};
enum RecoveryTrigger
{
PLANNING_R,
CONTROLLING_R,
OSCILLATION_R
};
MoveBase::MoveBase(tf::TransformListener& tf){
#action server
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
#參數的配置讀取
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
.........
//set up plan triple buffer
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
//set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
#global costmap，
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
#global planner
planner_ = bgp_loader_.createInstance(global_planner);
#local costmap
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
#local planner
tc_ = blp_loader_.createInstance(local_planner);
// Start actively updating costmaps based on sensor data
planner_costmap_ros_->start();
controller_costmap_ros_->start();
}
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal){
//we have a goal so start the planner(通知planner線程進行路徑規划)
boost::unique_lock<boost::mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
#通知規划路徑線程
planner_cond_.notify_one();
lock.unlock();
ros::NodeHandle n;
while(n.ok())
{
#被搶占了(可能是發出新的goal，也可能是取消了)
if(as_->isPreemptRequested()){
if(as_->isNewGoalAvailable()){
#發布新的goal，通知planner線程工作。
planner_cond_.notify_one();
}
else{
//if we've been preempted explicitly we need to shut things down
//強制退出
return;
}
}
//the real work on pursuing a goal is done here
//真正工作的代碼
bool done = executeCycle(goal, global_plan);
//這個是一般的警告信息，規划的時間超時
if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
}
}
void MoveBase::planThread(){
ros::NodeHandle n;
boost::unique_lock<boost::mutex> lock(planner_mutex_);
while(n.ok()){
//等待上面的executeCb函數使得runPlanner_ = true
while(wait_for_wake || !runPlanner_){
//if we should not be running the planner then suspend this thread
ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
}
//time to plan! get a copy of the goal and unlock the mutex
geometry_msgs::PoseStamped temp_goal = planner_goal_;
//路徑規划global
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
if(gotPlan){
//成功規划路徑
ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");
}
//if we didn't get a plan and we are in the planning state (the robot isn't moving)
else if(state_==PLANNING){
//check if we've tried to make a plan for over our time limit
if(ros::Time::now() > attempt_end && runPlanner_){
//we'll move into our obstacle clearing mode
state_ = CLEARING;
publishZeroVelocity();
recovery_trigger_ = PLANNING_R;
}
}
//定時器，多久沒有規划路徑，就通知一次規划路徑
timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
}
}
//這個是在global costmap下做的global planner
bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
// global costmap
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));
//得到機器人現在的位置信息
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
return false;
}
//global planner
if(!planner_->makePlan(start, goal, plan) || plan.empty()){
ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
return false;
}
}
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
//發布速度topic
geometry_msgs::Twist cmd_vel;
//push the feedback out
//發布一些反饋信息
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position = current_position;
as_->publishFeedback(feedback);
//check to see if we've moved far enough to reset our oscillation timeout
if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
{
last_oscillation_reset_ = ros::Time::now();
oscillation_pose_ = current_position;
//if our last recovery was caused by oscillation, we want to reset the recovery index
if(recovery_trigger_ == OSCILLATION_R)
recovery_index_ = 0;
}
//if we have a new plan then grab it and give it to the controller
if(new_global_plan_){
std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
latest_plan_ = temp_plan;
//這是local planner的規划
if(!tc_->setPlan(*controller_plan_)){
//ABORT and SHUTDOWN COSTMAPS
ROS_ERROR("Failed to pass global plan to the controller, aborting.");
resetState();
//disable the planner thread
lock.lock();
runPlanner_ = false;
lock.unlock();
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
return true;
}
//make sure to reset recovery_index_ since we were able to find a valid plan
if(recovery_trigger_ == PLANNING_R)
recovery_index_ = 0;
}
//the move_base state machine, handles the control logic for navigation
switch(state_){
//if we are in a planning state, then we'll attempt to make a plan
case PLANNING:
runPlanner_ = true;
planner_cond_.notify_one();
ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
break;
//if we're controlling, we'll attempt to find valid velocity commands
case CONTROLLING:
//check to see if we've reached our goal
//這是local planner的
if(tc_->isGoalReached()){
ROS_DEBUG_NAMED("move_base","Goal reached!");
as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
return true;
}
//這個似乎是小車不知道為什么來回走動，在一定的時間沒有移動足夠的距離
if(oscillation_timeout_ > 0.0 && last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
{
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = OSCILLATION_R;
}
//發布小車行走的cmd_vel topic
if(tc_->computeVelocityCommands(cmd_vel)){
ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
vel_pub_.publish(cmd_vel);
if(recovery_trigger_ == CONTROLLING_R)
recovery_index_ = 0;
}
else{
ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
if(ros::Time::now() > attempt_end){
//we'll move into our obstacle clearing mode
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = CONTROLLING_R;
}
else{
//otherwise, if we can't find a valid control, we'll go back to planning
planner_cond_.notify_one();
}
}
break;
//we'll try to clear out space with any user-provided recovery behaviors
case CLEARING:
ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size());
recovery_behaviors_[recovery_index_]->runBehavior();
ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
state_ = PLANNING;
//update the index of the next recovery behavior that we'll try
recovery_index_++;
}
else{
ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
runPlanner_ = false;
if(recovery_trigger_ == CONTROLLING_R){
ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
}
else if(recovery_trigger_ == PLANNING_R){
ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
}
else if(recovery_trigger_ == OSCILLATION_R){
ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
}
resetState();
}
break;
default:
ROS_ERROR("This case should never be reached, something is wrong, aborting");
resetState();
}
}
}
從構造函數的actionServer 和線程函數planThread(等待executeCb通知他工作，規划一下路徑，這個是global plan)
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
每次有goal發布的時候，都會去調用executeCb,executeCb會去調用executeCycle
進行local plan,發布cmd_vel的topic，根據小車處於的狀態，進行相應的實現(會進行plan,control,clear obstacle)
API
-
提供兩種方式
send goal目標位置的信息- SimpleActionServer(可以追蹤移動到目標過程的狀態信息，到了沒有，是不是沒有plan，取消了etc)
- Subscribe topic(move_base_simple/goal),直接發布這個topic的信息，不會有目標執行過程中的反饋信息
-
提供的
service- ~make_plan(只會提供plan該怎么走的位置信息，不會使機器人移動)
- ~clear_unknown_space(告訴move_base清楚機器人周圍的unknown space)
- ~clear_costmaps(告訴move_base清楚costmap中的障礙物信息，可能導致撞到障礙物)
nav_core
提供三類接口(這些接口都是以plugin的形式存在，很容易更換)：
- BaseGlobalPlanner
- BaseLocalPlanner
- RecoveryBehavior
BaseGlobalPlanner
所有的global planner都必須實現nav_core::BaseGlobalPlanner定義的接口(純續函數)
主要實現的函數定義如下：
virtual void initialize (std::string name, costmap_2d::Costmap2DROS *costmap_ros)=0
virtual bool makePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)=0
BaseLocalPlanner
所有的local planner都必須實現nav_core::BaseLocalPlanner定義的接口(純續函數)
主要實現的函數定義如下：
#Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
virtual bool computeVelocityCommands (geometry_msgs::Twist &cmd_vel)=0
virtual void initialize (std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros)=0
virtual bool isGoalReached ()=0
virtual bool setPlan (const std::vector< geometry_msgs::PoseStamped > &plan)=0
RecoveryBehavior
所有的recovery behaviors都必須實現nav_core::RecoveryBehavior定義的接口(純續函數)
主要實現的函數定義如下：
virtual void initialize (std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *global_costmap, costmap_2d::Costmap2DROS *local_costmap)=0
virtual void runBehavior ()=0
Recovery behavior
首先, obstacles outside of a user-specified region will be cleared from the robot's map.(conservative_reset_dist半徑之外的非NO_INFORMATION
都會被設置成NO_INFORMATION)。然后, if possible, the robot will perform an in-place rotation to clear out space. If this too fails, 再然后，
the robot will more aggressively clear its map, removing all obstacles outside of the rectangular region in
which it can rotate in place.(默認是4倍的最小外接圓半徑) This will be followed by another in-place rotation. If all this fails,
the robot will consider its goal infeasible and notify the user that it has aborted
插件的配置
~recovery_behaviors
(list, default: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
{name: rotate_recovery, type: rotate_recovery/RotateRecovery},
{name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]
參數
#The distance away from the robot in meters at which obstacles will be cleared from the costmap when attempting to clear space in the map.
~conservative_reset_dist (double, default: 3.0)
#Whether or not to enable the move_base recovery behaviors to attempt to clear out space.
~recovery_behavior_enabled (bool, default: true)
#Determines whether or not the robot will attempt an in-place rotation when attempting to clear out space.
~clearing_rotation_allowed (bool, default: true)
#來回震盪的距離，超過一定的時間沒有移動足夠的距離就會進行recovery behavior
#How far in meters the robot must move to be considered not to be oscillating. Moving this far resets the timer counting up to the ~oscillation_timeout
~oscillation_distance (double, default: 0.5)
#How long in seconds to allow for oscillation before executing recovery behaviors. A value of 0.0 corresponds to an infinite timeout.
~oscillation_timeout (double, default: 0.0)
**For the default parameters, the aggressive_reset behavior will clear out to a distance of 4 * ~/local_costmap/circumscribed_radius. **
clear_costmap_recovery
是把local,global costmap在reset_distance半徑之外的free，occupied都清除，變成NO_INFORMATION。不對static layer操作
重要的參數
#距離以機器人為中心，reset_distance米為半徑的圓外面的costmap(local,global)都會被清除掉，
#只留下static map原來的信息
~<name>/reset_distance (double, default: 3.0)
怎么使用
#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <clear_costmap_recovery/clear_costmap_recovery.h>
tf::TransformListener tf(ros::Duration(10));
costmap_2d::Costmap2DROS global_costmap("global_costmap", tf);
costmap_2d::Costmap2DROS local_costmap("local_costmap", tf);
clear_costmap_recovery::ClearCostmapRecovery ccr;
ccr.initialize("my_clear_costmap_recovery", &tf, &global_costmap, &local_costmap);
//recovery 行為
ccr.runBehavior();
源碼
namespace clear_costmap_recovery{
class ClearCostmapRecovery : public nav_core::RecoveryBehavior {
//在多大的半徑之外的obstacle全部清除
double reset_distance_;
//那些layer的obstacle要清除
std::set<std::string> clearable_layers_; ///< Layer names which will be cleared.
void ClearCostmapRecovery::initialize(std::string name, tf::TransformListener* tf,
costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap){
if(!initialized_){
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
private_nh.param("reset_distance", reset_distance_, 3.0);
std::vector<std::string> clearable_layers_default, clearable_layers;
clearable_layers_default.push_back( std::string("obstacles") );
private_nh.param("layer_names", clearable_layers, clearable_layers_default);
for(unsigned i=0; i < clearable_layers.size(); i++) {
ROS_INFO("Recovery behavior will clear layer %s", clearable_layers[i].c_str());
clearable_layers_.insert(clearable_layers[i]);
}
initialized_ = true;
}
}
void ClearCostmapRecovery::runBehavior(){
ROS_WARN("Clearing costmap to unstuck robot (%fm).", reset_distance_);
//看好是把global，local的costmap的obstacle都清除掉
clear(global_costmap_);
clear(local_costmap_);
}
void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS* costmap){
{
//得到costmap中定義的所有layer
std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();
for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
{
if(clearable_layers_.count(name)!=0){
costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin);
clearMap(costmap, x, y);
}
}
}
void ClearCostmapRecovery::clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap,
double pose_x, double pose_y){
{
double start_point_x = pose_x - reset_distance_ / 2;
double start_point_y = pose_y - reset_distance_ / 2;
double end_point_x = start_point_x + reset_distance_;
double end_point_y = start_point_y + reset_distance_;
int start_x, start_y, end_x, end_y;
costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
for(int x=0; x<(int)costmap->getSizeInCellsX(); x++){
bool xrange = x>start_x && x<end_x;
for(int y=0; y<(int)costmap->getSizeInCellsY(); y++){
if(xrange && y>start_y && y<end_y)
continue;
int index = costmap->getIndex(x,y);
//注意重點
if(grid[index]!=NO_INFORMATION){
grid[index] = NO_INFORMATION;
}
}
}
double ox = costmap->getOriginX(), oy = costmap->getOriginY();
double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
costmap->addExtraBounds(ox, oy, ox + width, oy + height);
}
}
rotate_recovery
rotate_recovery::RotateRecovery默認的local planner是base_local_planner::TrajectoryPlannerROS ,如果使用別的local planner
你要具體的定義一些參數，具體參考這里
他的作用只是在原地旋轉，這樣costmap就會自己更新，mark或者clear一點的值
#The distance in radians between checks for obstacles when checking if an in-place rotation is safe. Defaults to 1 degree.
~<name>/sim_granularity (double, default: 0.017)
源碼
namespace rotate_recovery{
class RotateRecovery : public nav_core::RecoveryBehavior {
double sim_granularity_, min_rotational_vel_, max_rotational_vel_, acc_lim_th_, tolerance_, frequency_;
base_local_planner::CostmapModel* world_model_;
void RotateRecovery::initialize(std::string name, tf::TransformListener* tf,
costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap){
{
if(!initialized_){
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
//get some parameters from the parameter server
ros::NodeHandle private_nh("~/" + name_);
#默認的local planner，他的參數空間，要不是的話，要自己顯性設置下面那些參數
ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");
//we'll simulate every degree by default
private_nh.param("sim_granularity", sim_granularity_, 0.017);
private_nh.param("frequency", frequency_, 20.0);
blp_nh.param("acc_lim_th", acc_lim_th_, 3.2);
blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0);
blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4);
blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);
world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
initialized_ = true;
}
}
void RotateRecovery::runBehavior(){
ros::Rate r(frequency_);
ros::NodeHandle n;
ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
//下面就是看能不能在原地旋轉，不會碰到障礙物
vel_pub.publish(cmd_vel);
}
}
costmap_2d
使用分層的機制，每層的信息都加到costmap_2d::LayeredCostmap進行管理。
costmap layer
- static_layer
- obstacle_layer
- inflation_layer
- otherlayers
- Social Costmap Layer跟人行走，移動有關的
- Range Sensor Layer跟超聲波傳感器有關
Marking and Clearing
Each sensor is used to either mark (insert obstacle information into the costmap),
clear (remove obstacle information from the costmap), or both.
A marking operation is just an index into an array to change the cost of a cell.
A clearing operation, however, consists of raytracing through a grid from the origin of the sensor outwards for each observation reported.
Occupied, Free, and Unknown Space
注意這里說的是列，那是不是意味着它使用的voxel，3維存儲信息
Columns that have a certain number of occupied cells (see mark_threshold parameter)
are assigned a costmap_2d::LETHAL_OBSTACLE cost, columns that have a certain number of unknown cells (see unknown_threshold parameter)
are assigned a costmap_2d::NO_INFORMATION cost, and other columns are assigned a
costmap_2d::FREE_SPACE cost.
costmap更新的頻率由參數update_frequency確定。注意costmap嚴重依賴於tf的信息，要是tf太久沒有更新的話，navigation就會掛掉
這個時間tolerance是由參數transform_tolerance確定的
Map type
有兩種方法初始化costmap_2d::Costmap2DROS對象：
-
static map(map_server)
這種情況下，costmap初始化的長度寬度和static map的一樣,obstacle的信息也來自static map。
一般用在amcl定位導航中，隨着機器人的移動，使用傳感器的信息更新costmap -
rolling window
自己給一個長，寬值，設置
rolling_window參數為true,這個參數設置機器人的位置在costmap的中心，丟棄離機器人比較遠的obstacle信息。
一般用在在里程計坐標下的移動，機器人只關心在他周邊的障礙物信息。
注意提供plugin申明，還有在move_base node下面運行，不然有一些兼容性問題(可能是Hydro版本以前的初始化方式)
code
A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacles in either the form of PointCloud or LaserScan messages
namespace costmap_2d
{
class Costmap2DROS
{
Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf){
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
//發布costmap
publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
always_send_full_costmap);
timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);
map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
}
void Costmap2DROS::mapUpdateLoop(double frequency)
updateMap();
publisher_->publishCostmap();
}
void Costmap2DROS::updateMap()
{
if (!stop_updates_)
{
// get global pose
tf::Stamped < tf::Pose > pose;
if (getRobotPose (pose))
{
double x = pose.getOrigin().x(),
y = pose.getOrigin().y(),
yaw = tf::getYaw(pose.getRotation());
//對所有layer的costmap進行更新
layered_costmap_->updateMap(x, y, yaw);
}
}
}
}
}
namespace costmap_2d
{
class LayeredCostmap{
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
// Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
// implement thread unsafe updateBounds() functions.
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();++plugin)
{
double prev_minx = minx_;
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
}
int x0, xn, y0, yn;
costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
x0 = std::max(0, x0);
xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
y0 = std::max(0, y0);
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
if (xn < x0 || yn < y0)
return;
costmap_.resetMap(x0, y0, xn, yn);
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
//cost 值的更新，每個plugin都實現這個函數,是一份costmap在不同的plugin下的更新，costmap是一樣的，cost值是疊加上去的
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
}
}
}
}
namespace costmap_2d
{
class StaticLayer : public CostmapLayer
{
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map);
void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update);
}
}
namespace costmap_2d
{
class ObstacleLayer : public CostmapLayer
{
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void activate();
void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
https://zhuanlan.zhihu.com/p/21738718?refer=robotics-learning const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
double* max_x, double* max_y);
}
}
怎么計算每個costmap中的單元格的cost代碼還沒有研究