Local costmap for robot with obstacles

[Victorsoeby]

I am trying to implement a corner of the nav2 stack on a robot. What i am interested in, is using the costmap_2d package, to keep track of incoming obstacles in a local costmap, by rolling it infront of the robot.

Currently, i am using the nav2_costmap_2d node, and running it with:

ros2 run nav2_costmap_2d nav2_costmap_2d --ros-args -p rolling_window:=true -p height:=5 -p width:=5 -p resolution:=0.1 -p global_frame:=odom

This correctly spawns a costmap for me, that i can visualize in RVIZ. I can also call the /costmap/costmap_updates to insert values into the map, which then changes color of the cells in rviz. My problem is that whenever the costmap updates, all the cells that i have filled through the update topic disappears. Furthermore, when i echo the /costmap/costmap_raw it always displays 0, 0, 0, 0 etc. in data, never reflecting that i have actually updated the costmap. I have tried to enable that with setting always_send_full_costmap to true, but this does not work either.

How can i get the costmap to function properly?

[SteveMacenski]

I can also call the /costmap/costmap_updates to insert values into the map, which then changes color of the cells in rviz. My problem is that whenever the costmap updates, all the cells that i have filled through the update topic disappears

Costmap updates are updates from the costmap to rviz. When you publish to that topic, you're not actually updating the costmap at all, you're just displaying your own message in rviz. The costmap has its values updated either by the local server that hosts the costmap object (which you don't have, by launching it independently) or via the costmap layer plugins. These are algorithm that sit inside of the costmap object that subscribe to sensor data or otherwise to update the costmap's values.

So, I think you need to make a "real" config file for your costmap launching to give it your CLI settings + add in other plugins and other settings to get it behave the way you like. You could, for instance, use the StaticLayer to take in a nav_msgs/OccupancyGrid message (kind of like what you're doing right now with the updates) with your information for Costmap to store. Though, if you already have that and you're not fusing it with other sensors / layers, I'm not sure what the purpose of Costmap2D has for you. If you're going to add in other sources, like sensor data, then costmap and its layer plugins are the right answer for you.

Check out our docs:

• Concepts: https://navigation.ros.org/concepts/index.html
• Costmap configuration guide: https://navigation.ros.org/configuration/packages/configuring-costmaps.html
• A list of officially supported layers: https://navigation.ros.org/plugins/index.html#costmap-layers
• Tutorial to create your own layers, if need be (also might be helpful just to read through it quickly to give you an understanding of how the layers work): https://navigation.ros.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html

[Victorsoeby]

Hi @SteveMacenski.

Thanks for your answer. Using the plugin layers to update the actual costmap makes sense, as i will be adding more sensors afterwards.
Looking at the code of the static_layer it looks to be doing exactly what i actually want: Listening to the update topic and updating the costmap. What is the quickest way for me to add this plugin to do some testing? I have seen the gigantic config files in the nav2 bringup package, but i am just interested in running the costmap part of nav2 with my robot. Should i modify the costmap2d node and apply the logic there, or can it be run in conjunction with nav2_costmap_2d somehow? The costmap node that is spawned has multiple configurable rosparams, but as far as i remember nothing for the layers. I am new to nav2, and it is unsure for me what can be run independently, as the costmap package doesn't demonstrate this.

[SteveMacenski]

Obviously add / subtract settings and adjust to the appropriate node name you're using, but this is more or less what you should use as a base: https://github.com/ros-planning/navigation2/blob/main/nav2_bringup/params/nav2_params.yaml#L139-L179

[Victorsoeby]

Alright. But do i need to do this from the nav2 bringup package, or should i be able to start the layers with the default nav2_costmap_2d node included in the nav2_costmap package without modifying it? When i get back to my setup i will make my paramsfile, and launch my costmap like this:

´´´
ros2 run nav2_costmap_2d nav2_costmap_2d --params-file params.xml
´´´

What still confuses me is how the actual plugin layers are launched, as i don't see anything about them in the .cpp file which launches the costmap_2d node: https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/src/costmap_2d.cpp
Including them and specifying their rosparams in the param file that you sent me won't actually bring them up. Right?

In my head i would need to write my own rosnode that utilizes the staticlayer plugin, and bring that up with the costmap node.

[SteveMacenski]

They're just parameters, treat it as you wish using standard ROS parameter APIs in CLI or launch or what have you