Costmap Clear Operation Not Working
I've posted this question https://github.com/ros-planning/navig.... Reposting here as well.
I've been trying to clear the costmap as a recovery behavior. First I tried with the default one with no success. Now I'm trying to debug clear costmap recovery package.
First of all, I changed the layer name from obstacle to obstacle_layer. I've added some debug messages to ensure nothing's blocking the code execution. For example, I've added ROS_INFO as below.
void Recovery::runBehavior()
{
if (!initialized_){
ROS_ERROR("Object needs to be initialized first");
return;
}
if (local_costmap_ == NULL){
ROS_ERROR("The costmap should not be null");
return;
}
ros::WallTime t0 = ros::WallTime::now();
ROS_INFO("behavior just started to clear the costmap");
clear(local_costmap_);
if (force_updating_)
{
local_costmap_->updateMap();
ROS_INFO("Costmap should be updated.. please check!");
}
}
void Recovery::clear(costmap_2d::Costmap2DROS* costmap)
{
std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();
geometry_msgs::PoseStamped pose;
if(!costmap->getRobotPose(pose)){
ROS_ERROR("Cannot clear map because pose cannot be retrieved");
return;
}
double x = pose.pose.position.x;
double y = pose.pose.position.y;
for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
std::string name = plugin->getName();
int slash = name.rfind('/');
if( slash != std::string::npos ){
name = name.substr(slash+1);
}
ROS_INFO("Reading %s layer", name.c_str());
if(clearable_layers_.count(name)!=0){
ROS_INFO("Will clear %s layer", name.c_str());
boost::shared_ptr<costmap_2d::CostmapLayer> costmap;
costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin);
clearMap(costmap, x, y);
}
}
}
void Recovery::clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap,
double pose_x, double pose_y){
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
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);
costmap->clearArea(start_x, start_y, end_x, end_y);
ROS_INFO("Will clear area %d %d %d %d",start_x, start_y, end_x, end_y);
double ox = costmap->getOriginX(), oy = costmap->getOriginY();
double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
costmap->addExtraBounds(ox, oy, ox + width, oy + height);
return;
}
Everything gets printed on the console but the costmap is not cleared. Btw, I can clear my costmap with rosservice call. So, I'm assuming there's something wrong in the code?