Ask Your Question
1

Clearing free space in costmap at a specific location

asked 2014-10-30 23:25:29 -0500

cdeste gravatar image

updated 2014-11-02 19:07:36 -0500

I'm trying to remove obstacles that are being detected with the laser and mark them as free space. Using a layer I am able to create obstacles that are included in the global costmap, but I don't seem to be able to remove them completely. I am guessing this is because of how the layers are merged - any lethal obstacle being added to the final costmap. Is there any way I can remove obstacles from the obstacle layer, or force free space from my layer to be dominant in the merging of layers?

xml file

<class_libraries>
  <library path="lib/libcustom_layer">
    <class type="custom_cost_namespace::CustomLayer" base_class_type="costmap_2d::Layer">
      <description>Obstacle adding and removing based on labels </description>
    </class>
  </library>
</class_libraries>

yaml file

plugins: 
- {name: static_layer, type: 'costmap_2d::StaticLayer'}
- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
- {name: custom_cost, type: 'custom_cost_namespace::CustomLayer'}
- {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
publish_frequency: 1.0
footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]

launch file

<rosparam file="$(find custom_cost)/config/custom_layer.yaml" command="load" ns="global_costmap" />

update costs method

void CustomLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
 min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  if(master_grid.worldToMap(x, y, mx, my))
    {
    if (remove)
      master_grid.setCost(mx, my, FREE_SPACE);
    else
      master_grid.setCost(mx, my, LETHAL_OBSTACLE);
    }
}
edit retag flag offensive close merge delete

Comments

Can you post your costmap configuration?

David Lu gravatar imageDavid Lu ( 2014-10-31 09:26:20 -0500 )edit

Hi David, I have added the xml and yaml files. Is that all you need?

cdeste gravatar imagecdeste ( 2014-11-02 16:48:44 -0500 )edit

How does your CustomLayer look like? Did you just copy some stuff from another layer? In the updateCost() function you can specify how the layer should update the final / master costmap. The update area normally is the same for all layers (parameters of updateCosts)

AReimann gravatar imageAReimann ( 2014-11-02 18:46:28 -0500 )edit

I have added my updateCosts function. I did just use the tutorial as an example.

cdeste gravatar imagecdeste ( 2014-11-02 19:08:38 -0500 )edit

With an additional layer, you can block the obstacle layer's lethal values from staying in the master costmap. However, it will not remove the value from the obstacle layer.

David Lu gravatar imageDavid Lu ( 2014-11-02 20:32:40 -0500 )edit

So would the configuration above block the obstacle layer? If so, could it be that it is not happening at the correct location? I have an x,y already in the map frame, and am concerned that the worldToMap might not be appropriate.

cdeste gravatar imagecdeste ( 2014-11-02 20:50:48 -0500 )edit

The tutorial layer does not save anything related to clearing so it does not overwrite the obstacle layer. Do you really need a custom layer? It would be easier if you just added the laser as observation source to the obstacle layer. This way the laser could clear thing in the obstacle layer.

AReimann gravatar imageAReimann ( 2014-11-02 21:09:31 -0500 )edit

You could make it kind of overwriting with commenting the lines 71 and 72 out in the custom layer cpp (the if (costmap_[index] == NO_INFORMATION) continue; thing). But this probably won't do what you actually want (if you have more than one sensor).

AReimann gravatar imageAReimann ( 2014-11-02 21:15:42 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2014-11-09 21:07:26 -0500

fergs gravatar image

I would suggest deriving your layer from either ObstacleLayer or VoxelLayer, so that you can internally update the costmap_ and voxel_grid_ that they use. That said, I'd also be wondering why you are clearing out laserscan data, which tends to be one of the most reliable/robust types of sensor data, is that really safe and/or the right thing to do?

edit flag offensive delete link more

Comments

The robot only has a planar laser and it is identifying traversable objects as obstacles in an outdoor environment.

When you say derive the layer - do you mean inherit from those layer classes? Would this mean there would be two obstacle layers?

cdeste gravatar imagecdeste ( 2014-11-10 22:15:20 -0500 )edit

Each layer is just a plugin (loaded by pluginlib). The only requirement then is that the layer inherit from a particular baseclass (in this case, Layer). So yes, you can inherit from the VoxelLayer, add your functionality, and then have a single layer take care of your laser plus your new features.

fergs gravatar imagefergs ( 2014-11-10 22:38:18 -0500 )edit

Thanks. I can now seem to clear the area in the local costmap, but the obstacle is still present in the global map. It doesn't seem to matter which map I load the layer into with the yaml file. Do you have any recommendations as to appropriate plugin layers for the two costmaps?

cdeste gravatar imagecdeste ( 2014-11-11 21:57:23 -0500 )edit

How are you clearing the area in the local costmap?

David Lu gravatar imageDavid Lu ( 2014-11-12 12:57:31 -0500 )edit
0

answered 2014-11-04 18:04:16 -0500

David Lu gravatar image

I think I understand your problem now.

Let's say you have some array of locations in the camera's reference frame that you believe to be free space, according to the camera.

Within your custom layer, you would first need to translate these points into the map's reference frame. This will give you the position of the points in METERS relative to the reference frame.

Then to figure out which grid cells they correspond to in the costmap, for each x and y in the map's reference frame, you'd call worldToMap(x,y,mx,my) to get mx and my, the coordinates of the grid cell in the costmap (integers).

Then, if you wanted to replace LETHAL costs from the obstacle layer with FREE_SPACE, in updateCosts, you should loop through all the points, get the mx and my and then say

if(master_grid.getCost(mx, my)==LETHAL_OBSTACLE){
     master_grid.setCost(mx, my, FREE_SPACE);
}
edit flag offensive delete link more

Comments

Additional discussion of the problem is here: https://github.com/ros-planning/navig...

David Lu gravatar imageDavid Lu ( 2014-11-09 20:53:44 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2014-10-30 23:25:29 -0500

Seen: 1,706 times

Last updated: Nov 09 '14