= 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
ROS wiki
代码文档
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