Local costmap update on navigation2

asked 2020-12-10 07:06:32 -0500

AlexKaravaev gravatar image

Hey there!

I am using navigation2 stack and have a robot that need to update it local costmap not via plugin(laserscan).

I want for it to place detected road lanes on the map as obstacles, however, with publishing to the topic /local_costmap/costmap_updates seems to be doing nothing.

Right now, I have a solution, to just publish whole costmap to /local_costmap/costmap, instead of just update and it works, but in that case, I will discard obstacles, that are detected from laser scan.

Here is how I populate OccupancyGridUpdate message


costmap_update_msg = OccupancyGridUpdate()

costmap_update_msg.header.frame_id  = self.frame_id
costmap_update_msg.header.stamp     = self.get_clock().now().to_msg()
costmap_update_msg.x = 0
costmap_update_msg.y = 0
costmap_update_msg.width  = self.size_x
costmap_update_msg.height = self.size_y

costmap_update_msg.data   = costmap_1d



Where costmap_1d is array with filled values. If I publish whole local costmap message filled with data as above, it publishes it correctly.

Probably it can be because I somehow populate this message wrong, because I did not found sufficient documentation on what is the x and y, I though it's the start of updated region.

And if there is way to do this combining of local costmap messages better, I will appreciate if you'll give a hint.

edit retag flag offensive close merge delete