Dynamic footprint support

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

Huibuh gravatar image

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

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:

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))
        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


Can you clarify what you mean by reset?

David Lu gravatar image David Lu  ( 2017-02-12 20:33:04 -0600 )edit

Changed the original question, see above.

Huibuh gravatar image Huibuh  ( 2017-02-23 08:16:16 -0600 )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 image frescho  ( 2019-07-25 08:19:54 -0600 )edit