Cost map layer implementation

[AbBaSaMo]

Overview

Occupancy grids are a grid of the space around the vehicle that show spaces that the vehicle can move in and spaces the vehicle cannot move in. We have 2 occupancy grids produced by our system:

1. SLAM toolbox generated grid that shows obstacles detected by the LiDAR system
2. Lane detection generated grid that shows areas considered to be lanes or potholes

Nav2 requires a cost map (basically an occupancy grid with some differences) as an input so it can calculate where and how to drive but since we have 2 grids, we need to merge these. Luckily, Nav2 also provides a system by which we can merge grids as layers that are then concatenated and this task aims to implement these layers.

Relevant resources

• Writing a costmap plugin
• Costmaps
• More on costmaps
• Occupancy grids

This is what an occupancy grid looks like when visualised

Acceptance criteria

• The relevant ros topic that Nav2 outputs the cost map to must be populated with a costmap that when visualised when visualised in rviz, resembles the environment around the vehicle including obstacles, lanes and potholes if any.

Todo

• a new cost map layer must be implemented in addition to what was done last time [refer to @Jiawei-Liao]
• set up the vehicle in a spot outside on grass with some obstacles and lane lines spray painted
• launch sensors launch file with all 3 sensors hooked up
• launch nav2 or just the costmap layer component
• either record ros topics and visualise in the workshop or visualise on the spot somehow in rviz2

[Jiawei-Liao]

Following instructions from:
https://docs.nav2.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html
and
https://www.youtube.com/watch?v=X128gB2lVF0

Got this to work:

Seems to be overlaying nicely. Although I don't know why there seems to be 3 parts when there should only be 2.

There was this constant warning in terminal:

It didn't seem to make any trouble so hopefully we don't need to worry about it.

It also occasionally gave this message:

Not too sure why this is happening either.

Seems like a good start. We will be researching how to get occupancy grids from ros2 topics and using that instead of the gradient. We will be looking at static layer's code as that has a topic input for occupancy grids.

[Jiawei-Liao]

Nav2's static layer actually provides updating costmap functionality. The only difference is that it requires messages of type "OccupancyGridUpdate" from "map_msgs" instead of "OccupancyGrid" from "nav_msgs".

For testing, I have just passed it dummy data of a grid with a single line of obstacles. Here are some observations of the results:
Obstacles are indicated by bright pink. Kinda confusing since some parts that is clear is in pink/purple.
Inflation layer works on this.
Obstacles will remain there, unless something overwrites it.
Where the obstacle starts, which is currently at the center of this map is actually (200, 200). Not sure if that will change based on grid size. Kinda makes sense since each of those grey grid squares has a resolution of 20x20 occupancy grid squares.

https://drive.google.com/file/d/1Y_nrtleCLj4pSTPHsCZQyk6lNB0huH79/view?usp=sharing

@AbBaSaMo How would we be able to test this? Since the update message requires (x, y) to update from. Does the sensors provide current position (not sure what the 3 sensors are)? We would also need to change the zed topic publisher to use "OccupancyGridUpdate" messages as well as listen to current position.

[Jiawei-Liao]

Also just for reference, these transforms need to be run:
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link odom
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link map

[AbBaSaMo]

x and y of the robot? this would be provided by odom: robot localisation takes imu and gnss data and outputs odom https://docs.ros2.org/foxy/api/nav_msgs/msg/Odometry.html

[AbBaSaMo]

@Jiawei-Liao @Jokua

is there some parameter to specify a link or frame for it to be relative to? or does the message type have a frame attribute you can change?

[AbBaSaMo]

@Jokua @Jiawei-Liao industry night demo week 8, we want this configured with the esda sim by then too

[AbBaSaMo]

industry night demo will only use lidar

[AbBaSaMo]

also find the gazebo map of the igvc comp that anthony made and push it to the github. he might have it on his personal device

[Jiawei-Liao]

@Jiawei-Liao @Jokua

is there some parameter to specify a link or frame for it to be relative to? or does the message type have a frame attribute you can change?

I haven't noticed one when looking through the code. Although I have also noticed that it wasn't able to spawn an entity, which might be why the (0, 0) point is off center.

I was trying to look at why it gives a spawning error. Haven't found anything yet.

[Jiawei-Liao]

industry night demo will only use lidar

does this mean we don't need the zed layer?

[AbBaSaMo]

sorry for the late reply, we need the zed layer for the actual build but not industry night demo

[Jiawei-Liao]

@Jokua Instead of having another costmap on nav2 (I don't think this works?), you can try to fuse the occupancy grids of both slam and zed camera.

This node would:

• Listen for messages on slam and zed camera topics for changes in occupancy grid
• Listen for messages on odom (/odometry/global) and maybe tf in case there are some transforms that changes the robot
• Based on odom and tf, it would find where the robot is and where it is looking
• The maps can then be aligned (quaternion or some other method) and merged
• The new occupancy grid can then be published and used for nav2

To try it out, you could launch tb3 as that generates slam topic. You can just publish a dummy zed cam topic. Finally, launch rviz and view the combined occupancy grid topic to check if its right.