Add amcl3 demo #9

pvela2017 · 2024-11-23T08:17:59Z

Proposed changes

Added a demo for the AMCL3 algorithm based on the likelihood sensor model 3D (Ekumen-OS/beluga#440)

Type of change

🐛 Bugfix (change which fixes an issue)
🚀 Feature (change which adds functionality)
📚 Documentation (change which fixes or extends documentation)

Checklist

Put an x in the boxes that apply. This is simply a reminder of what we will require before merging your code.

Lint and unit tests (if any) pass locally with my changes
I have added tests that prove my fix is effective or that my feature works
I have added necessary documentation (if appropriate)
All commits have been signed for DCO

Additional comments

Signed-off-by: Pablo Vela <pablovela@naver.com>

hidmic

First pass

hidmic · 2024-11-25T22:30:28Z

docker/files/jazzy_base.repos

+repositories:
+  beluga:
+    type: git
+    url: https://github.com/pvela2017/beluga.git


@pvela2017 this needs to change before merging.

This can now point to the main repository.

Changed in 623f842

hidmic · 2024-11-25T22:31:48Z

docker/images/jazzy/Dockerfile

+    libopenvdb-dev \
+    python3-pip \
+    mc \
+    ros-jazzy-pcl-ros \


@pvela2017 why adding this here instead of in the corresponding package.xml?

Changed in 150be6e

hidmic · 2024-11-25T22:35:08Z

...ation/beluga_demo_amcl3_localization/launch/demo_botanic_garden_amcl3_localization.launch.py

+    ]
+
+    return LaunchDescription(nodes)
+


@pvela2017 nit: missing EOF newline, here and elsewhere.

Changed in b15a8cc and 471d716

localization/beluga_demo_amcl3_localization/scripts/marker_publisher.py

hidmic · 2024-11-25T22:38:53Z

localization/beluga_demo_amcl3_localization/scripts/tf_publisher.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+


@pvela2017 meta: I wonder if it wouldn't be simpler to modify the dataset (which we have already modified) to include odom to base_link transforms.

Did we do this or not @pvela2017?

I tried to implement this on an other project. But there when playing the bag there was a warning about the timestamp. I dont know if it is a bug or I did something wrong. I will try again for this project

Its working! How can I send you the new rosbag?
Updated in 5978183

hidmic · 2024-11-25T22:43:08Z

localization/beluga_demo_amcl3_localization/src/beluga_amcl3_demo.cpp

+
+        // Initial position
+        const auto pose = Sophus::SE3d{
+            Sophus::SO3d{/*kInitialPoseYaw, kInitialPosePitch, kInitialPoseRoll*/},


@pvela2017 nit: remove commented out code.

Changed in 64e5615

hidmic · 2024-11-25T23:07:20Z

localization/beluga_demo_amcl3_localization/src/beluga_amcl3_demo.cpp

+        point_color.b = 1.0;
+        point_color.a = 1.0;
+
+        for (openvdb::FloatGrid::ValueOnCIter iter = grid->cbeginValueOn(); iter; ++iter) {


@pvela2017 the condition expression is unusual, is there no cendValueOn()?

As discussed in the meeting it is ok.

OpenVDB iterators are not STL-compatible in that one can always request an iterator that points to the beginning of a collection of elements (nodes, voxels, etc.), but one usually cannot request an iterator that points to the end of the collection. (This is because finding the end might require a full tree traversal.)

Reference

hidmic · 2024-11-25T23:08:31Z

localization/beluga_demo_amcl3_localization/src/beluga_amcl3_demo.cpp

+        visualization_msgs::msg::Marker marker_msg;
+        std_msgs::msg::ColorRGBA point_color;
+        openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox();
+        double min_z, max_z;


@pvela2017 nit: better define and assign on the same line, then it can be const

Changed in 64e5615

hidmic · 2024-11-25T23:08:40Z

localization/beluga_demo_amcl3_localization/src/beluga_amcl3_demo.cpp

+            marker_msg.colors.push_back(point_color);
+        }
+
+        double size                   = grid->transform().voxelSize()[0];


@pvela2017 nit: can be const

Changed in 64e5615

hidmic · 2024-11-25T23:13:44Z

docker/run.sh

+                     "localization/beluga_demo_bearing_localization" 
+                     "localization/beluga_demo_fiducial_localization" 
+                     "localization/beluga_demo_lidar_localization"
+    )


@pvela2017 I'm a bit ambivalent about this, but I can't think of a better solution. CC @glpuga for thoughts.

Ping @glpuga

I don't like it. I'm assuming these don't build or have missing dependencies in Jazzy.

Why supporting anything other than Humble, if this is a demos repository and we provide a docker to run it?

Why supporting anything other than Humble, if this is a demos repository and we provide a docker to run it?

Because OpenVDB packaging is broken in Ubuntu 22.04, see https://bugs.launchpad.net/ubuntu/+source/openvdb/+bug/1970108.

Nav2 folks realized about this and vendored it (see openvdb_vendor) but depending on their vendored version means we are (again) subject to their own release timeline to release ourselves. There is no good solution.

I'm assuming these don't build or have missing dependencies in Jazzy.

Thinking out loud, and this is out of scope for this PR but, Humble is the old LTS distro. Jazzy is new LTS distro. Having some our demos be incompatible with the latest LTS distro of ROS 2 is not good.

Oh, I remember now.

Jazzy is new LTS distro. Having some our demos be incompatible with the latest LTS distro of ROS 2 is not good.

I'm not so absolute. People are still by far mostly using Humble, while Jazzy is at best "up-and-coming" right now. Core stuff should support both already, but leaf-stuff -like this self-contained demos repo- does not need to.

In any case, there's no reason I can think off the top of my head why these would not build in Jazzy. Is there any external dependency not available?

Looking at the package.xml files, everything seems either standard packages or packages with code we wrote ourselves. Indirect dependencies might be gazebo (classic?) and apriltag_ros which we use the "foxy-devel" branch (because there's no other).

So, https://github.com/ROBOTIS-GIT/turtlebot3_simulations has not seen a release since Humble and it doesn't look like it will ever do. Nav2 folks spinned up https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation in place for it. Some of our demos already depend on Nav2 code, so I'll go ahead and use this for both Humble and Jazzy. It's source distributed anyways.

In any case, there's no reason I can think off the top of my head why these would not build in Jazzy. Is there any external dependency not available?

@pvela2017 ? I think it was Gazebo related, though my memory might be failing me. Isn't Gazebo Classic gone?

Edit: https://discourse.ros.org/t/gazebo-classic-end-of-life-ros-2-jazzy/36239 indeed.

I dont remember if I reached that step were Gazebo was an issue.

So, https://github.com/ROBOTIS-GIT/turtlebot3_simulations has not seen a release since Humble and it doesn't look like it will ever do. Nav2 folks spinned up https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation in place for it. Some of our demos already depend on Nav2 code, so I'll go ahead and use this for both Humble and Jazzy. It's source distributed anyways.

I remember this was one of the issues. Also, I removed a lot of dependencies so the rosdep wouldn't pop errors.

So, Should we update everything to Gazebo ignition?

Should we update everything to Gazebo ignition?

@pvela2017 indeed. I'm on it.

Should we update everything to Gazebo ignition?

@pvela2017 indeed. I'm on it.

Can I help?

Sure! I'll tag you on the PR as soon as it is up for review. If you can then adjust this PR for it, that'd be amazing.

Signed-off-by: Pablo Vela <pablovela@naver.com>

hidmic · 2025-01-28T11:23:37Z

@pvela2017 Ekumen-OS/beluga#440 got merged. Are you still up for wrapping this up?

pvela2017 · 2025-01-28T12:06:08Z

@pvela2017 Ekumen-OS/beluga#440 got merged. Are you still up for wrapping this up?

Yep! I'm here

hidmic

Great job. Left a couple final comments but I'm overall satisfied with this PR.

hidmic · 2025-01-29T11:09:58Z

localization/beluga_demo_amcl3_localization/scripts/tf_publisher.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+


Did we do this or not @pvela2017?

hidmic · 2025-01-29T11:14:52Z

localization/beluga_demo_amcl3_localization/src/beluga_amcl3_demo.cpp

+  const std::string kMapFile =
+      "/home/developer/ws/src/beluga_demo/localization/"
+      "beluga_demo_amcl3_localization/maps/map.vdb";


@pvela2017 consider installing map.vdb under share. Then you can use ament_index_cpp::get_package_share_directory to find it instead of hard-coding the path.

Updated in 8d7f101

Signed-off-by: Pablo Vela <pablovela@naver.com>

pvela2017 added 6 commits November 20, 2024 16:49

Add amcl3 demo

569f821

Signed-off-by: Pablo Vela <pablovela@naver.com>

Add jazzy image

3a6ccdb

Signed-off-by: Pablo Vela <pablovela@naver.com>

Add crop filter

b609953

Signed-off-by: Pablo Vela <pablovela@naver.com>

Improve jazzy instructions

2839e03

Signed-off-by: Pablo Vela <pablovela@naver.com>

Add ignore vdb files

678dfe1

Signed-off-by: Pablo Vela <pablovela@naver.com>

Change files permission

0da26dd

Signed-off-by: Pablo Vela <pablovela@naver.com>

hidmic self-requested a review November 25, 2024 11:30

hidmic reviewed Nov 25, 2024

View reviewed changes

pvela2017 added 7 commits November 28, 2024 12:44

Add EOF

b15a8cc

Signed-off-by: Pablo Vela <pablovela@naver.com>

Fix nit and add particle markers

64e5615

Signed-off-by: Pablo Vela <pablovela@naver.com>

Add EOF

471d716

Signed-off-by: Pablo Vela <pablovela@naver.com>

Change to rosdep dependencies

150be6e

Signed-off-by: Pablo Vela <pablovela@naver.com>

Fix covariance calculation

0975f94

Signed-off-by: Pablo Vela <pablovela@naver.com>

Activate particle markers

880d938

Signed-off-by: Pablo Vela <pablovela@naver.com>

Fix formatting

a5d0485

Signed-off-by: Pablo Vela <pablovela@naver.com>

hidmic approved these changes Jan 29, 2025

View reviewed changes

pvela2017 added 6 commits February 6, 2025 16:58

Change beluga repo path

623f842

Signed-off-by: Pablo Vela <pablovela@naver.com>

Update to beluga_vdb

720f4ee

Signed-off-by: Pablo Vela <pablovela@naver.com>

Remove hard-coded map path

8d7f101

Signed-off-by: Pablo Vela <pablovela@naver.com>

Remove test map

93dc030

Signed-off-by: Pablo Vela <pablovela@naver.com>

Update readme

f0f5fa6

Signed-off-by: Pablo Vela <pablovela@naver.com>

Move tf to rosbag

5978183

Signed-off-by: Pablo Vela <pablovela@naver.com>

Add amcl3 demo #9

Are you sure you want to change the base?

Add amcl3 demo #9

Conversation

pvela2017 commented Nov 23, 2024

Proposed changes

Type of change

Checklist

Additional comments

hidmic left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic Jan 29, 2025 • edited Loading

Choose a reason for hiding this comment

glpuga Jan 29, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic commented Jan 28, 2025

pvela2017 commented Jan 28, 2025

hidmic left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic Jan 29, 2025 •

edited

Loading

glpuga Jan 29, 2025 •

edited

Loading