Dynamic footprint support

asked 2017-02-09 08:12:33 -0500

Huibuh gravatar image

updated 2017-02-23 08:28:19 -0500

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

edit retag flag offensive close merge delete

Comments

Can you clarify what you mean by reset?

David Lu gravatar imageDavid Lu ( 2017-02-12 20:33:04 -0500 )edit

Changed the original question, see above.

Huibuh gravatar imageHuibuh ( 2017-02-23 08:16:16 -0500 )edit

Hi, I realize this point is a bit older, but I've been looking into making some parameters of the navigation stack dynamic, is there anywhere I could look at your solution? Thanks!

frescho gravatar imagefrescho ( 2019-07-25 08:19:54 -0500 )edit