ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

[ROS2][Nav2] Is there a way to expand the global costmap ?

asked 2022-07-08 08:15:14 -0500

Bastian2909 gravatar image

updated 2022-07-10 10:03:50 -0500

ljaniec gravatar image

I am working in ros2 foxy in ubuntu 20.04 with a custom made robot.

In my current setup i use an imu and odmotry (produced by diff_drive) and a localisaton system outside of the robot (like gps). My goal is to map an environment using only the footprint of the robot ( as i don't have any other source of information ). To do that, the only way i found was to publish a large empty static map using a map server as global costmap and have the robot move around. This leaves a tray of "free cell" in my global costmap. I then go around the area i want to map and then save it and fill the inside with a python script. This leaves me with a map containing "free cells" and "unnown cells".

The only issue I have with that is that I don't know how to expand my global costmap : If my robot goes to far, it's radius won't be "printed" on any map and won't be saved.

Is there a way to expand the global costmap in a more dynamic way ?

my nav2 global costmap config :

global_costmap:
  global_costmap:
    ros__parameters:
      publish_frequency: 1.0
      update_frequency: 5.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: False
      robot_radius: 0.5
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

I use an obstacle layer in the config file but I don't have anything publishing on the /scan topic, I just use this layer to "print" the footprint of my robot on the map.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-07-08 19:07:29 -0500

The static layer will size the costmap strictly speaking to its size - hence the word Static. That will resize the layered costmap to match its size when a new map comes in https://github.com/ros-planning/navig... by design.

You don't need to use that though, you can just set the size of the costmap to use as a parameter and then that's used to size the space (so you don't need to publish a giant empty map on the map server; just set the size of the costmap in yaml to the size you'd like). Think about it like the local rolling costmap; if you set it to 3x3, then that's the size of the costmap. Instead set a non-rolling costmap of whatever size you like, preferably just larger than you know the space would realistically be so that you can explore without a problem.

If you genuinely have no idea and cannot make even a best guess of what the size is of a space, you could conceptually build a new costmap layer that is an "exploration layer" that every time you get close to an edge, extends the map in that direction using the same logic that the Static layer uses, but that's not a default provided capability. Generally speaking you're either (1) using a pre-made map which is the space you care about (2) using SLAM to generate a map on the fly so that is being fed into the costmap to expand the explored space in the Static layer. If you're not doing either of those, you'd need to create a layer to explore OR some logic that would send a new sized empty map to the Static layer (analogous to (2)) to resize the space if you want to be a little hacky about it.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-07-08 08:15:14 -0500

Seen: 743 times

Last updated: Jul 10 '22