How to clear older costmap just before updating the map?

asked 2011-06-27 09:28:01 -0600

288 ●10 ●15 ●24 http://kadiruyanik.wor...

updated 2011-06-27 21:39:08 -0600

Hi,

What I'm trying to do is putting a rolling window on top of each robot (5 robots in total), then inflating the points representing each obstacle near to the robot of interest as soon as a new sensor data is received, but the map will be reset after a new sensor data comes (clearing).

Here are some of the important parameter settings:

nh.setParam ("pointcloud/observation_persistence", 0.0);
nh.setParam ("pointcloud/expected_update_rate", 0.080);
nh.setParam ("pointcloud/clearing", true);
nh.setParam ("pointcloud/marking", true);
nh.setParam ("pointcloud/obstacle_range", 5.0);
nh.setParam ("static_map", false);
nh.setParam ("rolling_window", true);
nh.setParam ("width", 1);
nh.setParam ("height", 1);
nh.setParam ("obstacle_range", 1.0);
nh.setParam ("raytrace_range", 1.0);

When I run the simulation, everything seems to be properly set, but somehow obstacles are stored, and they stay there all the time. I've read costmap doc and nav tutorials many times, I checked if the parameters are set, I debugged the costmap and observation buffer source codes and verified that buffer is actually cleared(since observation_persistence is set to 0.0).

I thought making the sensor as "marking+clearing" will help it to overwrite already obtained data automatically. But since it is set as rolling_window with the width and height of 1 meter, sensor couldn't clear the inflated regions since the origin of the sensor becomes outside of the window. "The origin for the sensor at (0.00, 0.00) is out of map bounds. So, the costmap cannot raytrace for it".

Having set the raytrace_range 1.0 as in the case of obstacle_range didn't change anything. I was hoping that somehow it will be handled automatically that the sensor data inside the rolling window will be updated and cleared automatically.

Then, why still see the traces of the dynamic objects, as it is shown in this video?

In this video, there are 5 static yellow robots, and five dynamic blue robots, one of which is creating a costmap for itself, and the point clouds being sent to the costmap is shown with red, orange points are obstacles, green regions are the inflated obstacles.

Many thanks in advance..

edit retag flag offensive close merge delete

add a comment

1

answered 2011-06-27 21:37:49 -0600

Kadir Firat Uyanik

288 ●10 ●15 ●24 http://kadiruyanik.wor...

updated 2011-06-27 21:40:57 -0600

Hi Eitan,

Current system is very slow since costmap modules -I suppose- were not designed to consider a fully observable, offboard sensing and computing multi-agent platform like this one.

To be more hacky about clearing the costmap, I dig into the costmap source code. Since the current system with five controllable nodes can achieve at most 30Hz refresh rate, which is half of what it is supposed to be, and we cannot call a service due to the problem you've mentioned, and it will not be that reasonable to make the system spend extra effort on clearing operations, I will clear the map at each updateMap operation.

It seems Costmap2DROS::resetMapOutsideWindow method does what I'ld like to do. That is, having a rolling window on top of each robot and considering only the latest sensor observation but discarding whole history, clearing everything at each iteration.

But I don't want to crash other things in dwa_local_planner. Could you please guide me what is the most proper line that I've to insert resetMapOutsideWindow.

My initial solution is in costmap_2d_ros.cpp line 653:

costmap_->resetMapOutsideWindow(wx, wy, 0, 0);
costmap_->updateWorld(wx, wy, observations, clearing_observations);

It did work indeed, please see this video, but I'm not sure if it's gonna cause any trouble in dwa_local_planner or other parts of the ros::navigation software that I use for this application.

Thanks a lot for your assistance..

edit flag offensive delete link

Comments

I assume you are computing the obstacles directly from the robots. If you know the previous/old locations you could use eitan's second option, but only fake clearing information on the old ones, not the full grid. That might be more efficient.

dornhege ( 2011-06-27 22:03:27 -0600 )edit

1

Hi, Kadir! I'm struggling with the same issue. And I'm not sure when this two functions should be called? In the beginning of mapUpdate() function?

Peter Listov ( 2012-12-04 23:28:01 -0600 )edit

add a comment

2

answered 2011-06-27 15:07:54 -0600

eitan
2743 ●16 ●18 ●36

In order for the costmap to clear obstacles, it must be presented with a "clearing" observation for the previously occupied space. Typically, these observations come from an on-board sensor, such as a laser, from which ray-tracing is performed to clear space. From the video you posted, it looks like you're getting information for the robots' positions from an external sensor. To clear space with an external sensor, you have a couple of options, though they're sort of hacky:

If you're running navigation 1.3.1 or greater, you can call the "clear_costmaps" service which will clear all obstacles from the costmap. Then, upon receiving a new observation from your obstacle sensor, only those obstacles will be put into the map. One thing to note here, however, is that for the period of time between making the service call and receiving a new observation the robot will have an empty costmap and could hit something. This option would be super easy to try out, but does have the caveat of being able to run into something if you're not careful about how you run things.
Create a node that publishes clearing information in the robot's base_frame. Basically, you'd be faking a laser with a 360 degree field of view. The fake laser won't clear out observations that are actively being asserted by your obstacle detection, but it will clear out stale obstacles. This option is fairly easy to hack together, and doesn't have the race condition above that could lead to an empty map, but remember to set the frame_id of the LaserScan or PointCloud message correctly as otherwise you'll get the "outside of map bounds..." error you referenced above.

Hope this helps.

edit flag offensive delete link

Comments

Hello. I know that this answer was a lot of time ago, but I have the same issue on ROS Kinetic. I would like to call the clear costmap service at the beggining of each marking process but I don't know the way to do it.

I opened this question if you want to answer me there.

Best regrets.

Alessandro Melino ( 2020-03-27 05:40:11 -0600 )edit

add a comment

1

answered 2011-06-28 04:11:52 -0600

eitan
2743 ●16 ●18 ●36

That hack should do what you want, it'll make sure to clear the costmap of all obstacles before putting observations into the map, and I don't think it should cause any trouble with the local planner. When you say the current system is slow, I'm assuming that you're talking about performing raytracing operations for fake sensors on all five robots? Perhaps a parameter should be added to the costmap to allow for clearing completely every cycle to support this use case.

edit flag offensive delete link

add a comment

How to clear older costmap just before updating the map?

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

How to clear older costmap just before updating the map? edit

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

How to clear older costmap just before updating the map?