Costmap Clear Operation Not Working

edit

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?