Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Publish collision points for debug purposes #3879

Merged

Conversation

tonynajjar
Copy link
Contributor

@tonynajjar tonynajjar commented Oct 13, 2023


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

@tonynajjar tonynajjar changed the title Add collision points debug main Publish collision points for debug purposes Oct 13, 2023
@codecov
Copy link

codecov bot commented Oct 13, 2023

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.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Generally look good! Missing test coverage excerising though

nav2_collision_monitor/src/collision_monitor_node.cpp Outdated Show resolved Hide resolved
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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Is the same thing

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK

nav2_collision_monitor/src/collision_monitor_node.cpp Outdated Show resolved Hide resolved
nav2_collision_monitor/src/collision_monitor_node.cpp Outdated Show resolved Hide resolved
nav2_collision_monitor/src/collision_monitor_node.cpp Outdated Show resolved Hide resolved
nav2_collision_monitor/src/collision_monitor_node.cpp Outdated Show resolved Hide resolved
@tonynajjar tonynajjar mentioned this pull request Oct 16, 2023
7 tasks
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Then should be good to go!

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
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Otherwise LGTM once the test works!

@tonynajjar
Copy link
Contributor Author

Should be good to go

@SteveMacenski
Copy link
Member

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

@tonynajjar
Copy link
Contributor Author

tonynajjar commented Oct 19, 2023

@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

@SteveMacenski SteveMacenski merged commit c0fd78f into ros-navigation:main Oct 24, 2023
1 of 2 checks passed
@tonynajjar
Copy link
Contributor Author

Backport to iron please?

@SteveMacenski
Copy link
Member

beep boop, mergify, do yo thing

mergify bot pushed a commit that referenced this pull request Oct 24, 2023
* 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
mergify bot pushed a commit that referenced this pull request Oct 24, 2023
* 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
Copy link
Member

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)

jplapp pushed a commit to logivations/navigation2 that referenced this pull request Jan 17, 2024
* 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)
SteveMacenski pushed a commit that referenced this pull request Jan 24, 2024
* Rename PushRosNamespace to PushROSNamespace

* Fix min_points checking

* .

* fixes

* add to collision detector

* fix

* fix

* .

* fixes

* add namespace to topic

* fixes
SteveMacenski added a commit that referenced this pull request Jan 24, 2024
* 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>
HovorunB pushed a commit to logivations/navigation2 that referenced this pull request Jan 24, 2024
* 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)
enricosutera pushed a commit to enricosutera/navigation2 that referenced this pull request May 19, 2024
* 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>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants