Skip to content

Commit

Permalink
Improvements in getCostsCallback (ros-navigation#4767)
Browse files Browse the repository at this point in the history
* fix(simple-action-server): info log instead of warn on cancel (ros-navigation#4684)

Cancelling a goal is nominal behavior and therefore it should not log
warning.

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* feat(controller-server): `publish_zero_velocity` parameter (ros-navigation#4675)

* feat(controller-server): `publish_zero_velocity` parameter

For optionally publishing a zero velocity command reference on goal
exit. Publishing a zero velocity is not desired when we are following
consecutive path segments that end with a velocity.

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>

* processed comments

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>

* comments Steve

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>

---------

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Improvements in RemoveInCollisionGoals and adjacent features (ros-navigation#4676)

* improvements

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* ament_uncrustify

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix formatting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix building

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fixes

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add stamp

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Correct paper name for graceful controller

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Added missing action clients in robot_navigator(BasicNavigator) to destroy_node (ros-navigation#4698)

* fix: added assisted_teleop_client to robot_navigator(BasicNavigator) destroy_node

Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca>

* fix: added other missing action clients to robot_navigator(BasicNavigator) destroy_node

Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca>

---------

Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Adding disengagement threshold to rotation shim controller (ros-navigation#4699)

* adding disengagement threshold to rotation shim controller

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

* change default to 22.5 deg

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

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Switch nav2_waypoint_follower to modern CMake idioms. (ros-navigation#4648)

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fixing SGF in MPPI and Smoother (ros-navigation#4669)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Feat/migrate gps nav2 system test (ros-navigation#4682)

* include missing docking station parameters

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* fix crach RL

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* lintering

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* Change naming

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* update submodule

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* minor changes

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* fix issue with caching

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* fix issue with caching, increase version number

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* Pin git ref via sha to bust underlay workspace cache

---------

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Co-authored-by: ruffsl <roxfoxpox@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix: handle transition failures in all servers (ros-navigation#4708)

* fix: handle transition failures in planner/controller/smoother servers

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* adding support for rest of servers + review comments

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

* Replacing throws with error and failed lifecycle transitions

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

* fix vel smoother unit tests

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

* fixing docking server unit testing

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

* fixing last bits

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

---------

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* [RotationShimController] fix: rotate on short paths (ros-navigation#4716)

Add header data to goal for short paths.

Commit d8ae3c1 added the possibility to
the rotation shim controller to rotate towards the goal when the goal
was closer that the `forward_sampling_distance`. This feature was not
fully working as the goal was missing proper header data, causing the
rotation shim to give back control to the main controller.

Co-authored-by: agennart <antoine.gennart@quimesis.be>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Improve Docking panel (ros-navigation#4717)

* Added load and save panel

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

* Improved dock_panel state machine

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

* Added loading dock plugins log

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

* Redo UI

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

* Update tooltips

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

* Fix null-dereference

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

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Added parameter `rotate_to_heading_once` (ros-navigation#4721)

Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* [RotationShimController] fix: rotate to goal heading (ros-navigation#4724)

Add frame_id to goal when rotating towards goal heading, otherwise the
transform would fail. This bug was introduced in 30e2cde by not setting
the frame_id.

Signed-off-by: agennart <antoine.gennart@quimesis.be>
Co-authored-by: agennart <antoine.gennart@quimesis.be>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* [loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (ros-navigation#4726)

* Publish /clock from loopback sim

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>

* [nav2_costmap_2d] Fix obstacle_layer trying to use RELIABLE QoS

Use QoS profile from rclcpp::SensorDataQoS() instead of rmw_qos_profile_t.
This solves an issue where the subscriber uses RELIABLE setting even when initialized from rmw_qos_profile_sensor_data.
In addition the Subscriber(..., rmw_qos_profile_t) constructor is deprecated in favor of Subscriber(..., rclcpp::QoS)

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>

* [nav2_smac_planner] fix typos

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>

* Use single quotes

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>

---------

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Remove nav2_loopback_sim dependency on transforms3d. (ros-navigation#4738)

The package never uses it, so don't declare it.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix incorrect doxygen comment (ros-navigation#4741)

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix missing dependency on nav2_costmap_2d (ros-navigation#4742)

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Updating error logging in Smac collision detector object (ros-navigation#4743)

* Updating error logging in Smac configs

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

* linting

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

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* [map_io] Replace std logs by rclcpp logs (ros-navigation#4720)

* replace std logs by rclcpp logs

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

* RCLCPP_DEBUG to RCLCPP_INFO for visibility

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

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Pass IDLE to on_tick, use that for initialize condition (ros-navigation#4744)

* Pass IDLE to on_tick, use that for initialize condition

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix battery sub creation bug

Signed-off-by: redvinaa <redvinaa@gmail.com>

---------

Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* nav2_costmap_2d: add missing default_value_ copy in Costmap2D operator= (ros-navigation#4753)

default_value_ is an attribute of the Costmap2D class and should be
copied along with the other attributes.

Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* improvements

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* change back to NO_INFORMATION

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Revert "change back to NO_INFORMATION"

This reverts commit 9f8c69c.

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add IsStoppedBTNode

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add topic name + reformat

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix comment

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix abs

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove log

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add getter functions for raw twist

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove unused code

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* use odomsmoother

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix formatting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* update groot

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add test

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* reset at success

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* mppi parameters_handler: Improve verbose handling (ros-navigation#4704) (ros-navigation#4711)

* mppi parameters_handler: Improve verbose handling (ros-navigation#4704)

The "verbose" parameter of the parameters_handler is
a special case that needs registration before the
dynamic parameter handler callback is registered.

In verbose mode make the parameter handler info/warn/debug
messages more expressive.

Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>

* Address review comments. (ros-navigation#4704)

* remove comments.
* Use RCLCPP_DEBUG instead of INFO for low level messages.
* Add test for trying to access parameters that are not declared.

Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>

* mppi parameters_handler: Improve static/dynamic/not defined logging (ros-navigation#4704)

Attempts to change undefined parameters will not be successful
and will log an error.

Attempts to change static parameters will be ignored, a debug
message is logged if a change in parameters is attempted.

Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>

* mppi parameters_handler: populate SetParametersResult (ros-navigation#4704)

Provide a mechanism to populate an rcl_interfaces::msg::SetParametersResult
with the reasons for unsuccessful parameter setting, so that it may be
propogated back to a set parameter client.

The mechanism provides feedback when attempting to set undefined
parameters, static parameters and could be used to validate dynamic
parameters and provide a reason for rejection.

NOTE: This changes public interface of ParametersHandler class.
s/setDynamicParamCallback/setParamCallback
s/addDynamicParamCallback/addParamCallback which takes
a callback function that is able to populate a
rcl_interfaces::msg::SetParametersResult.

In order to indicate an unsuccessful parameter change and the
reason, callback functions should append a "\n"
to the reason before appending to if it is not empty.

Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>

* mppi parameters_handler: fix reason handling and improve tests

Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>

---------

Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Added collision detection for docking (ros-navigation#4752)

* Added collision detection for docking

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

* Minor fixes

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

* Improve collision  while undocking and test

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

* Fix smoke testing

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

* Rename dock_collision_threshold

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

* Added docs and params

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

* Minor changes in README

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

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Use BT.CPP Tree::sleep (ros-navigation#4761)

* Use BT.cpp sleep

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Implement BT Loop Rate

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix formatting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix formatting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* move to nav2_behavior_tree

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix lint

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* cache

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* FIX velocity_threshold_

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix stopped Node

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add tests  to odometry_utils

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix linting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Simplify namespaced bringups and multirobot sim (ros-navigation#4715)

* WIP single robot namespacing working

Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com>

* Remove manual namespace substitution

Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com>

* Remove all multirobot specific configs

Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com>

* Refactor parsing function to common, add for rest of layers

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Fix dwb_critics test

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Add alternative API for costmap construction

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Address review feedback

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Remove remaining usage of `use_namespace` parameter

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Always join with parent namespace

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Use private parameter for parent namespace

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Fix integration test

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Revert "Use private parameter for parent namespace"

This reverts commit 0c958dc.

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Revert "Fix integration test"

This reverts commit 137d577.

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Remove global map_topic parameter

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Simplify Costmap2DROS constructor

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

---------

Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Make RecoveryNode return Running (ros-navigation#4777)

* Add IsStoppedBTNode

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add topic name + reformat

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix comment

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix abs

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove log

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add getter functions for raw twist

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove unused code

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* use odomsmoother

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix formatting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* update groot

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add test

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* reset at success

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* FIX velocity_threshold_

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix stopped Node

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add tests  to odometry_utils

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix linting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Make RecoveryNode return RUNNING

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* PR review

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add halt at the end

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Migrating twist to twiststamped in simulations for recommended default bringups (ros-navigation#4779)

* migrating from twist to twiststamped

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

* bump ci cache for updated TB4 sim files

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

* fixing collision monitor, velocity smoother unit tests

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

* fix assisted teleop test

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

* fixing docking server smoke test

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

* bust nav2 minimal tb sim cache

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* address comments

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* set response to true

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix test

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fail if out of bounds

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca>
Signed-off-by: Chris Lalancette <clalancette@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com>
Signed-off-by: agennart <antoine.gennart@quimesis.be>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>
Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Co-authored-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Tiwa Ojo <55967921+tiwaojo@users.noreply.github.com>
Co-authored-by: Chris Lalancette <clalancette@gmail.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Co-authored-by: ruffsl <roxfoxpox@gmail.com>
Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Co-authored-by: Saitama <gennartan@users.noreply.github.com>
Co-authored-by: agennart <antoine.gennart@quimesis.be>
Co-authored-by: Alberto Tudela <ajtudela@gmail.com>
Co-authored-by: Daniil Khaninaev <khaninaev@yahoo.com>
Co-authored-by: Adi Vardi <57910756+adivardi@users.noreply.github.com>
Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Vince Reda <60265874+redvinaa@users.noreply.github.com>
Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com>
Co-authored-by: aosmw <116058035+aosmw@users.noreply.github.com>
Co-authored-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Joseph Duchesne <josephgeek@gmail.com>
  • Loading branch information
20 people authored and josephduchesne committed Dec 10, 2024
1 parent 29a11e6 commit 3ff2d2a
Show file tree
Hide file tree
Showing 4 changed files with 118 additions and 22 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,14 @@ void RemoveInCollisionGoals::on_tick()
BT::NodeStatus RemoveInCollisionGoals::on_completion(
std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
{
if (!response->success) {
RCLCPP_ERROR(
node_->get_logger(),
"GetCosts service call failed");
setOutput("output_goals", input_goals_);
return BT::NodeStatus::FAILURE;
}

Goals valid_goal_poses;
for (size_t i = 0; i < response->costs.size(); ++i) {
if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,10 @@
#include "utils/test_behavior_tree_fixture.hpp"


class RemoveInCollisionGoalsService : public TestService<nav2_msgs::srv::GetCosts>
class RemoveInCollisionGoalsSucessService : public TestService<nav2_msgs::srv::GetCosts>
{
public:
RemoveInCollisionGoalsService()
RemoveInCollisionGoalsSucessService()
: TestService("/global_costmap/get_cost_global_costmap")
{}

Expand All @@ -40,9 +40,28 @@ class RemoveInCollisionGoalsService : public TestService<nav2_msgs::srv::GetCost
(void)request_header;
(void)request;
response->costs = {100, 50, 5, 254};
response->success = true;
}
};

class RemoveInCollisionGoalsFailureService : public TestService<nav2_msgs::srv::GetCosts>
{
public:
RemoveInCollisionGoalsFailureService()
: TestService("/local_costmap/get_cost_local_costmap")
{}

virtual void handle_service(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav2_msgs::srv::GetCosts::Request> request,
const std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
{
(void)request_header;
(void)request;
response->costs = {255, 50, 5, 254};
response->success = false;
}
};

class RemoveInCollisionGoalsTestFixture : public ::testing::Test
{
Expand Down Expand Up @@ -86,15 +105,16 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test
delete config_;
config_ = nullptr;
node_.reset();
server_.reset();
success_server_.reset();
factory_.reset();
}

void TearDown() override
{
tree_.reset();
}
static std::shared_ptr<RemoveInCollisionGoalsService> server_;
static std::shared_ptr<RemoveInCollisionGoalsSucessService> success_server_;
static std::shared_ptr<RemoveInCollisionGoalsFailureService> failure_server_;

protected:
static rclcpp::Node::SharedPtr node_;
Expand All @@ -106,12 +126,14 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test
rclcpp::Node::SharedPtr RemoveInCollisionGoalsTestFixture::node_ = nullptr;

BT::NodeConfiguration * RemoveInCollisionGoalsTestFixture::config_ = nullptr;
std::shared_ptr<RemoveInCollisionGoalsService>
RemoveInCollisionGoalsTestFixture::server_ = nullptr;
std::shared_ptr<RemoveInCollisionGoalsSucessService>
RemoveInCollisionGoalsTestFixture::success_server_ = nullptr;
std::shared_ptr<RemoveInCollisionGoalsFailureService>
RemoveInCollisionGoalsTestFixture::failure_server_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> RemoveInCollisionGoalsTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> RemoveInCollisionGoalsTestFixture::tree_ = nullptr;

TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals)
TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_success)
{
// create tree
std::string xml_txt =
Expand Down Expand Up @@ -141,11 +163,13 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals)

config_->blackboard->set("goals", poses);

// tick until node succeeds
while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
// tick until node is not running
tree_->rootNode()->executeTick();
while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) {
tree_->rootNode()->executeTick();
}

EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
// check that it removed the point in range
std::vector<geometry_msgs::msg::PoseStamped> output_poses;
EXPECT_TRUE(config_->blackboard->get("goals", output_poses));
Expand All @@ -156,6 +180,54 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals)
EXPECT_EQ(output_poses[2], poses[2]);
}

TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail)
{
// create tree
std::string xml_txt =
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<RemoveInCollisionGoals service_name="/local_costmap/get_cost_local_costmap" input_goals="{goals}" output_goals="{goals}" cost_threshold="253"/>
</BehaviorTree>
</root>)";

tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));

// create new goal and set it on blackboard
std::vector<geometry_msgs::msg::PoseStamped> poses;
poses.resize(4);
poses[0].pose.position.x = 0.0;
poses[0].pose.position.y = 0.0;

poses[1].pose.position.x = 0.5;
poses[1].pose.position.y = 0.0;

poses[2].pose.position.x = 1.0;
poses[2].pose.position.y = 0.0;

poses[3].pose.position.x = 2.0;
poses[3].pose.position.y = 0.0;

config_->blackboard->set("goals", poses);

// tick until node is not running
tree_->rootNode()->executeTick();
while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) {
tree_->rootNode()->executeTick();
}

// check that it failed and returned the original goals
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
std::vector<geometry_msgs::msg::PoseStamped> output_poses;
EXPECT_TRUE(config_->blackboard->get("goals", output_poses));

EXPECT_EQ(output_poses.size(), 4u);
EXPECT_EQ(output_poses[0], poses[0]);
EXPECT_EQ(output_poses[1], poses[1]);
EXPECT_EQ(output_poses[2], poses[2]);
EXPECT_EQ(output_poses[3], poses[3]);
}

int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);
Expand All @@ -164,17 +236,24 @@ int main(int argc, char ** argv)
rclcpp::init(argc, argv);

// initialize service and spin on new thread
RemoveInCollisionGoalsTestFixture::server_ =
std::make_shared<RemoveInCollisionGoalsService>();
std::thread server_thread([]() {
rclcpp::spin(RemoveInCollisionGoalsTestFixture::server_);
RemoveInCollisionGoalsTestFixture::success_server_ =
std::make_shared<RemoveInCollisionGoalsSucessService>();
std::thread success_server_thread([]() {
rclcpp::spin(RemoveInCollisionGoalsTestFixture::success_server_);
});

RemoveInCollisionGoalsTestFixture::failure_server_ =
std::make_shared<RemoveInCollisionGoalsFailureService>();
std::thread failure_server_thread([]() {
rclcpp::spin(RemoveInCollisionGoalsTestFixture::failure_server_);
});

int all_successful = RUN_ALL_TESTS();

// shutdown ROS
rclcpp::shutdown();
server_thread.join();
success_server_thread.join();
failure_server_thread.join();

return all_successful;
}
22 changes: 15 additions & 7 deletions nav2_costmap_2d/src/costmap_2d_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -825,15 +825,14 @@ void Costmap2DROS::getCostsCallback(
unsigned int mx, my;

Costmap2D * costmap = layered_costmap_->getCostmap();

response->success = true;
for (const auto & pose : request->poses) {
geometry_msgs::msg::PoseStamped pose_transformed;
transformPoseToGlobalFrame(pose, pose_transformed);
bool in_bounds = costmap->worldToMap(
pose_transformed.pose.position.x,
pose_transformed.pose.position.y, mx, my);

if (!in_bounds) {
if (!transformPoseToGlobalFrame(pose, pose_transformed)) {
RCLCPP_ERROR(
get_logger(), "Failed to transform, cannot get cost for pose (%.2f, %.2f)",
pose.pose.position.x, pose.pose.position.y);
response->success = false;
response->costs.push_back(NO_INFORMATION);
continue;
}
Expand All @@ -857,6 +856,15 @@ void Costmap2DROS::getCostsCallback(
pose_transformed.pose.position.x,
pose_transformed.pose.position.y);

bool in_bounds = costmap->worldToMap(
pose_transformed.pose.position.x,
pose_transformed.pose.position.y, mx, my);

if (!in_bounds) {
response->success = false;
response->costs.push_back(LETHAL_OBSTACLE);
continue;
}
// Get the cost at the map coordinates
response->costs.push_back(static_cast<float>(costmap->getCost(mx, my)));
}
Expand Down
3 changes: 2 additions & 1 deletion nav2_msgs/srv/GetCosts.srv
Original file line number Diff line number Diff line change
Expand Up @@ -3,4 +3,5 @@
bool use_footprint
geometry_msgs/PoseStamped[] poses
---
float32[] costs
float32[] costs
bool success

0 comments on commit 3ff2d2a

Please sign in to comment.