Use with Nav2

[gglaspell]

What is the best way to use this package with Nav2? I believe that bt_navigator requires an odom topic, but did not see one generated from 2fast2lamaa. Any guidance or suggestions would be greatly appreciated.

[clegenti]

Hi, I never used Nav2. If you can make sure what topic is needed, we can look at publishing the required information from the various nodes.

[gglaspell]

In the Nav2 documentation, I found this:
In addition to the required odom => base_link transform, Nav2 also requires the publishing of nav_msgs/Odometry message because this message provides the velocity information of the robot.

Link:
https://docs.nav2.org/setup_guides/odom/setup_odom.html

Being able to incorporate 2fast2lamaa with Nav2 would be awesome! While the nav_msgs/Odometry is required, another nice feature for use in Nav2 is a 2D occupancy grid for global path planning. However, other packages, such as octomap-server, can be used to generate the 2D occupancy grid.

[clegenti]

Cool. I'll try to add the odometry messages by the end of the week.
Regarding the path planning, I'll have limited time to code any new feature (at least for the coming months). But I am happy to provide help and insight to anyone that is willing to interface path planning with 2fast2lamaa :)

[gglaspell]

@clegenti Thank you!!!! I'll definitely keep a look out for the odom topic update!!

[clegenti]

I have added the publication of an odometry topic. The visualisation seems to make sense for the pose but I don't have a quick way to check if the twist is OK. Let me know if that's good enough.

Additionally, the nature of 2Fast2Lamaa combines a local and global registration that are somewhat "independent", thus the odometry might be a bit "jumpy". If that's an issue we can think about addressing it :)

[gglaspell]

I saw that you posted yesterday and couldn't wait to try it. I did some initial testing in simulation and it looks great!! The image below shows the /odom_map_correction topic compared with /ground_truth. Overall it matches quite well. Also, it was a short test, but I did nott notice any jumping.

In regard to the twist component, I echoed each topic and for comparison as well. I was mainly looking to see if the signs matched and it looks like they do.

If it helps, this was the output of colcon build.

--- stderr: ffastllamaa                                                                              
/Humble/ros2_ws/src/2fast2lamaa/src/lib/state.cpp: In member function ‘std::pair<Eigen::Matrix<double, 3, 1>, Eigen::Matrix<double, 3, 1> > State::queryTwist(double, const Vec3&, const Vec3&, const Vec3&, const Vec3&, double) const’:
/Humble/ros2_ws/src/2fast2lamaa/src/lib/state.cpp:419:17: warning: unused variable ‘p0’ [-Wunused-variable]
  419 |     const Vec3& p0 = std::get<0>(state_pose.at(state_id));
      |                 ^~
/Humble/ros2_ws/src/2fast2lamaa/src/lib/state.cpp:420:17: warning: unused variable ‘p1’ [-Wunused-variable]
  420 |     const Vec3& p1 = std::get<0>(state_pose.at(state_id+1));
      |                 ^~
---

I plan to start testing in Nav2 tomorrow. Specifically using octomap_server to generate the 2D occupancy grid or maybe just do a rolling global costmap. I'll post the results here as soon as I do.

On an unrelated note, I just wanted to say meshing feature is very nice. From what I could tell the normals are rendered correctly to prevent blobbing. Also, using screen psr makes it really fast.

Thank you for adding the odom topic!!

[clegenti]

Great! Glad you find that framework useful :D
I am still not the happiest with the mesh reconstruction (there are scenarios where the mesh is not great ...). I have a few ideas to improve but I need to find time haha

Thanks for the colcon output.

Let me know how it goes, and don't hesitate to bring up any issues or new features to consider :)

[gglaspell]

I had to time today to try to integrate 2fast2lamaa with Nav2. I used octomap-server to generate an occupancy grid from the topic /lidar_static on the map frame. I was going to use /map but I wasn't sure if that removed dynamic obstacles and was pretty sure /lidar_Static did. That seemed to work well based on the image below. The image is taken with Rviz2 set to the map frame.

However, if you see the tfs for the robot they are off the map and titled. Here is an image taken with the frame set to odom.

It seems to me the orientation of the odom frame doesn't get aligned to a base frame, specifically base_footprint or base_link. I think I am stuck. I did poke around the DLIO code to see how they did it and found this:
https://github.com/vectr-ucla/direct_lidar_inertial_odometry/blob/fc8d183f18cdcfb9bb4fc754c6d373cedc4cbd04/src/dlio/odom.cc#L385

[clegenti]

Hmmm, does your lidar start aligned with gravity (like z-axis up)?
My first guess is that the octomap-server expects the main reference frame to be aligned with gravity.
DLIO provides the gravity alignment I think, but in its current version 2fast-2lamaa is referenced to the first IMU frame
--> If your IMU is titled when the system is switched on, then the map is not aligned with gravity.
If you can easily test that hypothesis by starting your system aligned with gravity, that would be great.

I'll look at providing that alignment over the weekend :)

PS:
The map topic provides all the points in the map thus without the dynamic points detected in the odometry step and without the carved free-space points (objects that were not dynamic when observed in the past but that are not present in the environment any longer).
The lidar_static provides the latest scan without the dynamic points.

[gglaspell]

Thank you for the quick reply. I have mainly been testing 2fast2lamaa in simulation, before transitioning to my physical UGV. The ability to create 3D representations of environments that remove dynamic obstacles is of significant interest to me. Often times people walking by create ghost artifacts in the final point cloud. Here are my steps in simulation. Note the robot hasn't moved through the example below.

First, I launch my turtlebot3 (tb3) robot and 2fast2lamaa. The top most frame of tb3 is base_footprint and the velodyne_points pointcloud2 topic and it is aligned with z-axis up.

In 2ast2lamma here are the images with fixed frame map

and fixed frame odom. I assume the tilt is due to the first frame of the simulated IMU that you mentioned in your previous post?

Note that my tf trees are separated at this point.

Thus, I run a static transform to connect them. This is the step that bothered me the most. Since I am connecting a fixed odom frame with a mobile base_footprint frame with a static link. RTAB-Map, DLIO and FAST-LIO2 typically provide a connected base frame so that is what I am used to.
ros2 run tf2_ros static_transform_publisher "0" "0" "0" "0" "0" "0" "odom" "base_footprint"

With the frames connected

the odom fixed frame, with the velodybe_points topic added, looks like this

and the map fixed frame looks like this

Hope that helps and thank you for explaining to me what the map topic is. Also wanted to say the field topic is quite clever. I expect that you probably run UAVs,, but I think this will really help a UGV too, especially when I hand the controls to a non-ROS user to drive the robot.

[clegenti]

I actually only worked with handheld devices but yeah I had the UAV scenarios in mind when mentioning downstream planning applications in the paper.

As I never integrated myself with mobile platforms, I might have messed up the standard names of topics.
odom is my drifty reference frame for my "front-end" odometry. The transformation between frames map and odom corrects that drift so that the current lidar scan is well registered in the map.
So the link that I believe you should do between your robot's TF tree and the one from 2fast-2lamaa is between lidar and base_footprint. The value of that link should be the inverse of the (static? as not changing through time) transformation between base_footprint and velodyne:
If we define T1 = base_footprint --> base_link and T2 = base_link --> velodyne, then you should add lidar --> base_footprint = inv( T1 * T2 ).
That might be an ugly fix but that might solve the alignment issue.

[gglaspell]

Thank you for you explanation. I made some headway today. The clue for me was 2fast2lamma's map frame is the first fixed frame (what I was calling the odom frame). Therefore, I should connect the map frame directly to my base_footprint. I then take 2fast2lamma's /map topic and feed it into octomap-server but do it in the world frame. The results are below.

my tf tree now looks like this:

However, this falls apart when I move since I have a static transform between the map frame and base_footprint as shown below. Note the /velodyne_points pointcloud2 topic has shifted in regard to the /map topic.

I think the fix is to add something like DLIO did but do it for 2fast2lamaa's map frame. The link is re-posted here for convenience:
https://github.com/vectr-ucla/direct_lidar_inertial_odometry/blob/fc8d183f18cdcfb9bb4fc754c6d373cedc4cbd04/src/dlio/odom.cc#L385

[gglaspell]

Quick update I also tried your suggestion linking the lidar frame to base_footprint
Before moving:

Tf tree:

After moving, this time /velodyne_points remained aligned to /map, but the occupancy grid shifted with the robot's poistion.

:

[clegenti]

I believe you should feed lidar_static point clouds to the octomap server while keeping the last tf tree you showed:

• in the documentation of the octomap server they say it need to be data in the sensor frame (map is not, lidar_static is)
• the octomap also does some free-space carving so it should also remove objects that are not detected similarly to how 2fast2lamaa does :)

[gglaspell]

You were right; that worked!! Thank you for all the help!!
Here a picture of a waypoint being set with nav2

and the robot moving to goal.

The lines that go off screen is 2fast2lamaa's odom node, but it doesn't seem to affect waypoint navigation.

This picture show's 2fast2lamaa's odom topic.

[clegenti]

Good to hear it works :)

I had a quick look at the gravity alignment but I don't have time now to do something clean. But looks like you don't need it so all good.

Closing the issue. Don't hesitate to re-open if required.