Different maps for navigation and path planning

classic Classic list List threaded Threaded
8 messages Options
Reply | Threaded
Open this post in threaded view
|

Different maps for navigation and path planning

Christian Verbeek
 Is it possible to have a map used by the path planner that differs from
the map that is used by gmapping for localization? The idea is to mark
certain areas as forbidden so that the robot never drives through these
areas.

My understanding right now is that if I mark a cell as occupied this
information would also be used by gmapping. If I mark to many cells
manually this would lower the localization performance.

Regards
Christian

_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users
Reply | Threaded
Open this post in threaded view
|

Re: Different maps for navigation and path planning

Christian Verbeek
 Ok, here is my solution to this problem:

I modified
/opt/ros/boxturtle/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
line 204 listening not for "map" but for "planner_map".

Then I set up a second map server remapping /map to /planner_map loading
my modified map.

Thats it.

_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users
Reply | Threaded
Open this post in threaded view
|

Re: Different maps for navigation and path planning

Adam Stambler
Hi Christian,

You had the right idea, but there is an easier way.  With ROS, topics names, node names, and parameters names can all be remapped to other names.  You can do this because the nodes query the master for the names value.

So you can remap map to planner_map in the launch file by doing 
<remap from="map" to="planner_map"/>

There is more info on the wiki.  It is a really useful thing to know as you will have to do a bunch of modifications like this to glue together the various ros components you are using.


Regards,
Adam

On Mon, Sep 6, 2010 at 12:12 PM, Christian Verbeek <[hidden email]> wrote:
 Ok, here is my solution to this problem:

I modified
/opt/ros/boxturtle/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
line 204 listening not for "map" but for "planner_map".

Then I set up a second map server remapping /map to /planner_map loading
my modified map.

Thats it.

_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users


_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users
Reply | Threaded
Open this post in threaded view
|

Re: Different maps for navigation and path planning

Christian Verbeek

> So you can remap map to planner_map in the launch file by doing
> <remap from="map" to="planner_map"/>
>
Adam,

I am using the move_base node. Remapping would then also apply to amcl.

_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users
Reply | Threaded
Open this post in threaded view
|

Re: Different maps for navigation and path planning

Brian Gerkey-2
In reply to this post by Christian Verbeek
On Mon, Sep 6, 2010 at 5:57 AM, Christian Verbeek
<[hidden email]> wrote:
>  Is it possible to have a map used by the path planner that differs from
> the map that is used by gmapping for localization? The idea is to mark
> certain areas as forbidden so that the robot never drives through these
> areas.
>
> My understanding right now is that if I mark a cell as occupied this
> information would also be used by gmapping. If I mark to many cells
> manually this would lower the localization performance.

hi Christian,

Yes, you can supply a different map to different parts of the system,
by using standard ROS name remapping. E.g., bring up a second map
server, and remap its outputs and move_base's inputs to match:

  <node name="map_server_planner" pkg="map_server" type="map_server"
args="path-to-planner-map.yaml">
   <remap from="map" to="map_planner"/>
   <remap from="map_metadata" to="map_metadata_planner"/>
   <remap from="static_map" to="static_map_planner"/>
  </node>
  <node pkg="move_base" type="move_base" name="move_base_node" output="screen">
    <remap from="map" to="map_planner"/>
    <other remappings / parameter loads.../>
  </node>

A more compact alternative is to push the second map_server down into
a namespace, then remap only move_base's input:

  <node ns="map_planner" name="map_server_planner" pkg="map_server"
type="map_server" args="path-to-planner-map.yaml"/>
  <node pkg="move_base" type="move_base" name="move_base_node">
    <remap from="map" to="map_planner/map"/>
    <other remappings / parameter loads.../>
  </node>

        brian.
_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users
Reply | Threaded
Open this post in threaded view
|

Re: Different maps for navigation and path planning

Eric Perko
In reply to this post by Christian Verbeek
Christian,

I did exactly what you are trying to do just the other day and was able to successfully get it to run with one map for AMCL and another for the navigation stack. Launch file attached.

We then ran into a separate problem, where the LIDAR in the navigation stack will clear out our static obstacles if it cannot sense them (glass walls or your 'forbidden zones'), as the static map seems to only be used to seed the costmaps, not continually make sure that they contain at least the static map. It also does not get assigned a height when using the voxel grids, so we were unable to just separate the static map and LIDAR into two different planes to prevent cross-clearing. Have you encountered this problem with LIDARs clearing your static_map forbidden zones? Also, do you load the static map into the costmap for the local_planner as well and if not, how do you give the local_planner knowledge of the forbidden zones? We are probably going to address these by creating a PointCloud sensor that reads in maps and can then be fed into the costmaps just like any other sensor. I'm interested to see if you came up with a different plan.

- Eric

On Mon, Sep 6, 2010 at 12:26 PM, Christian Verbeek <[hidden email]> wrote:

> So you can remap map to planner_map in the launch file by doing
> <remap from="map" to="planner_map"/>
>
Adam,

I am using the move_base node. Remapping would then also apply to amcl.

_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users


_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users

start_tour_guide_nav.launch (1K) Download Attachment
Reply | Threaded
Open this post in threaded view
|

Re: Different maps for navigation and path planning

Eitan Marder-Eppstein
Eric,

You're right. Right now there's not a great way to specify a "forbidden zone" in a map. If you specify the space as occupied, then sensors can clear it out when it is observed to be free. I've always intended on adding a different cost value that designates space as forbidden no matter what local sensor information says, but honestly just haven't gotten around to it. Since this seems like a critical thing for you, and something that a lot of people will want, I've created a ticket for it here: https://code.ros.org/trac/ros-pkg/ticket/4409.

In the meantime, as you alluded to, you can create a marking sensor that asserts that the "forbidden" region is seen as occupied and hook it up to the costmap. This will keep the LIDAR from clearing it out when the robot is close to it. It isn't quite as easy as drawing on a bitmap, but it should solve your problem. You can also have that sensor operate at a different height than your LIDAR in the voxel grid version of the costmap which means you'll only have to publish information about forbidden regions on startup. If you do create the sensor that reads in maps to create forbidden zones, it might be nice to share it with the ROS community, as the cost value fix likely won't be released until D-Turtle.

Come to think of it, the sensor approach is actually much better for dealing with the local planner's costmap which has no knowledge of the global static map. I'll have to think a little on the best way to deal with that with the special cost value approach.

Hope this helps,

Eitan

On Mon, Sep 6, 2010 at 10:41 AM, Eric Perko <[hidden email]> wrote:
Christian,

I did exactly what you are trying to do just the other day and was able to successfully get it to run with one map for AMCL and another for the navigation stack. Launch file attached.

We then ran into a separate problem, where the LIDAR in the navigation stack will clear out our static obstacles if it cannot sense them (glass walls or your 'forbidden zones'), as the static map seems to only be used to seed the costmaps, not continually make sure that they contain at least the static map. It also does not get assigned a height when using the voxel grids, so we were unable to just separate the static map and LIDAR into two different planes to prevent cross-clearing. Have you encountered this problem with LIDARs clearing your static_map forbidden zones? Also, do you load the static map into the costmap for the local_planner as well and if not, how do you give the local_planner knowledge of the forbidden zones? We are probably going to address these by creating a PointCloud sensor that reads in maps and can then be fed into the costmaps just like any other sensor. I'm interested to see if you came up with a different plan.

- Eric


On Mon, Sep 6, 2010 at 12:26 PM, Christian Verbeek <[hidden email]> wrote:

> So you can remap map to planner_map in the launch file by doing
> <remap from="map" to="planner_map"/>
>
Adam,

I am using the move_base node. Remapping would then also apply to amcl.

_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users


_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users



_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users
Reply | Threaded
Open this post in threaded view
|

Re: Different maps for navigation and path planning

Christian Verbeek

>
> You're right. Right now there's not a great way to specify a
> "forbidden zone" in a map. If you specify the space as occupied, then
> sensors can clear it out when it is observed to be free. I've always
> intended on adding a different cost value that designates space as
> forbidden no matter what local sensor information says, but honestly
> just haven't gotten around to it. Since this seems like a critical
> thing for you, and something that a lot of people will want, I've
> created a ticket for it
> here: https://code.ros.org/trac/ros-pkg/ticket/4409.
>
> In the meantime, as you alluded to, you can create a marking sensor
> that asserts that the "forbidden" region is seen as occupied and hook
> it up to the costmap. This will keep the LIDAR from clearing it out
> when the robot is close to it. It isn't quite as easy as drawing on a
> bitmap, but it should solve your problem. You can also have that
> sensor operate at a different height than your LIDAR in the voxel grid
> version of the costmap which means you'll only have to publish
> information about forbidden regions on startup. If you do create the
> sensor that reads in maps to create forbidden zones, it might be nice
> to share it with the ROS community, as the cost value fix likely won't
> be released until D-Turtle.
>
> Come to think of it, the sensor approach is actually much better for
> dealing with the local planner's costmap which has no knowledge of the
> global static map. I'll have to think a little on the best way to deal
> with that with the special cost value approach.
Eitan, Eric,

The solution to my problem was much easier. I simply removed the
rangefinder from the global costmap configuration. So my global map is
really static and the forbidden zones are not cleared out any more. The
working environment of my robot is highly structured so its reasonable
to simply stop the robot when it can not follow the global path and ask
someone to either remove the obstacle or to recreate the map.

But of course this is a simple work around.

_______________________________________________
ros-users mailing list
[hidden email]
https://code.ros.org/mailman/listinfo/ros-users