Add tutorial for ground consistency

[haider8645]

Hello @SteveMacenski ,

as discussed, please find the tutorial for ground consistency plugin. I have tested it on a freshly installed ubuntu with jazzy desktop installed on it. Could you give it a try as well to see if it is working for you.

Parameters used in demo:

  ground_consistency:
    plugin: "nav2_ground_consistency_costmap_plugin::GroundConsistencyLayer"
    ground_points_topic: /ground_segmentation/ground_points
    nonground_points_topic: /ground_segmentation/obstacle_points
    tf_timeout: 0.1
    min_clearance: 0.1
    robot_height: 0.92
    maximum_height_filter: 2.0
    ground_inc: 1.0
    nonground_inc: 1.5
    nonground_decay: 0.93
    ground_decay: 0.80
    max_score: 5000.0
    nonground_occ_thresh: 6.0
    nonground_prob_thresh: 0.75
    enable_kpi_logging: true
    discretize_costs: true
    max_data_range: 50.0
    ground_neighbor_search_cells: 0

I am open to any changes you would like to see in this tutorial.

Best regards,
Haider

[haider8645]

FYI: The tutorial has following dependencies.

NAV2 Documentation
Ground Consistency Layer Plugin
Ground Segmentation ROS2
KISS-ICP Odometry
Gazebo Simulator
Clearpath Husky Robot
ROS 2 Jazzy Documentation

I used KISS-ICP because the plugin maintains scores over a temporal window and a good odometry makes this possible. The built-in DiffDrive odomtry plugin in gazebo is not very accurate.. so better avoid it for such plugins.

I included this husky package COSTAR_HUSKY_SENSOR_CONFIG_1 in the demo package because I needed to modify it. The modification was enabling orientation samples in IMU data.

[haider8645]

Height Filter of Obstacle Points w.r.t Local Ground Height:

Robot Height: 0.92m

If we increase the height of the robot, we expect more of the canopy to be marked as obstacle.

Robot Height: 1.5m

\

Let's assume that the robot is even taller, standing at 3m, we expect most of the overhanging canopy to be marked obstacle. As can be seen in screenshot below.

Robot Height: 3m

[SteveMacenski]

This source code needs to be stripped down if possible. Can some or all of the gazebo assets (world, robot model, etc) be pulled in from apt packages and/or gazebo models on the registry?

Also, can this demo use Nav2 fully rather than replacing localization with kiss ICP? I think this is a nice use for a demo of just the layer, but this should be additive for Nav2 so its clear and concise how to integrate this into a full Nav2 stack. You should basically just add the layer to the Nav2 configuration and launch Nav2 as-usual (+ your ground segmentation package). So the full_stack launch file should launch Nav2 navigation (not just controller server) with a configuration file containing the layer + your ground segmentation package + simulation (check out how we do that in other Nav2 packages to follow that pattern). Remove the joystick stuff to simplify. You're welcome to replace KISS ICP from the Nav2 Localization launch file if you/want need to, as long as the rest of it is standard practice/concise.

I'll review the tutorial on your docs.nav2.org but I assume its the same as the README here so probably apply here as well :-)

[haider8645]

Can some or all of the gazebo assets (world, robot model, etc) be pulled in from apt packages and/or gazebo models on the registry?

The terrain model is pulled from gazebo assets. The robot model available on fuel does not give imu orientations so that needs to be modified. I can pull the robot model as well from the gazebo assets and then ask the user to make a small change in the model to send imu orientations. Would that work? I made a local copy of it to avoid to have the user change it but the change is just minimal here

Also, can this demo use Nav2 fully rather than replacing localization with kiss ICP?

Yes, ofc it can. I will do it

You should basically just add the layer to the Nav2 configuration and launch Nav2 as-usual (+ your ground segmentation package). So the full_stack launch file should launch Nav2 navigation (not just controller server) with a configuration file containing the layer + your ground segmentation package + simulation (check out how we do that in other Nav2 packages to follow that pattern)

Yes, sounds good. I will make the changes so that the full nav2 stack is launched.

Remove the joystick stuff to simplify.

Ok

[haider8645]

You're welcome to replace KISS ICP from the Nav2 Localization launch file if you/want need to, as long as the rest of it is standard practice/concise.

I will try out the nav2 localization options and come back to you.I understand and agree that it is better to keep this purely nav2 due to obvious reasons, it's for nav2 :D

[SteveMacenski]

The robot model available on fuel does not give imu orientations so that needs to be modified.

Can you explain this?

---

I'd actually say keep KISS ICP. Its a nice demo showing that as well in a way.

[haider8645]

The robot model available on fuel does not give imu orientations so that needs to be modified.

Can you explain this?

The sdf file of the husky model on gazebo fuel has an imu_sensor but by default the imu_sensor has the following:

<enable_orientation>0</enable_orientation>

This makes the orientation part of the IMU message invalid (all zeros). I had to set this flag to 1 to get valid orientation values in the IMU msg.

You may say that why use husky after all... but I kind of like it over turtlebot :D

[SteveMacenski]

Why do you need the orientation from the IMU? I guess that's my question

[haider8645]

Why do you need the orientation from the IMU? I guess that's my question

Yeah so the IMU is actually optional in my ground segmentation, but it becomes quite important once you deal with uneven terrain or slopes.

The basic idea is that the algorithm fits planes to local point cloud patches (in grid cells) to estimate the ground slope. Now the problem is: the point cloud is in the robot frame. So if the robot is tilted (like going uphill), the whole cloud is also tilted with it.

If you don’t take the robot orientation into account, the algorithm kind of “thinks” the world is tilted, not the robot. That can lead to weird effects, like flat ground behind the robot suddenly being classified as an obstacle, just because it looks angled in the sensor frame.

With the IMU, you can get the robot orientation and you can rotate the point cloud back into a gravity-aligned frame, so the slope estimation actually reflects the real terrain and not the robot pose.

That said, it still works without IMU (it’s configurable), and on flat terrain you probably won’t notice much difference. But on slopes it can definitely degrade and give some false positives.