Add particle cloud display plugin for RViz

[naiveHobo]

Signed-off-by: Sarthak Mittal sarthakmittal2608@gmail.com

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1678
Primary OS tested on	Ubuntu 18.04
Robotic platform tested on	TurtleBot3 gazebo simulation

---

Description of contribution in a few bullet points

• Added Particle Cloud Display plugin for RViz to display AMCL particle clouds
• Particles scale according to weight
• Update bringup rviz files

---

Future work that may be required in bullet points

• Move to the default rviz display plugin for nav2_msgs/ParticleCloud after support is available (Support for new particle filter messages for AMCL ros2/rviz#538)

[SteveMacenski]

I have no serious objections. Can you also put a screen shot of rviz of this working for posterity?

Please make the changes requested and submit a PR to add this to Rviz2 directly. Its a primitive type important to enough users it belongs there in my opinion. Link the Nav2 and Rviz2 tickets and CC me.

[naiveHobo]

[SteveMacenski]

@naiveHobo I don't notice any difference between this and the PoseArray visualization. I'm not saying that's a problem, but if you wanted to highlight the changes in weights, you may want to consider making it more clear.

Let me know. Otherwise, I'm good with this. You should open a PR on RVIZ2 to add this.

[SteveMacenski]

Great job on this and fast turn around 🥇 Let me know if you're interested in doing something next or what's on your mind. I recall your interest in control and AI. There's also a body of software work as well if you're interested in continuing in this vein. Obviously, up to you 👍 Your help is appreciated.

[naiveHobo]

@naiveHobo I don't notice any difference between this and the PoseArray visualization. I'm not saying that's a problem, but if you wanted to highlight the changes in weights, you may want to consider making it more clear.

I was testing why this looks so similar to the PoseArray visualization and the scaling is not apparent based on the weights. Seems to me that this might be a bug in AMCL. The weights are always evenly distributed among all the particles. When I say evenly distributed, it means they are exactly the same for every particle in the cloud. Hence, the arrows all look the same in the visualization.

@SteveMacenski as far as I can tell, that's not the expected behavior, right?

Let me know. Otherwise, I'm good with this. You should open a PR on RVIZ2 to add this.

I was about to open a PR on rviz2, but I'd have to add nav2_msgs as a dependency for rviz_default_plugins in rviz2. If I'm understanding correctly, that's not possible right not, right? The default ros2 packages should build without any nav2 dependencies right now, right?

Great job on this and fast turn around 1st_place_medal Let me know if you're interested in doing something next or what's on your mind. I recall your interest in control and AI. There's also a body of software work as well if you're interested in continuing in this vein. Obviously, up to you +1 Your help is appreciated.

Thanks, love to be of help! I'd love to contribute to anything that needs help really.

[SteveMacenski]

that's not the expected behavior

Yeah, I'd say so.

nav2_msgs as a dependency for rviz_default_plugins in rviz2

True, I'd be interested in their thoughts on it. We can spin it out into another repo or something, I don't mind, but I think they'll have something they want in mind so rather than guessing, lets just find out. Its also already dependent on nav_msgs so I can make a really good argument that this should be added too. If they don't like that, we can literally just put the particle messages into nav_msgs and no new dependencies.

Thanks, love to be of help! I'd love to contribute to anything that needs help really.

Awesome, let me think about this for a bit. Anything you want to learn about in ROS2 / navigation / software? Just to help narrow the field. If you're at all interested in the launch system (python) and lifecycle, there's a few things there that could be good. Working on some AI demos could be really good reference designs. Leading the ship on a new controller project could be a good opportunity if you had ideas / thoughts on that. CiLQR, iLQR, MPC, etc keep coming up. I'm working (well, I was before my days got consumed with replying to tickets / PRs / slack) on a motion planner that touches non-linear optimization, search, and software design. All are options. Also anything else you see on GItHub or on the pinned posts on Slack channels.

[codecov]

Codecov Report

Merging #1688 (528107d) into main (2efdad8) will decrease coverage by 0.22%.
The diff coverage is n/a.

@@            Coverage Diff             @@
##             main    #1688      +/-   ##
==========================================
- Coverage   84.11%   83.89%   -0.23%     
==========================================
  Files         247      247              
  Lines       12119    12119              
==========================================
- Hits        10194    10167      -27     
- Misses       1925     1952      +27     

Flag	Coverage Δ
project	83.89% <ø> (-0.23%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Impacted Files	Coverage Δ
...tmap_2d/plugins/costmap_filters/costmap_filter.cpp	73.68% <0.00%> (-10.53%)	⬇️
nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp	90.00% <0.00%> (-10.00%)	⬇️
nav2_navfn_planner/src/navfn_planner.cpp	87.42% <0.00%> (-5.99%)	⬇️
...v2_recoveries/include/nav2_recoveries/recovery.hpp	83.52% <0.00%> (-5.89%)	⬇️
...lifecycle_manager/src/lifecycle_manager_client.cpp	85.71% <0.00%> (-2.39%)	⬇️
nav2_amcl/src/sensors/laser/beam_model.cpp	95.45% <0.00%> (-2.28%)	⬇️
nav2_bt_navigator/src/bt_navigator.cpp	91.97% <0.00%> (-1.24%)	⬇️
nav2_navfn_planner/src/navfn.cpp	90.86% <0.00%> (-0.49%)	⬇️
nav2_amcl/src/amcl_node.cpp	84.84% <0.00%> (-0.16%)	⬇️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 2efdad8...528107d. Read the comment docs.

[SteveMacenski]

Ah, codecov to the reminder can you write some tests for this? I have to be honest, I have no clue how to write rviz display tests....

.... luckily for us this resembles closely pose array and they have tests for that https://github.com/ros2/rviz/tree/ros2/rviz_default_plugins/test/rviz_default_plugins/displays/pose_array to steal + convert

[naiveHobo]

Turns out rviz has a brilliant library for visual testing. I had to port over a few files as they were from rviz_default_plugins, though.

Leading the ship on a new controller project could be a good opportunity if you had ideas / thoughts on that. CiLQR, iLQR, MPC, etc keep coming up. I'm working (well, I was before my days got consumed with replying to tickets / PRs / slack) on a motion planner that touches non-linear optimization, search, and software design.

I'd love to help with the controller development. Your project sounds extremely interesting, would love to be a part there. I have worked with local planners/controllers before (pure pursuit and DWA) so I think the learning curve wouldn't be too steep for me and I can get to development quickly.

[SteveMacenski]

Got it, hopefully they don't live here too long and we can just merge it into rviz2 right away. It might be worth putting a PR into Rviz2 now that you have some tests as well (once they work).

Your build seems to be failing on a linking issue. Might need to link against some rviz libraries.

Lets chat in slack

• S

[naiveHobo]

Can't seem to figure out the linking issue. There are undefined references to rviz_default_plugins::tools::PoseTool but PoseTool is only used with nav2_rviz_plugins::GoalTool and there haven't been any library linking changes in nav2_rviz_plugins. Plus, I can't reproduce the linking issue on my system.

@SteveMacenski @shivaang12 any idea what this could be about?

[SteveMacenski]

It may be because you didn't make the 2 rviz plugins different libraries. It looks like you appended your file to the same library as the pose tool and panel. Try keeping different libraries.
In the test CMake section you find packages like rviz_default_plugins but you don't link against them.

I also don't want you wasting too much time trying to get these tests working in Nav2 since I really want this to live in Rviz2. If you can put stuff there and have it work, lets just do that and move on.

[SteveMacenski]

That seems to have fixed it, what ended up being the issue?

[naiveHobo]

I think it's pretty much what you suggested. Since I had added the display plugin to the same library as panel and goal tool, during the test build I'm linking the complete library to the tests. During the tests, it's not able to find the links for panel and goal tool since I'd written the CMakeLists.txt assuming I'd only have to find the packages that the display plugin needed during the tests.

Though, I can't seem to reproduce the issue on my system. I tried a clean build as well.

[SteveMacenski]

Well, that's what we have CI for 👍

[naiveHobo]

I'm not sure what the codecov errors are. There's a small problem with plugin loading, so I'd hold the merge till the next commit.

[SteveMacenski]

Codecov error just means that you didnt make the test coverage higher. Its mostly just a note of "yes, you increased coverage" or "no, you didnt increase coverage". That was happening because of I think your loading issue, it didnt show any coverage for your additions.

[naiveHobo]

Ah, I just realised the test coverage is an issue. The rviz plugin tests are based on the availability of a display. The tests only run if a display is available, as per these lines. These tests are pretty much useless with the CI.

# Tests currently only run on OS X @ OSRF jenkins
# Enable on Linux by providing a displays, enable on Windows via EnableDisplayTests=True
option(EnableDisplayTests "EnableDisplayTests")
set(DisplayTests "False" CACHE STRING "DisplayTestsVariable")

if(DEFINED ENV{DISPLAY})
  set(DISPLAYPRESENT TRUE)
endif()

if(APPLE OR DISPLAYPRESENT OR EnableDisplayTests STREQUAL "True")
  message(STATUS "Enabling tests requiring a display")
else()
  set(SKIP_DISPLAY_TESTS "SKIP_TEST")
endif()

# options and directories for visual tests (see visual_testing_framework documentation)
option(EnableVisualTests "decides whether or not to enable the tests")

[naiveHobo]

I also don't want you wasting too much time trying to get these tests working in Nav2 since I really want this to live in Rviz2. If you can put stuff there and have it work, lets just do that and move on.

Makes sense. I shall revert back to the commits before I added the tests and move on I guess? I mean, everything seems to work on my system and the PR on Rviz2 should work without any problems since that just integrates the particle cloud display to the existing library.

[SteveMacenski]

Sounds good. Please do submit that PR to RVIZ2. I dont think it will be merged before Foxy since they're all busy, but it gives them time to review it. RVIZ2 PRs typically take a while to merge so that's why I want you to submit it sooner than later so we can get that in. And potentially if we can get in before foxy, we just don't include it in this repo at all.

[naiveHobo]

I have submitted the PR (ros2/rviz#541). Everything should be good to go after the revert.

[SteveMacenski]

Do the revert and lets wait a few days one way or another

[SteveMacenski]

Why close this?

[naiveHobo]

Ah, sorry closed this by mistake.

[SteveMacenski]

I'm of a mind that we just merge this. Can you pull in master so we can trigger a modern CI build? If it passes, lets just merge it.

We've waited more than enough time to get the nav msgs stuff done if it were ever going to be. We should move on. Plus we don't count the rviz plugin anymore in our coverage metrics since its not run-time on robot.

[naiveHobo]

@SteveMacenski good to go?

[SteveMacenski]

Yup! Thanks for baring with me on this