Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Dynamic footprint support

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 global costmap to be reset. When digging into the code of the costmap_2d layer plugins, I discovered that setting the parameter footprint_clearing_enabled=false prevents this from happening, see this code section in obstacle_layer.cpp.

However, setting footprint_clearing_enabled=false is not what I want, as this causes these errors when navigation starts and the robot has not moved yet:

The origin for the sensor at (-0.00, 0.00, 0.00) is out of map bounds. So, the costmap cannot raytrace for it.

What do you suggest how to keep footprint_clearing_enabled but without resetting the costmaps each time?

Thanks

Dynamic footprint support

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 global costmap of the obstacle layer to be reset. When digging into reset, i.e. all obstacles that so far have been detected are deleted and the code of the costmap_2d layer plugins, I discovered that setting the parameter footprint_clearing_enabled=false prevents this from happening, see this code section in obstacle_layer.cpp.costmap is re-initialized empty.

However, setting footprint_clearing_enabled=false is not what Edit: Removed part of the original question text as I want, as this causes these errors when navigation starts and discovered that parameter footprint_clearing_enabled has nothing to do with the robot has not moved yet:

The origin for the sensor at (-0.00, 0.00, 0.00) is out of map bounds. So, the costmap cannot raytrace for it.

What do you suggest how to keep footprint_clearing_enabled but without resetting the costmaps each time?observed behavior.

Thanks

Dynamic footprint support

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.

Thanks

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