Publish collision points for debug purposes #3879

tonynajjar · 2023-10-13T15:16:13Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu
Robotic platform tested on	Custom Simulation

Description of contribution in a few bullet points

Added publish of collision points in form of MarkerArray for debugging purposes

Description of documentation updates required from your changes

Docs + Migration guide

Future work that may be required in bullet points

For Maintainers:

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

nav2_collision_monitor/src/collision_monitor_node.cpp

…debug-main

codecov · 2023-10-13T16:17:49Z

Codecov Report

Attention: 21 lines in your changes are missing coverage. Please review.

Comparison is base (461a7ba) 90.20% compared to head (55b2e8a) 90.29%.
Report is 1 commits behind head on main.

❗ Current head 55b2e8a differs from pull request most recent head cce70c8. Consider uploading reports for the commit cce70c8 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3879      +/-   ##
==========================================
+ Coverage   90.20%   90.29%   +0.09%     
==========================================
  Files         413      413              
  Lines       18318    18346      +28     
==========================================
+ Hits        16523    16566      +43     
+ Misses       1795     1780      -15

Files	Coverage Δ
...2_collision_monitor/src/collision_monitor_node.cpp	`90.43% <27.58%> (-8.22%)`	⬇️

... and 4 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

SteveMacenski

Generally look good! Missing test coverage excerising though

nav2_collision_monitor/src/collision_monitor_node.cpp

SteveMacenski · 2023-10-13T23:49:38Z

nav2_collision_monitor/src/collision_monitor_node.cpp

  if (!state_topic.empty()) {
    state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionMonitorState>(
      state_topic, 1);
  }

+  if (visualize_collision_points) {
+    collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "~/collision_points_marker", 1);


Suggested change

"~/collision_points_marker", 1);

"collision_points_marker", 1);

Is the same thing

Not really, ~/collision_points_marker will result in /<node_name>/collision_points_marker. collision_points_marker will result in /collision_points_marker. Since all other topics are without ~/, I'll remove it

Actually I just ran into a problem with this: I have both a collision detector and monitor running and since they have the same topic name, not having the namespace makes them publish to the same topic. I would add the ~/ if you don't object

nav2_collision_monitor/src/collision_monitor_node.cpp

SteveMacenski

Then should be good to go!

nav2_collision_monitor/src/collision_detector_node.cpp

nav2_collision_monitor/test/collision_monitor_node_test.cpp

SteveMacenski

Otherwise LGTM once the test works!

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

tonynajjar · 2023-10-18T14:08:58Z

Should be good to go

SteveMacenski · 2023-10-18T20:20:10Z

@tonynajjar there are multiple collision monitor pkg test failures (ignore python linting… I’ll try to handle that Friday)

tonynajjar · 2023-10-19T09:07:01Z

@tonynajjar there are multiple collision monitor pkg test failures (ignore python linting… I’ll try to handle that Friday)

Ahh that came for the last topic change, I forgot to do the same in the test. Should be fine now

tonynajjar · 2023-10-24T17:40:42Z

Backport to iron please?

SteveMacenski · 2023-10-24T17:53:41Z

beep boop, mergify, do yo thing

* Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * . * fixes * add to collision detector * fix * fix * . * fixes * add namespace to topic * fixes (cherry picked from commit c0fd78f) # Conflicts: # nav2_collision_monitor/test/collision_monitor_node_test.cpp

* Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * . * fixes * add to collision detector * fix * fix * . * fixes * add namespace to topic * fixes (cherry picked from commit c0fd78f) # Conflicts: # nav2_collision_monitor/CMakeLists.txt # nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp # nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp # nav2_collision_monitor/package.xml # nav2_collision_monitor/src/collision_detector_node.cpp # nav2_collision_monitor/src/collision_monitor_node.cpp # nav2_collision_monitor/test/collision_detector_node_test.cpp # nav2_collision_monitor/test/collision_monitor_node_test.cpp

SteveMacenski · 2023-10-24T18:09:39Z

the auto backports failed horribly. The two options are for you to wait until I do a manual backport in bulk before a release or you own a PR to do the backport manually yourself. Yours will be done much faster than mine (weeks)

* Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * . * fixes * add to collision detector * fix * fix * . * fixes * add namespace to topic * fixes (cherry picked from commit c0fd78f)

* Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * . * fixes * add to collision detector * fix * fix * . * fixes * add namespace to topic * fixes

* collision_monitor: dynamic polygon and source enable/disable (#3825) * Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * initial * fix * fix * remove unrelated change * reset * fix format * PR fixes * Add test * fix comments * add to params * publish only if enabled * Add source dynamic enable/disable * add enabled param to sources * fix * add same to collision detector * Update README.md: fix typo (#3842) * Update package.xml * fix typo (#3850) * adjust link to point to v3.8 of behavior tree docs (#3851) BT.CPP_v3 is used, thereby the correct docs should be linked * Fix bug in nav2_behavior_tree/bt_action_node (#3849) * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Fix action test failure due to rate after Rolling sync API change (#3852) * Update CMakeLists.txt (#3843) * add option for sse4 and avs512 (#3853) * Remove all exit(-1) crash conditions (#3846) * Update transform_available_condition.cpp * wrapping all examples of get_plugin_type_param in exceptions and making that throw instead of crash * some linting * fix typo * Update controller.cpp * Update nav2_params.yaml * Update nav2_params.yaml * simplication of lattice_generator.py, fixes #3858 (#3859) * simplification of equation to compute the max_value/outer edge of the lattice based on number of headings. * Stop error diagnostics when pausing nav (#3830) * Added nodestate enum and a variable to keep track of current state of managed nodes. * Updating state_of_managed_nodes_ when switching states and using it to determine an accurate diagnostics message. * Fixing bugs. * Updated/added docstrings. * Publishing OK status when nodes are unconfigured. Changed if-else chain to switch case. * Renamed NodeState PAUSED to INACTIVE, state_of_managed_nodes_ to managed_nodes_state_ and replaced system_active_ with an inline method. * Bugfix. --------- Co-authored-by: Pekka Myller <pekka.myller@karelics.fi> * Add a timeout to the wait for transforms step of the costmap activation. (#3866) * Add a timeout to the wait for transforms step of the costmap activation. Signed-off-by: Fabian König <fabiankoenig@gmail.com> * Rename wait_for_transforms_timeout to initial_transform_timeout * Activate costmap publishers only after transforms are checked * Check if controller server activation was succesful in planner_server * Add unittest for costmap activation Signed-off-by: Fabian König <fabiankoenig@gmail.com> --------- Signed-off-by: Fabian König <fabiankoenig@gmail.com> * Fix class doxygen * fix minor typos (#3892) Signed-off-by: Anton Kesy <antonkesy@gmail.com> * Publish collision points for debug purposes (#3879) * Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * . * fixes * add to collision detector * fix * fix * . * fixes * add namespace to topic * fixes * fix use after free (#3910) * fix build mppi (#3927) Signed-off-by: kevin <kevin@floatic.io> Co-authored-by: kevin <kevin@floatic.io> * Removing old TODOs * protect invalid max_velocity min_velocity (#3953) Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * protect properly max_accel and max_decel (#3952) Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * Fixed links for install and build in README (#3963) Currently the readme is linking to an invalida page in the docs (404 error). * adding support for rotate in place cusps (#3934) * Fix linting error (#3969) * Fix linting error * Update regulated_pure_pursuit_controller.cpp * fix a few outdated comments in smac planners (#3978) * adding soft realtime prioritization for collision monitor and velocity smoother (#3979) * adding soft realtime prioritization for collision monitor and velocity smoother * refactor simple action server to use new utils API * Update README.md * Synchronize map size information during map initialization (#4015) * Update costmap size configuration This commit updates the costmap_2d.cpp file to fix a bug where the costmap size wasn't appropriately updated. Two new lines of code have been added to ensure the size of the costmap is correctly configured each time it's instantiated. * Refactor costmap size assignment in Costmap2D class The code refactor eliminates the direct mutation of the size_x_ and size_y_ attributes in the Costmap2D class. Instead, the class uses the size of cells provided during initialization and calculation from map coordinates for better encapsulation and clarity. * check width&height params (#4017) Co-authored-by: GoesM <GoesM@buaa.edu.cn> * Fix SimpleActionServer nullprt callback (#4025) * add check before calling completion_callback_ * Fix lint * footprint checks (#4030) * footprint checks Signed-off-by: gg <josho.wallace@gmail.com> * lint fix Signed-off-by: gg <josho.wallace@gmail.com> --------- Signed-off-by: gg <josho.wallace@gmail.com> * Is path valid doc (#4032) * docs Signed-off-by: gg <josho.wallace@gmail.com> * update Signed-off-by: gg <josho.wallace@gmail.com> --------- Signed-off-by: gg <josho.wallace@gmail.com> * Update vision_opencv's branch for rolling Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * handle dynamically changes in parameters. (#4046) Signed-off-by: Sebastian Solarte <johan.solarte@kiwibot.com> * Add inflation_layer_name param (#4047) Signed-off-by: Renan Salles <renan028@gmail.com> * missing urdf dep (#4050) Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * bump to 1.2.6 for release --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Signed-off-by: Fabian König <fabiankoenig@gmail.com> Signed-off-by: Anton Kesy <antonkesy@gmail.com> Signed-off-by: kevin <kevin@floatic.io> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Sebastian Solarte <johan.solarte@kiwibot.com> Signed-off-by: Renan Salles <renan028@gmail.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Tony Najjar <tony.najjar@logivations.com> Co-authored-by: thandal <than@timbrel.org> Co-authored-by: Anton Kesy <antonkesy@gmail.com> Co-authored-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Co-authored-by: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com> Co-authored-by: Plaqueoff <44152820+Plaqueoff@users.noreply.github.com> Co-authored-by: Pekka Myller <pekka.myller@karelics.fi> Co-authored-by: Fabian König <fabiankoenig@gmail.com> Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com> Co-authored-by: kevin <kevin@floatic.io> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Abiel Fernandez <empoleom@gmail.com> Co-authored-by: Michael Ferguson <mfergs7@gmail.com> Co-authored-by: Hao-Li-Bachelorarbeit <141755843+Hao-Li-Bachelorarbeit@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: Sebastian Solarte <89881453+Sunart24@users.noreply.github.com> Co-authored-by: Renan Salles <renan028@gmail.com>

* Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * . * fixes * add to collision detector * fix * fix * . * fixes * add namespace to topic * fixes (cherry picked from commit c0fd78f)

* Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * . * fixes * add to collision detector * fix * fix * . * fixes * add namespace to topic * fixes Signed-off-by: enricosutera <enricosutera@outlook.com>

Tony Najjar and others added 12 commits March 10, 2023 15:32

Merge remote-tracking branch 'upstream/main'

a68c971

Merge branch 'ros-planning:main' into main

a5f870c

Merge branch 'ros-planning:main' into main

1e673c7

Merge remote-tracking branch 'upstream/main'

79aff93

Merge branch 'ros-planning:main' into main

0dd0492

Merge branch 'ros-planning:main' into main

c598634

Rename PushRosNamespace to PushROSNamespace

5f8281e

Fix min_points checking

212bbbd

Merge branch 'ros-planning:main' into main

4bce8ea

Merge branch 'ros-planning:main' into main

d00acc3

Merge branch 'ros-planning:main' into main

f91f3b3

.

183e077

tonynajjar changed the title ~~Add collision points debug main~~ Publish collision points for debug purposes Oct 13, 2023

tonynajjar commented Oct 13, 2023

View reviewed changes

nav2_collision_monitor/src/collision_monitor_node.cpp Outdated Show resolved Hide resolved

tonynajjar and others added 2 commits October 13, 2023 17:28

Merge branch 'ros-planning:main' into main

4b0bd57

Merge remote-tracking branch 'origin/main' into add-collision-points-…

55b2e8a

…debug-main

SteveMacenski requested changes Oct 13, 2023

View reviewed changes

fixes

86145e3

tonynajjar requested a review from SteveMacenski October 16, 2023 08:23

add to collision detector

cce70c8

tonynajjar mentioned this pull request Oct 16, 2023

CM/CD: Add polygon source #3885

Closed

7 tasks

SteveMacenski requested changes Oct 16, 2023

View reviewed changes

nav2_collision_monitor/src/collision_detector_node.cpp Outdated Show resolved Hide resolved

nav2_collision_monitor/src/collision_detector_node.cpp Outdated Show resolved Hide resolved

nav2_collision_monitor/src/collision_detector_node.cpp Outdated Show resolved Hide resolved

Tony Najjar added 2 commits October 17, 2023 12:52

fix

8abb418

fix

ff07238

tonynajjar requested a review from SteveMacenski October 17, 2023 10:56

.

9df3c88

tonynajjar commented Oct 17, 2023

View reviewed changes

nav2_collision_monitor/test/collision_monitor_node_test.cpp Show resolved Hide resolved

SteveMacenski reviewed Oct 17, 2023

View reviewed changes

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp Show resolved Hide resolved

fixes

a976020

tonynajjar requested a review from SteveMacenski October 18, 2023 14:08

add namespace to topic

44c8a06

tonynajjar mentioned this pull request Oct 18, 2023

Add Polygon source + add collision points debugging marker logivations/navigation2#51

Merged

fixes

c7380b3

SteveMacenski approved these changes Oct 24, 2023

View reviewed changes

SteveMacenski merged commit c0fd78f into ros-navigation:main Oct 24, 2023
3 of 5 checks passed

SteveMacenski added backport-humble backport-iron labels Oct 24, 2023

mergify bot mentioned this pull request Oct 24, 2023

Publish collision points for debug purposes (backport #3879) #3903

Closed

mergify bot mentioned this pull request Oct 24, 2023

Publish collision points for debug purposes (backport #3879) #3904

Closed

doisyg mentioned this pull request Oct 31, 2023

AUTO-1336 nav2 sync 31-10-2023 for Collision Monitor watchdog mechanism botsandus/navigation2#27

Closed

HovorunB mentioned this pull request Jan 18, 2024

Backport: [Collision Monitor] Add a watchdog mechanism logivations/navigation2#55

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Publish collision points for debug purposes #3879

Publish collision points for debug purposes #3879

tonynajjar commented Oct 13, 2023 •

edited

codecov bot commented Oct 13, 2023 •

edited

SteveMacenski left a comment •

edited

SteveMacenski Oct 13, 2023

tonynajjar Oct 16, 2023

tonynajjar Oct 18, 2023

SteveMacenski Oct 24, 2023

SteveMacenski left a comment

SteveMacenski left a comment

tonynajjar commented Oct 18, 2023

SteveMacenski commented Oct 18, 2023

tonynajjar commented Oct 19, 2023 •

edited

tonynajjar commented Oct 24, 2023

SteveMacenski commented Oct 24, 2023

SteveMacenski commented Oct 24, 2023

	"~/collision_points_marker", 1);
	"collision_points_marker", 1);

Publish collision points for debug purposes #3879

Publish collision points for debug purposes #3879

Conversation

tonynajjar commented Oct 13, 2023 • edited

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

codecov bot commented Oct 13, 2023 • edited

Codecov Report

SteveMacenski left a comment • edited

Choose a reason for hiding this comment

SteveMacenski Oct 13, 2023

Choose a reason for hiding this comment

tonynajjar Oct 16, 2023

Choose a reason for hiding this comment

tonynajjar Oct 18, 2023

Choose a reason for hiding this comment

SteveMacenski Oct 24, 2023

Choose a reason for hiding this comment

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski left a comment

Choose a reason for hiding this comment

tonynajjar commented Oct 18, 2023

SteveMacenski commented Oct 18, 2023

tonynajjar commented Oct 19, 2023 • edited

tonynajjar commented Oct 24, 2023

SteveMacenski commented Oct 24, 2023

SteveMacenski commented Oct 24, 2023

tonynajjar commented Oct 13, 2023 •

edited

codecov bot commented Oct 13, 2023 •

edited

SteveMacenski left a comment •

edited

tonynajjar commented Oct 19, 2023 •

edited