Fix voxels not working in ROS2 navigation

[DatSpace]

Voxels are not used by ROS2 Navigation2 with the warning message being:

Sensor origin out of map bounds. The costmap can't raytrace for it.

It is probably a configuration issue.

[DatSpace]

The voxel layer doesn't seem to work in the real robot either. Even though the pointcloud looks fine and a /local_costmap/voxel_grid topic exists. The voxel grid topic has published data with what seems to be a correct timestamp. Rviz does not complain about anything.

In sim vs real nav2 complains for different things. In sims it seems the camera is out of bounds (possibly the tf is wrong). In real it complains that the camera tf (extra_camera_tf) is earlier than any other tf data known. It has a similar message for mobile_double_lidar tf.

The clocks on the computers are synchronized and everything (except the visualization) was running on the robot. The visualization should not affect anything as not even the costmap had the voxel obstacles.

Could it be a delay between the tf given by yarp from the pointcloud and a delay given by ros for the rest of tf ? It seems to be around 0.3-0.5 secs.

[DatSpace]

Simply put the voxel layer now works. It was a configuration problem and a node missing that was not part of the costmap but needs to be run separately. It is now part of the navigation launch file.

There are 2 extra issues:

1. The voxels work in the real robot but the correct tf needs to be specified in the pointCloudCreator config as we had changed it in the past during some demos.
2. The simulations still shows some warnings about not being able to raytrace but that is also a configuration problem to be resolved.

[DatSpace]

The simulation doesnt seem to show any warnings any more. The pointcloud is there and the voxel marker node is working but saying it publishes 0 voxels.

Looking at the costmap voxel_grid topic it seems all of the grid is populated with values: 65535.