…_dependencies (#5514)

Signed-off-by: JPLDevMaster <joao.penha.lopes@tecnico.ulisboa.pt>

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* Add Vector Object server

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Meet review comments

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

* Simplify shapes param configuring

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Rename getROSParameter() to getParameter()

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Return back getMaskData() to nav2_costmap_2d

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Add composition node support

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

* Remove redundant methods

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update nav2_map_server/src/vo_server/vector_object_server.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Avoid shapes clearing

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Optimize switchMapUpdate() method

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Switch to vector of shapes

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Minor fixes

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Meet review comments

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Move isPointInside algorithm to nav2_util

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Testcases covering new functionality

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Fix linting issues

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Adjust for Vector Objects demonstration

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Code clean-up
* Corrected headers
* Functions ordering
* Comment fixes

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Additional code facelift
* Correct licensing years
* Fix Vector Object server dependencies
* Funcion rename for better readability
* Improve/fix comments

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

* Minor fixing after rebase

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Rename vector object server

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Minor changes

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update tests

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Merge branch 'main' into feature/vector_object_server

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix merge issues and pre-commit checks

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Change tf2_ros headers from `.h` to `.hpp`

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix race condition in pub-sub of VO map

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Cleanup

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Remove use of ament_target_dependencies

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix review comments

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix linter errors

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix exception handling

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Update nav2_map_server/include/nav2_map_server/vector_object_utils.hpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_util/include/nav2_util/raytrace_line_2d.hpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Add error logs

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix cpplint

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

---------

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Alberto Tudela <ajtudela@gmail.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Add missing dependency declaration

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Remove unused header

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

---------

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Get rid of rclcpp spin all and spin some

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Linting

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Vector object server

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Linting

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Update nav2_rviz_plugins/src/utils.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Error log

Signed-off-by: Maurice <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Maurice <mauricepurnawan@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

… dock plugins (#5015) (#5218)

* feat(opennav_docking): Add dynamic lifecycle for external detectors

This change introduces an API to dynamically enable and disable external
docking detectors, such as those based on AI, to conserve system
resources when not actively docking.

Key changes:
- Added `startDetectionProcess()` and `stopDetectionProcess()` to the
  docking plugin interface (`ChargingDock` and `NonChargingDock`).
- Implemented this logic in `SimpleChargingDock` and
  `SimpleNonChargingDock` using a `std_srvs/Trigger` service call and
  dynamic topic subscription to manage the detector's lifecycle.
- The `DockingServer` now ensures `stopDetectionProcess()` is called
  on all exit paths, including success, failure, and cancellation,
  to prevent dangling resources.
- Added new parameters (`detector_service_name`, `subscribe_toggle`, etc.)
  to configure this behavior and updated the README documentation.
- Added comprehensive C++ unit tests to verify the new detector
  lifecycle logic, specifically covering service failure cases and
  proper cleanup on action termination.

Closes #5015
Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com>

-

updtae

Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com>
Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* fix: Address review feedback and fix unit tests

Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com>

fix(docking): Change subscribe_toggle default to false

Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com>

-

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* refactor(docking): Improve plugin lifecycle and resource management

This commit addresses several code review suggestions to improve the design and robustness of the docking plugins.

- Replaced the `DetectorState` enum with a simpler `bool detector_enabled_` for clarity.
- Encapsulated the detection process lifecycle within the plugin itself. The `deactivate` method now ensures `stopDetectionProcess` is called, improving encapsulation and simplifying the `DockDatabase`.
- Refactored the `configure` method in plugins to group related logic, improving readability.
- Cleaned up redundant checks and calls in both the plugins and the `DockingServer` for more robust and intentional code.

Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com>
Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* fix(plugins): Resolve race condition by setting enabled state in callback

The previous refactoring introduced a race condition where the detector
was marked as 'enabled' in startDetection() before a message was
actually received.

This caused tests to fail because getRefinedPose() would be called
with an enabled state but no valid/recent data.

This commit fixes the issue by moving the `detector_enabled_ = true`
assignment into the subscription callback. This ensures the plugin's
state accurately reflects that it has received at least one valid
data point before being considered active.

Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com>
Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* refactor(docking): migrate to nav2_ros_common (node_utils, LifecycleNode, QoS)

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* build(opennav_docking): update deps (std_srvs, package.xml + CMakeLists)

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* refactor(Docking): migrate to nav2::LifecycleNode

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* refactor(docking): use nav2::qos::StandardTopicQoS for subscriptions

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* refactor(opennav_docking): replace raw queue size with rclcpp::QoS(1) in pubs/subs

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* refactor(tests): migrate nav2_util::NodeThread to nav2::NodeThread

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* refactor(tests): migrate to 3-args service callbacks

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* style(test): apply ament_uncrustify

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* refactor(opennav_docking) migrate detector Trigger service to node_->create_client()

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* docking: use nav2 types for pubs/subs in SimpleChargingDock and add explicit detection state flags

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* docking: use nav2 types for pubs/subs in SimpleNonChargingDock and add explicit detection state flags

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* docking: split detection state in SimpleChargingDock (detection_started_ vs initial_pose_received_) and warn before first pose

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* docking: split detection state in SimpleNonChargingDock (detection_started_ vs initial_pose_received_) and warn before first pose

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* tests: adapt SimpleChargingDockTestable to initial_pose_received_ state

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* tests: adapt SimpleNonChargingDockTestable to initial_pose_received_ state

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* docs(docking): clarify external detection gating and subscribe_toggle behavior (default=false)

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* fix(docking): keep SimpleNonChargingDock registered as ChargingDock (stay in-scope)

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* docs(docking): revert README note to pre-e881de19 state

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* fix(docking_server): remove redundant null-check before stopDetectionProcess() in terminal cleanup

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* style(docking_server): unify terminal order (stash->publishZeroVelocity->stopDetection->terminate)

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* lint

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* fix(docking): inline detection process

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* chore(docking): drop redundant detector comment

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* chore(docking): clarify detector logs

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* fix(docking): activate lifecycle publishers

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* chore(docking): reuse dock pose subscription

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* lint

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* fix(docking_server): drop redundant DB deactivate on cleanup

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* refactor(docking): rename detection state flag to detection_active

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

* fix(docking): reset detection flags on cleanup

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

---------

Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com>
Signed-off-by: bkoensgen <b.koensgen@gmail.com>