Dynamic footprint support

edit

Hello @David Lu

My robot runs on four tracks whose angle can be adjusted during runtime. To account for the change of the robots' footprint, I use the dynamic recunfigure callback mechanism by calling the /move_base/global_costmap/set_parameters and /move_base/local_costmap/set_parameters services, passing a new footprint string.

My problem is that each footprint change while move_base navigation is active causes the costmap of the obstacle layer to be reset, i.e. all obstacles that so far have been detected are deleted and the costmap is re-initialized empty.

Edit: Removed part of the original question text as I discovered that parameter footprint_clearing_enabled has nothing to do with the observed behavior.

As I discovered myself by now, the reason for the observed behavior is that by using the dynamic reconfigure mechanism to adjust the footprint during runtime is that the callback Costmap2DROS::reconfigureCB() each time it is executed first deletes and then re-instantiates the mapUpdateLoop thread:

map_update_thread_->join();
delete map_update_thread_;
...
map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));

Hence, the content of each layer is reset. The solution I have come up with is to add a DynamicFootprint service to Costmap2DROS, that only updates the footprint:

bool Costmap2DROS::cbDynamicFootprint(DynamicFootprint::Request &req, DynamicFootprint::Response &res)
{
  if (req.footprint_str != "")
  {
    std::vector<geometry_msgs::Point> new_footprint;
    if (makeFootprintFromString(req.footprint_str, new_footprint))
        setUnpaddedRobotFootprint(new_footprint);
    else
        ROS_ERROR("Invalid footprint string from dynamic reconfigure");
  }    
  return true;
}

This seems to be working, and allows me to adjust the robot footprint during runtime without loosing the costmap data. Do you have a better idea? Is it worth creating a pull request so others can benefit form this change, too?

Regards, Heiko