From 3140a6a65c2ecbcf975dac94e3f3c64c0d9efc84 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 8 Nov 2024 23:36:54 +0100 Subject: [PATCH] Jazzy sync 3: Nov 8, 2024 (#4747) * Adding non-charging dock support to docking server (for conveyers, pallots, etc) (#4627) * adding non-charging dock support to docking server Signed-off-by: Steve Macenski * docs and linting * adding unit tests Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski * Publish optimal trajectory as a Path message (#4640) * Publish optimal trajectory as a Path message Signed-off-by: Alyssa Agnissan * move publish_optimal_path to TrajectoryVisualizer + minor refactoring Signed-off-by: Alyssa Agnissan * tests added for optimal path publication Signed-off-by: Alyssa Agnissan * populate optimal path message in add() Signed-off-by: Alyssa Agnissan * move path population in add_marker Signed-off-by: Alyssa Agnissan --------- Signed-off-by: Alyssa Agnissan * [collision monitor] Select the observation sources used with each polygon (#4227) * Collision monitor: select specific observation sources for polygon Signed-off-by: asarazin * optimization Signed-off-by: asarazin * add tests Signed-off-by: asarazin --------- Signed-off-by: asarazin Co-authored-by: asarazin * Restore exported BT test utils header files after cmake revamp (#4652) (#4654) Signed-off-by: Mike Wake * fix(bt_nodes): Correct default `server_timeout` behavior by using `getInputPortOrBlackboard()` (#4649) Signed-off-by: Alan Xue * PoseStamped vector specialization (#4607) * PoseStamped vector specialization Signed-off-by: Tony Najjar * merge master Signed-off-by: Tony Najjar * add path Signed-off-by: Tony Najjar * fix size check Signed-off-by: Tony Najjar * fix test Signed-off-by: Tony Najjar * Revert "fix test" This reverts commit 51f54ebb9bc8af4cab21d46f99cb37e326fd4a3a. * fix test Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * [DWB] Option to limit velocity commands in trajectory generator (#4663) * Option to limit vel cmd through traj generator Signed-off-by: huiyulhy * Cleanup Signed-off-by: huiyulhy * fix linting Signed-off-by: huiyulhy * Update linting Signed-off-by: huiyulhy * uncrustify Signed-off-by: huiyulhy * uncrustify Signed-off-by: huiyulhy --------- Signed-off-by: huiyulhy * Adding planner server timeout for costmap waiting (#4673) * Adding planner server timeout for costmap waiting Signed-off-by: Steve Macenski * Adding controller server's costmap timeout as well Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski * fixing path longer on approach (#4622) * fixing path longer on approach Signed-off-by: Pradheep * removing the short circuit Signed-off-by: Pradheep * adding additional layer of check Signed-off-by: Pradheep --------- Signed-off-by: Pradheep * fix to bt action server logging before bt execution result being ready (#4677) Signed-off-by: DreamWest * Correct paper name for graceful controller Signed-off-by: Steve Macenski * Added missing action clients in robot_navigator(BasicNavigator) to destroy_node (#4698) * fix: added assisted_teleop_client to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo * fix: added other missing action clients to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo --------- Signed-off-by: Tiwa Ojo * Fixing SGF in MPPI and Smoother (#4669) Signed-off-by: Steve Macenski * fix: handle transition failures in all servers (#4708) * fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas * adding support for rest of servers + review comments Signed-off-by: Steve Macenski * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski * fix vel smoother unit tests Signed-off-by: Steve Macenski * fixing docking server unit testing Signed-off-by: Steve Macenski * fixing last bits Signed-off-by: Steve Macenski --------- Signed-off-by: Kemal Bektas Signed-off-by: Steve Macenski Co-authored-by: Kemal Bektas * [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1f9b8233e86ed54dfbe615b1ba56b51b6d 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 * Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev * [RotationShimController] fix: rotate to goal heading (#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 Co-authored-by: agennart * [loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (#4726) * Publish /clock from loopback sim Signed-off-by: Adi Vardi * [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 * [nav2_smac_planner] fix typos Signed-off-by: Adi Vardi * Use single quotes Signed-off-by: Adi Vardi --------- Signed-off-by: Adi Vardi * Fix incorrect doxygen comment (#4741) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> * Updating error logging in Smac collision detector object (#4743) * Updating error logging in Smac configs Signed-off-by: Steve Macenski * linting Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski * [map_io] Replace std logs by rclcpp logs (#4720) * replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * manual backport to Jazzy of 6b2e244 Signed-off-by: Steve Macenski * bump to 1.3.3 for jazzy sync Signed-off-by: Steve Macenski * fixing backport issue Signed-off-by: Steve Macenski * fixing backport of docking linking changes Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Alyssa Agnissan Signed-off-by: asarazin Signed-off-by: Mike Wake Signed-off-by: Alan Xue Signed-off-by: Tony Najjar Signed-off-by: huiyulhy Signed-off-by: Pradheep Signed-off-by: DreamWest Signed-off-by: Tiwa Ojo Signed-off-by: Kemal Bektas Signed-off-by: Daniil Khaninaev Signed-off-by: agennart Signed-off-by: Adi Vardi Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Guillaume Doisy Co-authored-by: alyquantillion <158563995+alyquantillion@users.noreply.github.com> Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: asarazin Co-authored-by: aosmw <116058035+aosmw@users.noreply.github.com> Co-authored-by: Alan Co-authored-by: Tony Najjar Co-authored-by: Huiyu Leong <26198479+huiyulhy@users.noreply.github.com> Co-authored-by: Pradheep Krishna Co-authored-by: DreamWest Co-authored-by: Tiwa Ojo <55967921+tiwaojo@users.noreply.github.com> Co-authored-by: Kemal Bektas Co-authored-by: Saitama Co-authored-by: agennart Co-authored-by: Daniil Khaninaev 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 Co-authored-by: Guillaume Doisy --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/CMakeLists.txt | 4 + .../nav2_behavior_tree/bt_action_node.hpp | 4 +- .../bt_action_server_impl.hpp | 6 +- .../bt_cancel_action_node.hpp | 4 +- .../nav2_behavior_tree/bt_service_node.hpp | 4 +- .../include/nav2_behavior_tree/bt_utils.hpp | 66 +++++ .../action/remove_passed_goals_action.hpp | 1 + .../condition/distance_traveled_condition.hpp | 1 + .../globally_updated_goal_condition.hpp | 1 + .../condition/goal_reached_condition.hpp | 1 + .../condition/goal_updated_condition.hpp | 1 + .../initial_pose_received_condition.hpp | 1 + .../path_expiring_timer_condition.hpp | 1 + .../plugins/decorator/distance_controller.hpp | 1 + .../decorator/goal_updated_controller.hpp | 1 + .../plugins/decorator/speed_controller.hpp | 1 + nav2_behavior_tree/package.xml | 2 +- .../action/remove_passed_goals_action.cpp | 1 - .../condition/distance_traveled_condition.cpp | 1 - .../globally_updated_goal_condition.cpp | 1 - .../condition/goal_reached_condition.cpp | 1 - .../condition/goal_updated_condition.cpp | 1 - .../initial_pose_received_condition.cpp | 1 - .../plugins/decorator/distance_controller.cpp | 1 - .../decorator/goal_updated_controller.cpp | 1 - .../decorator/path_longer_on_approach.cpp | 7 +- .../plugins/decorator/speed_controller.cpp | 1 - .../condition/test_path_expiring_timer.cpp | 1 - .../test_goal_updated_controller.cpp | 2 - nav2_behavior_tree/test/test_bt_utils.cpp | 130 +++++++++ nav2_behaviors/package.xml | 2 +- nav2_behaviors/src/behavior_server.cpp | 3 +- nav2_bringup/package.xml | 2 +- .../params/nav2_multirobot_params_1.yaml | 2 + .../params/nav2_multirobot_params_2.yaml | 2 + .../params/nav2_multirobot_params_all.yaml | 2 + nav2_bringup/params/nav2_params.yaml | 2 + nav2_bt_navigator/package.xml | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 6 +- .../collision_monitor_node.hpp | 11 +- .../nav2_collision_monitor/polygon.hpp | 25 +- nav2_collision_monitor/package.xml | 2 +- .../src/collision_detector_node.cpp | 11 +- .../src/collision_monitor_node.cpp | 86 +++--- nav2_collision_monitor/src/polygon.cpp | 61 +++- .../test/collision_monitor_node_test.cpp | 58 +++- nav2_collision_monitor/test/polygons_test.cpp | 100 +++++-- .../test/velocity_polygons_test.cpp | 6 + nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/CMakeLists.txt | 1 + .../nav2_controller/controller_server.hpp | 1 + nav2_controller/package.xml | 3 +- nav2_controller/src/controller_server.cpp | 25 +- .../nav2_core/controller_exceptions.hpp | 7 + nav2_core/package.xml | 2 +- .../include/nav2_costmap_2d/costmap_2d.hpp | 3 +- nav2_costmap_2d/package.xml | 2 +- nav2_docking/README.md | 19 +- nav2_docking/opennav_docking/CMakeLists.txt | 13 +- .../opennav_docking/simple_charging_dock.hpp | 1 + .../simple_non_charging_dock.hpp | 134 +++++++++ nav2_docking/opennav_docking/package.xml | 2 +- nav2_docking/opennav_docking/plugins.xml | 5 + .../opennav_docking/src/docking_server.cpp | 46 +-- .../src/simple_charging_dock.cpp | 3 +- .../src/simple_non_charging_dock.cpp | 274 ++++++++++++++++++ .../opennav_docking/test/CMakeLists.txt | 9 + .../test/test_docking_server_unit.cpp | 50 +++- .../test/test_simple_charging_dock.cpp | 3 + .../test/test_simple_non_charging_dock.cpp | 205 +++++++++++++ nav2_docking/opennav_docking_bt/package.xml | 2 +- .../opennav_docking_bt/src/dock_robot.cpp | 3 +- .../opennav_docking_core/charging_dock.hpp | 5 + .../non_charging_dock.hpp | 136 +++++++++ nav2_docking/opennav_docking_core/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- .../dwb_plugins/standard_traj_generator.hpp | 3 + nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../src/standard_traj_generator.cpp | 10 + .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_graceful_controller/README.md | 2 +- nav2_graceful_controller/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_loopback_sim/README.md | 10 +- .../nav2_loopback_sim/loopback_simulator.py | 13 + nav2_loopback_sim/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_map_server/src/map_io.cpp | 150 ++++++---- .../nav2_mppi_controller/controller.hpp | 4 +- .../tools/trajectory_visualizer.hpp | 6 +- .../nav2_mppi_controller/tools/utils.hpp | 155 +++------- nav2_mppi_controller/package.xml | 2 +- nav2_mppi_controller/src/controller.cpp | 8 +- .../src/trajectory_visualizer.cpp | 26 +- .../test/trajectory_visualizer_tests.cpp | 67 ++++- nav2_msgs/action/FollowPath.action | 1 + nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- .../include/nav2_planner/planner_server.hpp | 1 + nav2_planner/package.xml | 2 +- nav2_planner/src/planner_server.cpp | 13 +- .../package.xml | 2 +- .../nav2_rotation_shim_controller.hpp | 9 +- nav2_rotation_shim_controller/package.xml | 2 +- .../src/nav2_rotation_shim_controller.cpp | 24 +- .../test/test_shim_controller.cpp | 60 +++- nav2_rviz_plugins/package.xml | 2 +- .../nav2_simple_commander/robot_navigator.py | 4 + nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/README.md | 32 +- nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/collision_checker.cpp | 24 +- nav2_smoother/package.xml | 2 +- nav2_smoother/src/nav2_smoother.cpp | 5 +- nav2_smoother/src/savitzky_golay_smoother.cpp | 90 ++---- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- .../src/velocity_smoother.cpp | 49 +++- .../test/test_velocity_smoother.cpp | 15 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- .../src/waypoint_follower.cpp | 3 +- navigation2/package.xml | 2 +- 132 files changed, 1943 insertions(+), 493 deletions(-) create mode 100644 nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp create mode 100644 nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp create mode 100644 nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp create mode 100644 nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 82d248b489..bf1d10858c 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.3.2 + 1.3.3

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 9be5f99bce..9a58b70082 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -246,6 +246,10 @@ install(DIRECTORY test/utils/ DESTINATION include/${PROJECT_NAME}/utils/ ) +install(DIRECTORY test/utils/ + DESTINATION include/${PROJECT_NAME}/nav2_behavior_tree/test/utils +) + install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME}) install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME}) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 7276ef79ad..4e723f1ac5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -58,9 +58,7 @@ class BtActionNode : public BT::ActionNodeBase // Get the required items from the blackboard auto bt_loop_duration = config().blackboard->template get("bt_loop_duration"); - server_timeout_ = - config().blackboard->template get("server_timeout"); - getInput("server_timeout", server_timeout_); + getInputOrBlackboard("server_timeout", server_timeout_); wait_for_service_timeout_ = config().blackboard->template get("wait_for_service_timeout"); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index d2b120f44c..b19a2f17a9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -310,18 +310,18 @@ void BtActionServer::executeCallback() switch (rc) { case nav2_behavior_tree::BtStatus::SUCCEEDED: - RCLCPP_INFO(logger_, "Goal succeeded"); action_server_->succeeded_current(result); + RCLCPP_INFO(logger_, "Goal succeeded"); break; case nav2_behavior_tree::BtStatus::FAILED: - RCLCPP_ERROR(logger_, "Goal failed"); action_server_->terminate_current(result); + RCLCPP_ERROR(logger_, "Goal failed"); break; case nav2_behavior_tree::BtStatus::CANCELED: - RCLCPP_INFO(logger_, "Goal canceled"); action_server_->terminate_all(result); + RCLCPP_INFO(logger_, "Goal canceled"); break; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index b6458d881a..d63dc1e104 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -56,9 +56,7 @@ class BtCancelActionNode : public BT::ActionNodeBase callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); // Get the required items from the blackboard - server_timeout_ = - config().blackboard->template get("server_timeout"); - getInput("server_timeout", server_timeout_); + getInputOrBlackboard("server_timeout", server_timeout_); wait_for_service_timeout_ = config().blackboard->template get("wait_for_service_timeout"); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 6608eee67a..746ef61049 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -59,9 +59,7 @@ class BtServiceNode : public BT::ActionNodeBase // Get the required items from the blackboard auto bt_loop_duration = config().blackboard->template get("bt_loop_duration"); - server_timeout_ = - config().blackboard->template get("server_timeout"); - getInput("server_timeout", server_timeout_); + getInputOrBlackboard("server_timeout", server_timeout_); wait_for_service_timeout_ = config().blackboard->template get("wait_for_service_timeout"); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index 7075bc63f4..91e3e241b9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/time.hpp" #include "rclcpp/node.hpp" @@ -24,6 +25,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" namespace BT { @@ -102,6 +104,70 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) } } +/** + * @brief Parse XML string to std::vector + * @param key XML string + * @return std::vector + */ +template<> +inline std::vector convertFromString(const StringView key) +{ + // 9 real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if (parts.size() % 9 != 0) { + throw std::runtime_error("invalid number of fields for std::vector attribute)"); + } else { + std::vector poses; + for (size_t i = 0; i < parts.size(); i += 9) { + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[i])); + pose_stamped.header.frame_id = BT::convertFromString(parts[i + 1]); + pose_stamped.pose.position.x = BT::convertFromString(parts[i + 2]); + pose_stamped.pose.position.y = BT::convertFromString(parts[i + 3]); + pose_stamped.pose.position.z = BT::convertFromString(parts[i + 4]); + pose_stamped.pose.orientation.x = BT::convertFromString(parts[i + 5]); + pose_stamped.pose.orientation.y = BT::convertFromString(parts[i + 6]); + pose_stamped.pose.orientation.z = BT::convertFromString(parts[i + 7]); + pose_stamped.pose.orientation.w = BT::convertFromString(parts[i + 8]); + poses.push_back(pose_stamped); + } + return poses; + } +} + +/** + * @brief Parse XML string to nav_msgs::msg::Path + * @param key XML string + * @return nav_msgs::msg::Path + */ +template<> +inline nav_msgs::msg::Path convertFromString(const StringView key) +{ + // 9 real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if ((parts.size() - 2) % 9 != 0) { + throw std::runtime_error("invalid number of fields for Path attribute)"); + } else { + nav_msgs::msg::Path path; + path.header.stamp = rclcpp::Time(BT::convertFromString(parts[0])); + path.header.frame_id = BT::convertFromString(parts[1]); + for (size_t i = 2; i < parts.size(); i += 9) { + geometry_msgs::msg::PoseStamped pose_stamped; + path.header.stamp = rclcpp::Time(BT::convertFromString(parts[i])); + pose_stamped.header.frame_id = BT::convertFromString(parts[i + 1]); + pose_stamped.pose.position.x = BT::convertFromString(parts[i + 2]); + pose_stamped.pose.position.y = BT::convertFromString(parts[i + 3]); + pose_stamped.pose.position.z = BT::convertFromString(parts[i + 4]); + pose_stamped.pose.orientation.x = BT::convertFromString(parts[i + 5]); + pose_stamped.pose.orientation.y = BT::convertFromString(parts[i + 6]); + pose_stamped.pose.orientation.z = BT::convertFromString(parts[i + 7]); + pose_stamped.pose.orientation.w = BT::convertFromString(parts[i + 8]); + path.poses.push_back(pose_stamped); + } + return path; + } +} + /** * @brief Parse XML string to std::chrono::milliseconds * @param key XML string diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index 344afd546d..33552a24f0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -23,6 +23,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "behaviortree_cpp/action_node.h" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index 67747c62b8..77a80728dd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -24,6 +24,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 7e3e92e799..12344a5d3f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -22,6 +22,7 @@ #include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index b79fabe2f9..44e582c5f5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" #include "tf2_ros/buffer.h" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 89d4b7a573..22893e33ee 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -20,6 +20,7 @@ #include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp index 548c15268b..9958004bdf 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp @@ -17,6 +17,7 @@ #include #include "behaviortree_cpp/behavior_tree.h" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp index 8871892949..5e770b4bbd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp @@ -20,6 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" #include "nav_msgs/msg/path.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp index 7fbda19c63..38cf3369b0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp @@ -23,6 +23,7 @@ #include "tf2_ros/buffer.h" #include "behaviortree_cpp/decorator_node.h" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index bdd4171185..63a7f60655 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -23,6 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index ed454c0aa1..59a9fc5521 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -25,6 +25,7 @@ #include "nav2_util/odometry_utils.hpp" #include "behaviortree_cpp/decorator_node.h" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index fc34e0272b..04397ef742 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.3.2 + 1.3.3 Nav2 behavior tree wrappers, nodes, and utilities Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 86f5fffd6b..7e77152300 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -20,7 +20,6 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 7db1817c65..991e0ab7cf 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -20,7 +20,6 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index dbd84d8b2e..1e1e557e5f 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -16,7 +16,6 @@ #include #include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index 7024356203..e93ba5cc36 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -20,7 +20,6 @@ #include "nav2_util/node_utils.hpp" #include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index 88d329efc2..962eef70dd 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -15,7 +15,6 @@ #include #include #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp index 9d93022912..02a778a72e 100644 --- a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index 7f87695416..4d0f6fefef 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -26,7 +26,6 @@ #include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index d0de920545..b2f235dbd1 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -19,7 +19,6 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index 403c007fb0..443a29acf0 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -34,9 +34,10 @@ bool PathLongerOnApproach::isPathUpdated( nav_msgs::msg::Path & new_path, nav_msgs::msg::Path & old_path) { - return new_path != old_path && old_path.poses.size() != 0 && + return old_path.poses.size() != 0 && new_path.poses.size() != 0 && - old_path.poses.back().pose == new_path.poses.back().pose; + new_path.poses.size() != old_path.poses.size() && + old_path.poses.back().pose.position == new_path.poses.back().pose.position; } bool PathLongerOnApproach::isRobotInGoalProximity( @@ -64,7 +65,7 @@ inline BT::NodeStatus PathLongerOnApproach::tick() if (first_time_ == false) { if (old_path_.poses.empty() || new_path_.poses.empty() || - old_path_.poses.back() != new_path_.poses.back()) + old_path_.poses.back().pose != new_path_.poses.back().pose) { first_time_ = true; } diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index b8e5b3915a..87c9eb5bf1 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -19,7 +19,6 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp index 92834a15f2..e21ce307da 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp @@ -23,7 +23,6 @@ #include "utils/test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp" -#include "nav2_behavior_tree/bt_utils.hpp" using namespace std::chrono; // NOLINT using namespace std::chrono_literals; // NOLINT diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp index 2e5d34ed4f..4259e87a53 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -36,8 +36,6 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree poses1.push_back(goal1); config_->blackboard->set("goal", goal1); config_->blackboard->set("goals", poses1); - config_->input_ports["goals"] = ""; - config_->input_ports["goal"] = ""; bt_node_ = std::make_shared( "goal_updated_controller", *config_); dummy_node_ = std::make_shared(); diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index d495587955..008e0a36b7 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -194,6 +194,136 @@ TEST(PoseStampedPortTest, test_correct_syntax) EXPECT_EQ(value.pose.orientation.w, 7.0); } +TEST(PoseStampedVectorPortTest, test_wrong_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>>( + "PoseStampedVectorPortTest"); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + + xml_txt = + R"( + + + + + )"; + + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); +} + +TEST(PoseStampedVectorPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>>( + "PoseStampedVectorPortTest"); + auto tree = factory.createTreeFromText(xml_txt); + + tree = factory.createTreeFromText(xml_txt); + std::vector values; + tree.rootNode()->getInput("test", values); + EXPECT_EQ(rclcpp::Time(values[0].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values[0].header.frame_id, "map"); + EXPECT_EQ(values[0].pose.position.x, 1.0); + EXPECT_EQ(values[0].pose.position.y, 2.0); + EXPECT_EQ(values[0].pose.position.z, 3.0); + EXPECT_EQ(values[0].pose.orientation.x, 4.0); + EXPECT_EQ(values[0].pose.orientation.y, 5.0); + EXPECT_EQ(values[0].pose.orientation.z, 6.0); + EXPECT_EQ(values[0].pose.orientation.w, 7.0); + EXPECT_EQ(rclcpp::Time(values[1].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values[1].header.frame_id, "odom"); + EXPECT_EQ(values[1].pose.position.x, 8.0); + EXPECT_EQ(values[1].pose.position.y, 9.0); + EXPECT_EQ(values[1].pose.position.z, 10.0); + EXPECT_EQ(values[1].pose.orientation.x, 11.0); + EXPECT_EQ(values[1].pose.orientation.y, 12.0); + EXPECT_EQ(values[1].pose.orientation.z, 13.0); + EXPECT_EQ(values[1].pose.orientation.w, 14.0); +} + +TEST(PathPortTest, test_wrong_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>( + "PathPortTest"); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + + xml_txt = + R"( + + + + + )"; + + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); +} + +TEST(PathPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>( + "PathPortTest"); + auto tree = factory.createTreeFromText(xml_txt); + + tree = factory.createTreeFromText(xml_txt); + nav_msgs::msg::Path path; + tree.rootNode()->getInput("test", path); + EXPECT_EQ(rclcpp::Time(path.header.stamp).nanoseconds(), 0); + EXPECT_EQ(path.header.frame_id, "map"); + EXPECT_EQ(rclcpp::Time(path.poses[0].header.stamp).nanoseconds(), 0); + EXPECT_EQ(path.poses[0].header.frame_id, "map"); + EXPECT_EQ(path.poses[0].pose.position.x, 1.0); + EXPECT_EQ(path.poses[0].pose.position.y, 2.0); + EXPECT_EQ(path.poses[0].pose.position.z, 3.0); + EXPECT_EQ(path.poses[0].pose.orientation.x, 4.0); + EXPECT_EQ(path.poses[0].pose.orientation.y, 5.0); + EXPECT_EQ(path.poses[0].pose.orientation.z, 6.0); + EXPECT_EQ(path.poses[0].pose.orientation.w, 7.0); + EXPECT_EQ(rclcpp::Time(path.poses[1].header.stamp).nanoseconds(), 0); + EXPECT_EQ(path.poses[1].header.frame_id, "map"); + EXPECT_EQ(path.poses[1].pose.position.x, 8.0); + EXPECT_EQ(path.poses[1].pose.position.y, 9.0); + EXPECT_EQ(path.poses[1].pose.position.z, 10.0); + EXPECT_EQ(path.poses[1].pose.orientation.x, 11.0); + EXPECT_EQ(path.poses[1].pose.orientation.y, 12.0); + EXPECT_EQ(path.poses[1].pose.orientation.z, 13.0); + EXPECT_EQ(path.poses[1].pose.orientation.w, 14.0); +} + TEST(MillisecondsPortTest, test_correct_syntax) { std::string xml_txt = diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 596ddb47a2..09ee0b1306 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.3.2 + 1.3.3 Nav2 behavior server Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 1017bf595a..a04aea4031 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -78,7 +78,7 @@ BehaviorServer::~BehaviorServer() } nav2_util::CallbackReturn -BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -91,6 +91,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) behavior_types_.resize(behavior_ids_.size()); if (!loadBehaviorPlugins()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } setupResourcesForBehaviorPlugins(); diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index b808bad78a..2ac0335369 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.3.2 + 1.3.3 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 493c53fc2b..319ebd41ac 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -198,6 +199,7 @@ map_server: planner_server: ros__parameters: planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 605c86c66b..86e2b78e5e 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -197,6 +198,7 @@ map_server: planner_server: ros__parameters: planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 4bde87b47d..b6b27887a3 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -230,6 +231,7 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 + costmap_update_timeout: 1.0 planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 2581635e7f..2027f4c0e1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -276,6 +277,7 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 35f545c0fa..503e27d42e 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.3.2 + 1.3.3 Nav2 BT Navigator Server Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9ae6401674..c49fae0e2b 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -56,7 +56,7 @@ BtNavigator::~BtNavigator() } nav2_util::CallbackReturn -BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -133,6 +133,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create navigator id %s." " Exception: %s", navigator_ids[i].c_str(), ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -141,11 +142,12 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Activating"); for (size_t i = 0; i != navigators_.size(); i++) { if (!navigators_[i]->on_activate()) { + on_deactivate(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index ce4d96504b..c6caeeb5b2 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -159,28 +160,30 @@ class CollisionMonitor : public nav2_util::LifecycleNode /** * @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key and + * array of source's 2D obstacle points as value * @param velocity Desired robot velocity * @param robot_action Output processed robot action * @return True if returned action is caused by current polygon, otherwise false */ bool processStopSlowdownLimit( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const; /** * @brief Processes APPROACH action type * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key and + * array of source's 2D obstacle points as value * @param velocity Desired robot velocity * @param robot_action Output processed robot action * @return True if returned action is caused by current polygon, otherwise false */ bool processApproach( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index fdc9e07b7a..6bb3b5dd01 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" @@ -126,6 +127,12 @@ class Polygon */ virtual void getPolygon(std::vector & poly) const; + /** + * @brief Obtains the name of the observation sources for current polygon. + * @return Names of the observation sources + */ + std::vector getSourcesNames() const; + /** * @brief Returns true if polygon points were set. * Otherwise, prints a warning and returns false. @@ -145,16 +152,28 @@ class Polygon */ virtual int getPointsInside(const std::vector & points) const; + /** + * @brief Gets number of points inside given polygon + * @param sources_collision_points_map Map containing source name as key, + * and input array of source's points to be checked as value + * @return Number of points inside polygon, + * for sources in map that are associated with current polygon. + * If there are no points, returns zero value. + */ + virtual int getPointsInside( + const std::unordered_map> & sources_collision_points_map) const; + /** * @brief Obtains estimated (simulated) time before a collision. * Applicable for APPROACH model. - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key, + * and input array of source's 2D obstacle points as value * @param velocity Simulated robot velocity * @return Estimated time before a collision. If there is no collision, * return value will be negative. */ double getCollisionTime( - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity) const; /** @@ -269,6 +288,8 @@ class Polygon rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; + /// @brief Name of the observation sources to check for polygon + std::vector sources_names_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 6dc1571917..1b5e8a3450 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.3.2 + 1.3.3 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 4d8da1e241..7f87bc1bb9 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -40,7 +40,7 @@ CollisionDetector::~CollisionDetector() } nav2_util::CallbackReturn -CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionDetector::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -60,6 +60,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -168,10 +169,6 @@ bool CollisionDetector::getParameters() const bool base_shift_correction = get_parameter("base_shift_correction").as_bool(); - if (!configurePolygons(base_frame_id, transform_tolerance)) { - return false; - } - if (!configureSources( base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) @@ -179,6 +176,10 @@ bool CollisionDetector::getParameters() return false; } + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + return true; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 766ed713cd..2b0991c994 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -42,7 +42,7 @@ CollisionMonitor::~CollisionMonitor() } nav2_util::CallbackReturn -CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -60,6 +60,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -90,6 +91,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -270,10 +272,6 @@ bool CollisionMonitor::getParameters( stop_pub_timeout_ = rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double()); - if (!configurePolygons(base_frame_id, transform_tolerance)) { - return false; - } - if ( !configureSources( base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) @@ -281,6 +279,10 @@ bool CollisionMonitor::getParameters( return false; } + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + return true; } @@ -412,17 +414,21 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } // Points array collected from different data sources in a robot base frame - std::vector collision_points; + std::unordered_map> sources_collision_points_map; // By default - there is no action Action robot_action{DO_NOTHING, cmd_vel_in, ""}; // Polygon causing robot action (if any) std::shared_ptr action_polygon; - // Fill collision_points array from different data sources + // Fill collision points array from different data sources + auto marker_array = std::make_unique(); for (std::shared_ptr source : sources_) { + auto iter = sources_collision_points_map.insert( + {source->getSourceName(), std::vector()}); + if (source->getEnabled()) { - if (!source->getData(curr_time, collision_points) && + if (!source->getData(curr_time, iter.first->second) && source->getSourceTimeout().seconds() != 0.0) { action_polygon = nullptr; @@ -434,33 +440,35 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: break; } } + + if (collision_points_marker_pub_->get_subscription_count() > 0) { + // visualize collision points with markers + visualization_msgs::msg::Marker marker; + marker.header.frame_id = get_parameter("base_frame_id").as_string(); + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = "collision_points_" + source->getSourceName(); + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.color.r = 1.0; + marker.color.a = 1.0; + marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = true; + + for (const auto & point : iter.first->second) { + geometry_msgs::msg::Point p; + p.x = point.x; + p.y = point.y; + p.z = 0.0; + marker.points.push_back(p); + } + marker_array->markers.push_back(marker); + } } if (collision_points_marker_pub_->get_subscription_count() > 0) { - // visualize collision points with markers - auto marker_array = std::make_unique(); - visualization_msgs::msg::Marker marker; - marker.header.frame_id = get_parameter("base_frame_id").as_string(); - marker.header.stamp = rclcpp::Time(0, 0); - marker.ns = "collision_points"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::POINTS; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale.x = 0.02; - marker.scale.y = 0.02; - marker.color.r = 1.0; - marker.color.a = 1.0; - marker.lifetime = rclcpp::Duration(0, 0); - marker.frame_locked = true; - - for (const auto & point : collision_points) { - geometry_msgs::msg::Point p; - p.x = point.x; - p.y = point.y; - p.z = 0.0; - marker.points.push_back(p); - } - marker_array->markers.push_back(marker); collision_points_marker_pub_->publish(std::move(marker_array)); } @@ -479,12 +487,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: const ActionType at = polygon->getActionType(); if (at == STOP || at == SLOWDOWN || at == LIMIT) { // Process STOP/SLOWDOWN for the selected polygon - if (processStopSlowdownLimit(polygon, collision_points, cmd_vel_in, robot_action)) { + if (processStopSlowdownLimit( + polygon, sources_collision_points_map, cmd_vel_in, robot_action)) + { action_polygon = polygon; } } else if (at == APPROACH) { // Process APPROACH for the selected polygon - if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) { + if (processApproach(polygon, sources_collision_points_map, cmd_vel_in, robot_action)) { action_polygon = polygon; } } @@ -506,7 +516,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: bool CollisionMonitor::processStopSlowdownLimit( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const { @@ -514,7 +524,7 @@ bool CollisionMonitor::processStopSlowdownLimit( return false; } - if (polygon->getPointsInside(collision_points) >= polygon->getMinPoints()) { + if (polygon->getPointsInside(sources_collision_points_map) >= polygon->getMinPoints()) { if (polygon->getActionType() == STOP) { // Setting up zero velocity for STOP model robot_action.polygon_name = polygon->getName(); @@ -561,7 +571,7 @@ bool CollisionMonitor::processStopSlowdownLimit( bool CollisionMonitor::processApproach( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const { @@ -570,7 +580,7 @@ bool CollisionMonitor::processApproach( } // Obtain time before a collision - const double collision_time = polygon->getCollisionTime(collision_points, velocity); + const double collision_time = polygon->getCollisionTime(sources_collision_points_map, velocity); if (collision_time >= 0.0) { // If collision will occurr, reduce robot speed const double change_ratio = collision_time / polygon->getTimeBeforeCollision(); diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 523b446b25..19ac5053de 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -157,6 +157,11 @@ double Polygon::getTimeBeforeCollision() const return time_before_collision_; } +std::vector Polygon::getSourcesNames() const +{ + return sources_names_; +} + void Polygon::getPolygon(std::vector & poly) const { poly = poly_; @@ -229,19 +234,48 @@ int Polygon::getPointsInside(const std::vector & points) const return num; } +int Polygon::getPointsInside( + const std::unordered_map> & sources_collision_points_map) const +{ + int num = 0; + std::vector polygon_sources_names = getSourcesNames(); + + // Sum the number of points from all sources associated with current polygon + for (const auto & source_name : polygon_sources_names) { + const auto & iter = sources_collision_points_map.find(source_name); + if (iter != sources_collision_points_map.end()) { + num += getPointsInside(iter->second); + } + } + + return num; +} + double Polygon::getCollisionTime( - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity) const { // Initial robot pose is {0,0} in base_footprint coordinates Pose pose = {0.0, 0.0, 0.0}; Velocity vel = velocity; + std::vector polygon_sources_names = getSourcesNames(); + std::vector collision_points; + + // Save all points coming from the sources associated with current polygon + for (const auto & source_name : polygon_sources_names) { + const auto & iter = sources_collision_points_map.find(source_name); + if (iter != sources_collision_points_map.end()) { + collision_points.insert(collision_points.end(), iter->second.begin(), iter->second.end()); + } + } + // Array of points transformed to the frame concerned with pose on each simulation step std::vector points_transformed = collision_points; // Check static polygon - if (getPointsInside(points_transformed) >= min_points_) { + if (getPointsInside(collision_points) >= min_points_) { return 0.0; } @@ -392,6 +426,29 @@ bool Polygon::getCommonParameters( node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); } } + + // By default, use all observation sources for polygon + nav2_util::declare_parameter_if_not_declared( + node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); + const std::vector observation_sources = + node->get_parameter("observation_sources").as_string_array(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(observation_sources)); + sources_names_ = node->get_parameter(polygon_name_ + ".sources_names").as_string_array(); + + // Check the observation sources configured for polygon are defined + for (auto source_name : sources_names_) { + if (std::find(observation_sources.begin(), observation_sources.end(), source_name) == + observation_sources.end()) + { + RCLCPP_ERROR_STREAM( + logger_, + "Observation source [" << source_name << + "] configured for polygon [" << getName() << + "] is not defined as one of the node's observation_source!"); + return false; + } + } } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 40415868f5..f297ca363a 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -143,7 +143,8 @@ class Tester : public ::testing::Test void setCommonParameters(); void addPolygon( const std::string & polygon_name, const PolygonType type, - const double size, const std::string & at); + const double size, const std::string & at, + const std::vector & sources_names = std::vector()); void addPolygonVelocitySubPolygon( const std::string & polygon_name, const std::string & sub_polygon_name, const double linear_min, const double linear_max, @@ -309,7 +310,8 @@ void Tester::setCommonParameters() void Tester::addPolygon( const std::string & polygon_name, const PolygonType type, - const double size, const std::string & at) + const double size, const std::string & at, + const std::vector & sources_names) { if (type == POLYGON) { cm_->declare_parameter( @@ -408,6 +410,13 @@ void Tester::addPolygon( polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name)); cm_->set_parameter( rclcpp::Parameter(polygon_name + ".polygon_pub_topic", polygon_name)); + + if (!sources_names.empty()) { + cm_->declare_parameter( + polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".sources_names", sources_names)); + } } void Tester::addPolygonVelocitySubPolygon( @@ -1499,9 +1508,10 @@ TEST_F(Tester, testCollisionPointsMarkers) // Share TF sendTransforms(curr_time); + // No source published, empty marker array published publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCollisionPointsMarker(500ms)); - ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + ASSERT_EQ(collision_points_marker_msg_->markers.size(), 0u); publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); @@ -1580,6 +1590,48 @@ TEST_F(Tester, testVelocityPolygonStop) cm_->stop(); } +TEST_F(Tester, testSourceAssociatedToPolygon) +{ + // Set Collision Monitor parameters: + // - 2 sources (scan and range) + // - 1 stop polygon associated to range source + // - 1 slowdown polygon (associated with all sources by default) + setCommonParameters(); + addSource(SCAN_NAME, SCAN); + addSource(RANGE_NAME, RANGE); + std::vector range_only_sources_names = {RANGE_NAME}; + std::vector all_sources_names = {SCAN_NAME, RANGE_NAME}; + addPolygon("StopOnRangeSource", POLYGON, 1.0, "stop", range_only_sources_names); + addPolygon("SlowdownOnAllSources", POLYGON, 1.0, "slowdown"); + setVectors({"StopOnRangeSource", "SlowdownOnAllSources"}, {SCAN_NAME, RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + + // Publish sources so that : + // - scan obstacle is in polygons + // - range obstacle is far away from polygons + publishScan(0.5, curr_time); + publishRange(4.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + + // Publish cmd vel + publishCmdVel(0.5, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + + // Since the stop polygon is only checking range source, slowdown action should be applied + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, SLOWDOWN); + ASSERT_EQ(action_state_->polygon_name, "SlowdownOnAllSources"); + + // Stop Collision Monitor node + cm_->stop(); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index c703e12557..a0dbf9fcc9 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -46,6 +46,7 @@ static const char POLYGON_SUB_TOPIC[]{"polygon_sub"}; static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; static const char POLYGON_NAME[]{"TestPolygon"}; static const char CIRCLE_NAME[]{"TestCircle"}; +static const char OBSERVATION_SOURCE_NAME[]{"source"}; static const std::vector SQUARE_POLYGON { 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}; static const std::vector ARBITRARY_POLYGON { @@ -235,7 +236,11 @@ class Tester : public ::testing::Test protected: // Working with parameters - void setCommonParameters(const std::string & polygon_name, const std::string & action_type); + void setCommonParameters( + const std::string & polygon_name, const std::string & action_type, + const std::vector & observation_sources = + std::vector({OBSERVATION_SOURCE_NAME}), + const std::vector & sources_names = std::vector()); void setPolygonParameters( const char * points, const bool is_static); @@ -289,7 +294,10 @@ Tester::~Tester() tf_buffer_.reset(); } -void Tester::setCommonParameters(const std::string & polygon_name, const std::string & action_type) +void Tester::setCommonParameters( + const std::string & polygon_name, const std::string & action_type, + const std::vector & observation_sources, + const std::vector & sources_names) { test_node_->declare_parameter( polygon_name + ".action_type", rclcpp::ParameterValue(action_type)); @@ -336,6 +344,18 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(POLYGON_PUB_TOPIC)); test_node_->set_parameter( rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC)); + + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(observation_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", observation_sources)); + + if (!sources_names.empty()) { + test_node_->declare_parameter( + polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".sources_names", sources_names)); + } } void Tester::setPolygonParameters( @@ -862,25 +882,26 @@ TEST_F(Tester, testPolygonGetCollisionTime) // Forward movement check nav2_collision_monitor::Velocity vel{0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.2 m ahead the footprint (0.5 m) - std::vector points{{0.7, -0.01}, {0.7, 0.01}}; + std::unordered_map> points_map; + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.7, -0.01}, {0.7, 0.01}}}); // Collision is expected to be ~= 0.2 m / 0.5 m/s seconds - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); // Backward movement check vel = {-0.5, 0.0, 0.0}; // 0.5 m/s backward movement // Two points 0.2 m behind the footprint (0.5 m) - points.clear(); - points = {{-0.7, -0.01}, {-0.7, 0.01}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.7, -0.01}, {-0.7, 0.01}}}); // Collision is expected to be in ~= 0.2 m / 0.5 m/s seconds - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); // Sideway movement check vel = {0.0, 0.5, 0.0}; // 0.5 m/s sideway movement // Two points 0.1 m ahead the footprint (0.5 m) - points.clear(); - points = {{-0.01, 0.6}, {0.01, 0.6}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.01, 0.6}, {0.01, 0.6}}}); // Collision is expected to be in ~= 0.1 m / 0.5 m/s seconds - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.2, SIMULATION_TIME_STEP); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.2, SIMULATION_TIME_STEP); // Rotation check vel = {0.0, 0.0, 1.0}; // 1.0 rad/s rotation @@ -894,27 +915,27 @@ TEST_F(Tester, testPolygonGetCollisionTime) // | ' | // ----------- // ' - points.clear(); - points = {{0.49, -0.01}, {0.49, 0.01}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.49, -0.01}, {0.49, 0.01}}}); // Collision is expected to be in ~= 45 degrees * M_PI / (180 degrees * 1.0 rad/s) seconds double exp_res = 45 / 180 * M_PI; - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), exp_res, EPSILON); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), exp_res, EPSILON); // Two points are already inside footprint vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points inside - points.clear(); - points = {{0.1, -0.01}, {0.1, 0.01}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.1, -0.01}, {0.1, 0.01}}}); // Collision already appeared: collision time should be 0 - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.0, EPSILON); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.0, EPSILON); // All points are out of simulation prediction vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.6 m ahead the footprint (0.5 m) - points.clear(); - points = {{1.1, -0.01}, {1.1, 0.01}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{1.1, -0.01}, {1.1, 0.01}}}); // There is no collision: return value should be negative - EXPECT_LT(polygon_->getCollisionTime(points, vel), 0.0); + EXPECT_LT(polygon_->getCollisionTime(points_map, vel), 0.0); } TEST_F(Tester, testPolygonPublish) @@ -945,6 +966,9 @@ TEST_F(Tester, testPolygonDefaultVisualize) std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); + std::vector observation_sources = {OBSERVATION_SOURCE_NAME}; + test_node_->declare_parameter("observation_sources", rclcpp::ParameterValue(observation_sources)); + test_node_->set_parameter(rclcpp::Parameter("observation_sources", observation_sources)); setPolygonParameters(SQUARE_POLYGON_STR, true); // Create new polygon @@ -977,6 +1001,44 @@ TEST_F(Tester, testPolygonInvalidPointsString) ASSERT_FALSE(polygon_->configure()); } +TEST_F(Tester, testPolygonSourceDefaultAssociation) +{ + // By default, a polygon uses all observation sources + std::vector all_sources = {"source_1", "source_2", "source_3"}; + setCommonParameters(POLYGON_NAME, "stop", all_sources); // no polygon sources names specified + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + ASSERT_EQ(polygon_->getSourcesNames(), all_sources); +} + +TEST_F(Tester, testPolygonSourceInvalidAssociation) +{ + // If a source is not defined as observation source, polygon cannot use it: config should fail + setCommonParameters( + POLYGON_NAME, "stop", {"source_1", "source_2", "source_3"}, {"source_1", "source_4"}); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testPolygonSourceAssociation) +{ + // Checks that, if declared, only the specific sources associated to polygon are saved + std::vector poly_sources = {"source_1", "source_3"}; + setCommonParameters(POLYGON_NAME, "stop", {"source_1", "source_2", "source_3"}, poly_sources); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + ASSERT_EQ(polygon_->getSourcesNames(), poly_sources); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 4fd4f8bc0b..7d57cc00de 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -208,6 +208,12 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(POLYGON_PUB_TOPIC)); test_node_->set_parameter( rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC)); + + std::vector default_observation_sources = {"source"}; + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(default_observation_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", default_observation_sources)); } void Tester::setVelocityPolygonParameters(const bool is_holonomic) diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 585ca702bb..68f0c8e6f3 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.3.2 + 1.3.3 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index b29a0e4c67..b781f16b57 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.3.2 + 1.3.3 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index ded67fbf41..e83f91c7cd 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(angles REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(nav2_costmap_2d REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_msgs REQUIRED) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 0f250dee14..e2d3fd497a 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -267,6 +267,7 @@ class ControllerServer : public nav2_util::LifecycleNode double failure_tolerance_; bool use_realtime_priority_; + rclcpp::Duration costmap_update_timeout_; // Whether we've published the single controller warning yet geometry_msgs::msg::PoseStamped end_pose_; diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index ca202c2d1a..4bab7d28ef 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.3.2 + 1.3.3 Controller action interface Carl Delsey Apache-2.0 @@ -16,6 +16,7 @@ nav2_util nav2_msgs nav_2d_utils + nav2_costmap_2d nav_2d_msgs nav2_core pluginlib diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 4f2636beee..82557b1da1 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -44,7 +44,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, - default_types_{"dwb_core::DWBLocalPlanner"} + default_types_{"dwb_core::DWBLocalPlanner"}, + costmap_update_timeout_(300ms) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -64,6 +65,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0)); declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false)); declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true)); + declare_parameter("costmap_update_timeout", 0.30); // 300ms // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -80,7 +82,7 @@ ControllerServer::~ControllerServer() } nav2_util::CallbackReturn -ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { auto node = shared_from_this(); @@ -149,6 +151,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -175,6 +178,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create goal checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -203,6 +207,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create controller. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -223,6 +228,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + double costmap_update_timeout_dbl; + get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); + costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); + // Create the action server that we implement with our followPath method // This may throw due to real-time prioritzation if user doesn't have real-time permissions try { @@ -235,6 +244,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -492,7 +502,11 @@ void ControllerServer::computeControl() // Don't compute a trajectory until costmap is valid (after clear costmap) rclcpp::Rate r(100); + auto waiting_start = now(); while (!costmap_ros_->isCurrent()) { + if (now() - waiting_start > costmap_update_timeout_) { + throw nav2_core::ControllerTimedOut("Costmap timed out waiting for update"); + } r.sleep(); } @@ -555,6 +569,13 @@ void ControllerServer::computeControl() result->error_code = Action::Result::INVALID_PATH; action_server_->terminate_current(result); return; + } catch (nav2_core::ControllerTimedOut & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Result::CONTROLLER_TIMED_OUT; + action_server_->terminate_current(result); + return; } catch (nav2_core::ControllerException & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); diff --git a/nav2_core/include/nav2_core/controller_exceptions.hpp b/nav2_core/include/nav2_core/controller_exceptions.hpp index 5f87c4c287..2d75836fa5 100644 --- a/nav2_core/include/nav2_core/controller_exceptions.hpp +++ b/nav2_core/include/nav2_core/controller_exceptions.hpp @@ -69,6 +69,13 @@ class NoValidControl : public ControllerException : ControllerException(description) {} }; +class ControllerTimedOut : public ControllerException +{ +public: + explicit ControllerTimedOut(const std::string & description) + : ControllerException(description) {} +}; + } // namespace nav2_core #endif // NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_ diff --git a/nav2_core/package.xml b/nav2_core/package.xml index c135671ccf..de852df2d6 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.3.2 + 1.3.3 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 005dd205d8..927e580612 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -150,8 +150,7 @@ class Costmap2D /** * @brief Get the cost of a cell in the costmap - * @param mx The x coordinate of the cell - * @param my The y coordinate of the cell + * @param index The cell index * @return The cost of the cell */ unsigned char getCost(unsigned int index) const; diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index a73b20fbd0..6135472202 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.3.2 + 1.3.3 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_docking/README.md b/nav2_docking/README.md index c1aa1b10da..fba8ca1139 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -1,6 +1,6 @@ # Open Navigation's Nav2 Docking Framework -This package contains an automatic robot docking framework & auxiliary tools. It uses plugin `dock` implementations for a particular platform to enable the framework to generalize to robots of many different kinematic models, charging methods, sensor modalities, and so on. It can also handle a database of many different docking locations and dock models to handle a heterogeneous environment. This task server is designed be called by an application BT or autonomy application to dock once completed with tasks or battery is low -- _not_ within the navigate-to-pose action itself (though `undock` may be called from inside navigate actions!). +This package contains an automatic robot docking framework & auxiliary tools. It uses plugin `dock` implementations for a particular platform to enable the framework to generalize to robots of many different kinematic models, charging methods, sensor modalities, non-charging dock request needs, and so on. It can also handle a database of many different docking locations and dock models to handle a heterogeneous environment. This task server is designed be called by an application BT or autonomy application to dock once completed with tasks or battery is low -- _not_ within the navigate-to-pose action itself (though `undock` may be called from inside navigate actions!). This work is sponsored by [NVIDIA](https://www.nvidia.com/en-us/) and created by [Open Navigation LLC](https://opennav.org). @@ -24,9 +24,9 @@ The Docking Framework has 5 main components: - `Navigator`: A NavigateToPose action client to navigate the robot to the dock's staging pose if not with the prestaging tolerances - `DockDatabase`: A database of dock instances in an environment and their associated interfaces for transacting with each type. An arbitrary number of types are supported. - `Controller`: A spiral-based graceful controller to use for the vision-control loop for docking -- `ChargingDock`: Plugins that describe the dock and how to transact with it (check if charging, detection, etc). You can find this plugin header in the `opennav_docking_core` package. +- `ChargingDock` and `NonChargingDock`: Plugins that describe the dock and how to transact with it (check if charging, detection, etc). You can find these plugin headers in the `opennav_docking_core` package. -The `ChargingDock` plugins are the heart of the customizability of the framework to support any type of charging dock for any kind of robot. The `DockDatabase` is how you describe where these docks exist in your environment to interact with and any of them may be used in your docking request. +The `ChargingDock` and `NonChargingDock` plugins are the heart of the customizability of the framework to support any type of charging or non-charging dock for any kind of robot. This means you can both dock with charging stations, as well as non-charging infrastructure such as static locations (ex. conveyers) or dynamic locations (ex. pallets). The `DockDatabase` is how you describe where these docks exist in your environment to interact with and any of them may be used in your docking request. For dynamic locations like docking with movable objects, the action request can include an approximate docking location that uses vision-control loops to refine motion into it. The docking procedure is as follows: 1. Take action request and obtain the dock's plugin and its pose @@ -34,7 +34,7 @@ The docking procedure is as follows: 3. Use the dock's plugin to initially detect the dock and return the docking pose 4. Enter a vision-control loop where the robot attempts to reach the docking pose while its actively being refined by the vision system 5. Exit the vision-control loop once contact has been detected or charging has started -6. Wait until charging starts and return success. +6. Wait until charging starts (if applicable) and return success. If anywhere this procedure is unsuccessful, `N` retries may be made by driving back to the dock's staging pose and trying again. If still unsuccessful, it will return a failure code to indicate what kind of failure occurred to the client. @@ -74,7 +74,7 @@ This service exists to potentially reload the dock server's known dock database There are two unique elements to consider in specifying docks: dock _instances_ and dock _plugins_. Dock instances are instances of a particular dock in the map, as the database may contain many known docks (and you can specify which by name you'd like to dock at). Dock plugins are the model of dock that each is an instance of. The plugins contain the capabilities to generically detect and connect to a particular dock model. This separation allows us to efficiently enable many individual docking locations of potentially several different revisions with different attributes. -The **dock plugins** are specified in the parameter file as shown below. If you're familiar with plugins in other Nav2 servers, this should look like a familiar design pattern. Note that there is no specific information about the dock's pose or instances. These are generic attributes about the dock revision (such as staging pose, enable charging command, detection method, etc). You can add additional parameters in the dock's namespace as you choose (for example `timeout`). +The **dock plugins** are specified in the parameter file as shown below. If you're familiar with plugins in other Nav2 servers, this should look like a familiar design pattern. Note that there is no specific information about the dock's pose or instances. These are generic attributes about the dock revision (such as staging pose, enable charging command, detection method, etc). You can add additional parameters in the dock's namespace as you choose (for example `timeout`). These can be of both charging and non-charging types. ``` dock_plugins: ["dockv1", "dockv3"] @@ -145,23 +145,26 @@ There are two functions used during dock approach: * `isCharging`: The approach stops when the robot reports `isDocked`, then we wait for charging to start by calling `isCharging`. * This may be implemented using the `sensor_msgs/BatteryState` message to check the power status or for charging current. + * Used for charging-typed plugins. Similarly, there are two functions used during undocking: * `disableCharging`: This function is called before undocking commences to help prevent wear on the charge contacts. If the charge dock supports turning off the charge current, it should be done here. + * Used for charging-typed plugins. * `hasStoppedCharging`: This function is called while the controller is undocking. Undocking is successful when charging has stopped and the robot has returned to the staging pose. + * Used for charging-typed plugins. Keep in mind that the docking and undocking functions should return quickly as they will be called inside the control loop. Also make sure that `isDocked` should return true if `isCharging` returns true. -### Simple Charging Dock Plugin +### Simple (Non-) Charging Dock Plugins -The `SimpleChargingDock` plugin is an example with many common options which may be fully functional for +The `SimpleChargingDock` and `SimpleNonChargingDock` plugins are examples with many common options which may be fully functional for some robots. `getStagingPose` applys a parameterized translational and rotational offset to the dock pose to obtain the staging pose. @@ -174,7 +177,7 @@ During the docking approach, there are two options for detecting `isDocked`: 1. We can check the joint states of the wheels if the current has spiked above a set threshold to indicate that the robot has made contact with the dock or other physical object. 2. The dock pose is compared with the robot pose and `isDocked` returns true when the distance drops below the specified `docking_threshold`. -The `isCharging` and `hasStoppedCharging` functions have two options: +The `isCharging` and `hasStoppedCharging` functions have two options, when using the charging dock type: 1. Subscribing to a `sensor_msgs/BatteryState` message on topic `battery_state`. The robot is considered charging when the `current` field of the message exceeds the `charging_threshold`. 2. We can return that we are charging is `isDocked() = true`, which is useful for initial testing or low-reliability docking until battery state or similar information is available. diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index e50590d3e4..9278234d99 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -107,9 +107,20 @@ ament_target_dependencies(simple_charging_dock ) target_link_libraries(simple_charging_dock pose_filter) + + +add_library(simple_non_charging_dock SHARED + src/simple_non_charging_dock.cpp +) +ament_target_dependencies(simple_non_charging_dock + ${dependencies} +) +target_link_libraries(simple_non_charging_dock pose_filter) + pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml) -install(TARGETS ${library_name} controller pose_filter simple_charging_dock +install(TARGETS ${library_name} controller pose_filter simple_charging_dock simple_non_charging_dock + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp index 2b877442f5..49adf4124c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp @@ -142,6 +142,7 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock double charging_threshold_; // If not using an external pose reference, this is the distance threshold double docking_threshold_; + std::string base_frame_id_; // Offset for staging pose relative to dock pose double staging_x_offset_; double staging_yaw_offset_; diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp new file mode 100644 index 0000000000..0f25828944 --- /dev/null +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp @@ -0,0 +1,134 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ +#define OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +#include "opennav_docking_core/non_charging_dock.hpp" +#include "opennav_docking/pose_filter.hpp" + +namespace opennav_docking +{ + +class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock +{ +public: + /** + * @brief Constructor + */ + SimpleNonChargingDock() + : NonChargingDock() + {} + + /** + * @param parent pointer to user's node + * @param name The name of this planner + * @param tf A pointer to a TF buffer + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf); + + /** + * @brief Method to cleanup resources used on shutdown. + */ + virtual void cleanup() {} + + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + virtual void activate() {} + + /** + * @brief Method to deactive Behavior and any threads involved in execution. + */ + virtual void deactivate() {} + + /** + * @brief Method to obtain the dock's staging pose. This method should likely + * be using TF and the dock's pose information to find the staging pose from + * a static or parameterized staging pose relative to the docking pose + * @param pose Dock with pose + * @param frame Dock's frame of pose + * @return PoseStamped of staging pose in the specified frame + */ + virtual geometry_msgs::msg::PoseStamped getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame); + + /** + * @brief Method to obtain the refined pose of the dock, usually based on sensors + * @param pose The initial estimate of the dock pose. + * @param frame The frame of the initial estimate. + */ + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id); + + /** + * @copydoc opennav_docking_core::ChargingDock::isDocked + */ + virtual bool isDocked(); + +protected: + void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state); + + // Optionally subscribe to a detected dock pose topic + rclcpp::Subscription::SharedPtr dock_pose_sub_; + rclcpp::Publisher::SharedPtr dock_pose_pub_; + rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; + rclcpp::Publisher::SharedPtr staging_pose_pub_; + // If subscribed to a detected pose topic, will contain latest message + geometry_msgs::msg::PoseStamped detected_dock_pose_; + // This is the actual dock pose once it has the specified translation/rotation applied + // If not subscribed to a topic, this is simply the database dock pose + geometry_msgs::msg::PoseStamped dock_pose_; + + // Optionally subscribe to joint state message, used to determine if stalled + rclcpp::Subscription::SharedPtr joint_state_sub_; + std::vector stall_joint_names_; + double stall_velocity_threshold_, stall_effort_threshold_; + bool is_stalled_; + + // An external reference (such as image_proc::TrackMarkerNode) can be used to detect dock + bool use_external_detection_pose_; + double external_detection_timeout_; + tf2::Quaternion external_detection_rotation_; + double external_detection_translation_x_; + double external_detection_translation_y_; + + // Filtering of detected poses + std::shared_ptr filter_; + + // If not using an external pose reference, this is the distance threshold + double docking_threshold_; + std::string base_frame_id_; + // Offset for staging pose relative to dock pose + double staging_x_offset_; + double staging_yaw_offset_; + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::shared_ptr tf2_buffer_; +}; + +} // namespace opennav_docking + +#endif // OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index e2a6c58b20..43d89f5c64 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -2,7 +2,7 @@ opennav_docking - 1.3.2 + 1.3.3 A Task Server for robot charger docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking/plugins.xml b/nav2_docking/opennav_docking/plugins.xml index 2f1da5f283..3a3c91f067 100644 --- a/nav2_docking/opennav_docking/plugins.xml +++ b/nav2_docking/opennav_docking/plugins.xml @@ -4,6 +4,11 @@ A simple charging dock plugin. + + + A simple non-charging dock plugin. + + diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index a8bf62a6b2..b350cb4026 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -43,7 +43,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) } nav2_util::CallbackReturn -DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +DockingServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring %s", get_name()); auto node = shared_from_this(); @@ -90,6 +90,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) navigator_ = std::make_unique(node); dock_db_ = std::make_unique(mutex_); if (!dock_db_->initialize(node, tf2_buffer_)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -227,19 +228,20 @@ void DockingServer::dockRobot() if (goal->use_dock_id) { RCLCPP_INFO( get_logger(), - "Attempting to dock robot at charger %s.", goal->dock_id.c_str()); + "Attempting to dock robot at %s.", goal->dock_id.c_str()); dock = dock_db_->findDock(goal->dock_id); } else { RCLCPP_INFO( get_logger(), - "Attempting to dock robot at charger at position (%0.2f, %0.2f).", + "Attempting to dock robot at position (%0.2f, %0.2f).", goal->dock_pose.pose.position.x, goal->dock_pose.pose.position.y); dock = generateGoalDock(goal); } - // Check if the robot is docked or charging before proceeding - if (dock->plugin->isDocked() || dock->plugin->isCharging()) { - RCLCPP_INFO(get_logger(), "Robot is already charging, no need to dock"); + // Check if robot is docked or charging before proceeding, only applicable to charging docks + if (dock->plugin->isCharger() && (dock->plugin->isDocked() || dock->plugin->isCharging())) { + RCLCPP_INFO( + get_logger(), "Robot is already docked and/or charging (if applicable), no need to dock"); return; } @@ -280,9 +282,14 @@ void DockingServer::dockRobot() // Approach the dock using control law if (approachDock(dock, dock_pose)) { // We are docked, wait for charging to begin - RCLCPP_INFO(get_logger(), "Made contact with dock, waiting for charge to start"); + RCLCPP_INFO( + get_logger(), "Made contact with dock, waiting for charge to start (if applicable)."); if (waitForCharge(dock)) { - RCLCPP_INFO(get_logger(), "Robot is charging!"); + if (dock->plugin->isCharger()) { + RCLCPP_INFO(get_logger(), "Robot is charging!"); + } else { + RCLCPP_INFO(get_logger(), "Docking was successful!"); + } result->success = true; result->num_retries = num_retries_; stashDockData(goal->use_dock_id, dock, true); @@ -300,7 +307,7 @@ void DockingServer::dockRobot() } catch (opennav_docking_core::DockingException & e) { if (++num_retries_ > max_retries_) { RCLCPP_ERROR(get_logger(), "Failed to dock, all retries have been used"); - throw; + throw e; } RCLCPP_WARN(get_logger(), "Docking failed, will retry: %s", e.what()); } @@ -403,7 +410,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & publishDockingFeedback(DockRobot::Feedback::CONTROLLING); // Stop and report success if connected to dock - if (dock->plugin->isDocked() || dock->plugin->isCharging()) { + if (dock->plugin->isDocked() || (dock->plugin->isCharger() && dock->plugin->isCharging())) { return true; } @@ -450,7 +457,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & if (this->now() - start > timeout) { throw opennav_docking_core::FailedToControl( - "Timed out approaching dock; dock nor charging detected"); + "Timed out approaching dock; dock nor charging (if applicable) detected"); } loop_rate.sleep(); @@ -460,6 +467,11 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & bool DockingServer::waitForCharge(Dock * dock) { + // This is a non-charger docking request + if (!dock->plugin->isCharger()) { + return true; + } + rclcpp::Rate loop_rate(controller_frequency_); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(wait_charge_timeout_); @@ -589,11 +601,11 @@ void DockingServer::undockRobot() } RCLCPP_INFO( get_logger(), - "Attempting to undock robot from charger of type %s.", dock->getName().c_str()); + "Attempting to undock robot of dock type %s.", dock->getName().c_str()); // Check if the robot is docked before proceeding - if (!dock->isDocked()) { - RCLCPP_INFO(get_logger(), "Robot is not in the charger, no need to undock"); + if (dock->isCharger() && (!dock->isDocked() && !dock->isCharging())) { + RCLCPP_INFO(get_logger(), "Robot is not in the dock, no need to undock"); return; } @@ -623,7 +635,7 @@ void DockingServer::undockRobot() } // Don't control the robot until charging is disabled - if (!dock->disableCharging()) { + if (dock->isCharger() && !dock->disableCharging()) { loop_rate.sleep(); continue; } @@ -638,7 +650,7 @@ void DockingServer::undockRobot() RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); // Have reached staging_pose vel_publisher_->publish(std::move(command)); - if (dock->hasStoppedCharging()) { + if (!dock->isCharger() || dock->hasStoppedCharging()) { RCLCPP_INFO(get_logger(), "Robot has undocked!"); result->success = true; curr_dock_type_.clear(); @@ -647,7 +659,7 @@ void DockingServer::undockRobot() return; } // Haven't stopped charging? - throw opennav_docking_core::FailedToControl("Failed to control off dock, still charging"); + throw opennav_docking_core::FailedToControl("Failed to control off dock"); } // Publish command and sleep diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index 3ff04db47f..298a3be950 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -94,6 +94,7 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".stall_velocity_threshold", stall_velocity_threshold_); node_->get_parameter(name + ".stall_effort_threshold", stall_effort_threshold_); node_->get_parameter(name + ".docking_threshold", docking_threshold_); + node_->get_parameter("base_frame", base_frame_id_); // Get server base frame ID node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); @@ -250,7 +251,7 @@ bool SimpleChargingDock::isDocked() // Find base pose in target frame geometry_msgs::msg::PoseStamped base_pose; base_pose.header.stamp = rclcpp::Time(0); - base_pose.header.frame_id = "base_link"; + base_pose.header.frame_id = base_frame_id_; base_pose.pose.orientation.w = 1.0; try { tf2_buffer_->transform(base_pose, base_pose, dock_pose_.header.frame_id); diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp new file mode 100644 index 0000000000..25e67c5eb9 --- /dev/null +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -0,0 +1,274 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_util/node_utils.hpp" +#include "opennav_docking/simple_non_charging_dock.hpp" + +namespace opennav_docking +{ + +void SimpleNonChargingDock::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf) +{ + name_ = name; + tf2_buffer_ = tf; + node_ = parent.lock(); + if (!node_) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Parameters for optional external detection of dock pose + nav2_util::declare_parameter_if_not_declared( + node_, name + ".use_external_detection_pose", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_timeout", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_translation_x", rclcpp::ParameterValue(-0.20)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_translation_y", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_yaw", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_pitch", rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_roll", rclcpp::ParameterValue(-1.57)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); + + // Optionally determine if docked via stall detection using joint_states + nav2_util::declare_parameter_if_not_declared( + node_, name + ".use_stall_detection", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".stall_joint_names", rclcpp::PARAMETER_STRING_ARRAY); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".stall_velocity_threshold", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".stall_effort_threshold", rclcpp::ParameterValue(1.0)); + + // If not using stall detection, this is how close robot should get to pose + nav2_util::declare_parameter_if_not_declared( + node_, name + ".docking_threshold", rclcpp::ParameterValue(0.05)); + + // Staging pose configuration + nav2_util::declare_parameter_if_not_declared( + node_, name + ".staging_x_offset", rclcpp::ParameterValue(-0.7)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); + + node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_); + node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_); + node_->get_parameter( + name + ".external_detection_translation_x", external_detection_translation_x_); + node_->get_parameter( + name + ".external_detection_translation_y", external_detection_translation_y_); + double yaw, pitch, roll; + node_->get_parameter(name + ".external_detection_rotation_yaw", yaw); + node_->get_parameter(name + ".external_detection_rotation_pitch", pitch); + node_->get_parameter(name + ".external_detection_rotation_roll", roll); + external_detection_rotation_.setEuler(pitch, roll, yaw); + node_->get_parameter(name + ".stall_velocity_threshold", stall_velocity_threshold_); + node_->get_parameter(name + ".stall_effort_threshold", stall_effort_threshold_); + node_->get_parameter(name + ".docking_threshold", docking_threshold_); + node_->get_parameter("base_frame", base_frame_id_); // Get server base frame ID + node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); + node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + + // Setup filter + double filter_coef; + node_->get_parameter(name + ".filter_coef", filter_coef); + filter_ = std::make_unique(filter_coef, external_detection_timeout_); + + if (use_external_detection_pose_) { + dock_pose_.header.stamp = rclcpp::Time(0); + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", 1, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + }); + } + + bool use_stall_detection; + node_->get_parameter(name + ".use_stall_detection", use_stall_detection); + if (use_stall_detection) { + is_stalled_ = false; + node_->get_parameter(name + ".stall_joint_names", stall_joint_names_); + if (stall_joint_names_.size() < 1) { + RCLCPP_ERROR(node_->get_logger(), "stall_joint_names cannot be empty!"); + } + joint_state_sub_ = node_->create_subscription( + "joint_states", 1, + std::bind(&SimpleNonChargingDock::jointStateCallback, this, std::placeholders::_1)); + } + + dock_pose_pub_ = node_->create_publisher("dock_pose", 1); + filtered_dock_pose_pub_ = node_->create_publisher( + "filtered_dock_pose", 1); + staging_pose_pub_ = node_->create_publisher("staging_pose", 1); +} + +geometry_msgs::msg::PoseStamped SimpleNonChargingDock::getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame) +{ + // If not using detection, set the dock pose as the given dock pose estimate + if (!use_external_detection_pose_) { + // This gets called at the start of docking + // Reset our internally tracked dock pose + dock_pose_.header.frame_id = frame; + dock_pose_.pose = pose; + } + + // Compute the staging pose with given offsets + const double yaw = tf2::getYaw(pose.orientation); + geometry_msgs::msg::PoseStamped staging_pose; + staging_pose.header.frame_id = frame; + staging_pose.header.stamp = node_->now(); + staging_pose.pose = pose; + staging_pose.pose.position.x += cos(yaw) * staging_x_offset_; + staging_pose.pose.position.y += sin(yaw) * staging_x_offset_; + tf2::Quaternion orientation; + orientation.setEuler(0.0, 0.0, yaw + staging_yaw_offset_); + staging_pose.pose.orientation = tf2::toMsg(orientation); + + // Publish staging pose for debugging purposes + staging_pose_pub_->publish(staging_pose); + return staging_pose; +} + +bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string) +{ + // If using not detection, set the dock pose to the static fixed-frame version + if (!use_external_detection_pose_) { + dock_pose_pub_->publish(pose); + dock_pose_ = pose; + return true; + } + + // If using detections, get current detections, transform to frame, and apply offsets + geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; + + // Validate that external pose is new enough + auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_); + if (node_->now() - detected.header.stamp > timeout) { + RCLCPP_WARN(node_->get_logger(), "Lost detection or did not detect: timeout exceeded"); + return false; + } + + // Transform detected pose into fixed frame. Note that the argument pose + // is the output of detection, but also acts as the initial estimate + // and contains the frame_id of docking + if (detected.header.frame_id != pose.header.frame_id) { + try { + if (!tf2_buffer_->canTransform( + pose.header.frame_id, detected.header.frame_id, + detected.header.stamp, rclcpp::Duration::from_seconds(0.2))) + { + RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + return false; + } + tf2_buffer_->transform(detected, detected, pose.header.frame_id); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + return false; + } + } + + // Filter the detected pose + detected = filter_->update(detected); + filtered_dock_pose_pub_->publish(detected); + + // Rotate the just the orientation, then remove roll/pitch + geometry_msgs::msg::PoseStamped just_orientation; + just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation = detected.pose.orientation; + tf2::doTransform(just_orientation, just_orientation, transform); + + tf2::Quaternion orientation; + orientation.setEuler(0.0, 0.0, tf2::getYaw(just_orientation.pose.orientation)); + dock_pose_.pose.orientation = tf2::toMsg(orientation); + + // Construct dock_pose_ by applying translation/rotation + dock_pose_.header = detected.header; + dock_pose_.pose.position = detected.pose.position; + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + dock_pose_.pose.position.x += cos(yaw) * external_detection_translation_x_ - + sin(yaw) * external_detection_translation_y_; + dock_pose_.pose.position.y += sin(yaw) * external_detection_translation_x_ + + cos(yaw) * external_detection_translation_y_; + dock_pose_.pose.position.z = 0.0; + + // Publish & return dock pose for debugging purposes + dock_pose_pub_->publish(dock_pose_); + pose = dock_pose_; + return true; +} + +bool SimpleNonChargingDock::isDocked() +{ + if (joint_state_sub_) { + // Using stall detection + return is_stalled_; + } + + if (dock_pose_.header.frame_id.empty()) { + // Dock pose is not yet valid + return false; + } + + // Find base pose in target frame + geometry_msgs::msg::PoseStamped base_pose; + base_pose.header.stamp = rclcpp::Time(0); + base_pose.header.frame_id = base_frame_id_; + base_pose.pose.orientation.w = 1.0; + try { + tf2_buffer_->transform(base_pose, base_pose, dock_pose_.header.frame_id); + } catch (const tf2::TransformException & ex) { + return false; + } + + // If we are close enough, we are docked + double d = std::hypot( + base_pose.pose.position.x - dock_pose_.pose.position.x, + base_pose.pose.position.y - dock_pose_.pose.position.y); + return d < docking_threshold_; +} + +void SimpleNonChargingDock::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state) +{ + double velocity = 0.0; + double effort = 0.0; + for (size_t i = 0; i < state->name.size(); ++i) { + for (auto & name : stall_joint_names_) { + if (state->name[i] == name) { + // Tracking this joint + velocity += abs(state->velocity[i]); + effort += abs(state->effort[i]); + } + } + } + + // Take average + effort /= stall_joint_names_.size(); + velocity /= stall_joint_names_.size(); + + is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_); +} + +} // namespace opennav_docking + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(opennav_docking::SimpleNonChargingDock, opennav_docking_core::ChargingDock) diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index a49f245c92..3400c1d8db 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -52,6 +52,15 @@ ament_target_dependencies(test_simple_charging_dock target_link_libraries(test_simple_charging_dock ${library_name} simple_charging_dock ) +ament_add_gtest(test_simple_non_charging_dock + test_simple_non_charging_dock.cpp +) +ament_target_dependencies(test_simple_non_charging_dock + ${dependencies} +) +target_link_libraries(test_simple_non_charging_dock + ${library_name} simple_non_charging_dock +) # Test Pose Filter (unit) ament_add_gtest(test_pose_filter diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 144a612f3b..f12741774c 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -175,22 +175,65 @@ TEST(DockingServerTests, testErrorExceptions) EXPECT_TRUE(false); } } + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } TEST(DockingServerTests, getateGoalDock) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + node->on_configure(rclcpp_lifecycle::State()); std::shared_ptr goal = std::make_shared(); auto dock = node->generateGoalDock(goal); - EXPECT_EQ(dock->plugin, nullptr); + EXPECT_NE(dock->plugin, nullptr); EXPECT_EQ(dock->frame, std::string()); node->stashDockData(false, dock, true); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } TEST(DockingServerTests, testDynamicParams) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + node->on_configure(rclcpp_lifecycle::State()); node->on_activate(rclcpp_lifecycle::State()); @@ -221,6 +264,11 @@ TEST(DockingServerTests, testDynamicParams) EXPECT_EQ(node->get_parameter("base_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("max_retries").as_int(), 7); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index 6bc7439129..7193bb188b 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -48,6 +48,7 @@ TEST(SimpleChargingDockTests, ObjectLifecycle) EXPECT_FALSE(dock->isCharging()); EXPECT_TRUE(dock->disableCharging()); EXPECT_TRUE(dock->hasStoppedCharging()); + EXPECT_TRUE(dock->isCharger()); dock->deactivate(); dock->cleanup(); @@ -66,6 +67,8 @@ TEST(SimpleChargingDockTests, BatteryState) dock->configure(node, "my_dock", nullptr); dock->activate(); + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); // Below threshold sensor_msgs::msg::BatteryState msg; diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp new file mode 100644 index 0000000000..89c423f5ff --- /dev/null +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -0,0 +1,205 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "opennav_docking/simple_non_charging_dock.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +// Testing the simple non-charging dock plugin + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +namespace opennav_docking +{ + +TEST(SimpleNonChargingDockTests, ObjectLifecycle) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // Check initial states + EXPECT_THROW(dock->isCharging(), std::runtime_error); + EXPECT_THROW(dock->disableCharging(), std::runtime_error); + EXPECT_THROW(dock->hasStoppedCharging(), std::runtime_error); + EXPECT_FALSE(dock->isCharger()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StallDetection) +{ + auto node = std::make_shared("test"); + auto pub = node->create_publisher( + "joint_states", rclcpp::QoS(1)); + pub->on_activate(); + node->declare_parameter("my_dock.use_stall_detection", rclcpp::ParameterValue(true)); + std::vector names = {"left_motor", "right_motor"}; + node->declare_parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names)); + node->declare_parameter("my_dock.stall_velocity_threshold", rclcpp::ParameterValue(0.1)); + node->declare_parameter("my_dock.stall_effort_threshold", rclcpp::ParameterValue(5.0)); + + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + + // Stopped, but below effort threshold + sensor_msgs::msg::JointState msg; + msg.name = {"left_motor", "right_motor", "another_motor"}; + msg.velocity = {0.0, 0.0, 0.0}; + msg.effort = {0.0, 0.0, 0.0}; + pub->publish(msg); + rclcpp::Rate r(2); + r.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_FALSE(dock->isDocked()); + + // Moving, with effort + sensor_msgs::msg::JointState msg2; + msg2.name = {"left_motor", "right_motor", "another_motor"}; + msg2.velocity = {1.0, 1.0, 0.0}; + msg2.effort = {5.1, -5.1, 0.0}; + pub->publish(msg2); + rclcpp::Rate r1(2); + r1.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_FALSE(dock->isDocked()); + + // Stopped, with effort + sensor_msgs::msg::JointState msg3; + msg3.name = {"left_motor", "right_motor", "another_motor"}; + msg3.velocity = {0.0, 0.0, 0.0}; + msg3.effort = {5.1, -5.1, 0.0}; + pub->publish(msg3); + rclcpp::Rate r2(2); + r2.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_TRUE(dock->isDocked()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StagingPose) +{ + auto node = std::make_shared("test"); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 0.0, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StagingPoseWithYawOffset) +{ + // Override the parameter default + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.staging_yaw_offset", 3.14}, + } + ); + + auto node = std::make_shared("test", options); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + // Pose should be the same as default, but pointing in opposite direction + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 3.14, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, RefinedPoseTest) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + auto pub = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + pub->on_activate(); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::PoseStamped pose; + + // Timestamps are outdated; this is after timeout + EXPECT_FALSE(dock->isDocked()); + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 0.1; + detected_pose.pose.position.y = -0.5; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + pose.header.frame_id = "my_frame"; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); + EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +} // namespace opennav_docking diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 9a6dd91944..77d1a394eb 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -2,7 +2,7 @@ opennav_docking_bt - 1.3.2 + 1.3.3 A set of BT nodes and XMLs for docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp index 2f46b8cb76..fb0c93016d 100644 --- a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp @@ -31,7 +31,8 @@ DockRobotAction::DockRobotAction( void DockRobotAction::on_tick() { // Get core inputs about what to perform - if (getInput("use_dock_id", goal_.use_dock_id)) { + [[maybe_unused]] auto res = getInput("use_dock_id", goal_.use_dock_id); + if (goal_.use_dock_id) { getInput("dock_id", goal_.dock_id); } else { getInput("dock_pose", goal_.dock_pose); diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index b58f9408fb..695ab6e2cb 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -119,6 +119,11 @@ class ChargingDock */ virtual bool hasStoppedCharging() = 0; + /** + * @brief Gets if this is a charging-typed dock + */ + virtual bool isCharger() {return true;} + std::string getName() {return name_;} protected: diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp new file mode 100644 index 0000000000..9951b19dff --- /dev/null +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp @@ -0,0 +1,136 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENNAV_DOCKING_CORE__NON_CHARGING_DOCK_HPP_ +#define OPENNAV_DOCKING_CORE__NON_CHARGING_DOCK_HPP_ + +#include +#include + +#include "opennav_docking_core/charging_dock.hpp" + + +namespace opennav_docking_core +{ + +/** + * @class NonChargingDock + * @brief Abstract interface for a non-charging dock for the docking framework + */ +class NonChargingDock : public ChargingDock +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Virtual destructor + */ + virtual ~NonChargingDock() {} + + /** + * @param parent pointer to user's node + * @param name The name of this planner + * @param tf A pointer to a TF buffer + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf) = 0; + + /** + * @brief Method to cleanup resources used on shutdown. + */ + virtual void cleanup() = 0; + + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + virtual void activate() = 0; + + /** + * @brief Method to deactive Behavior and any threads involved in execution. + */ + virtual void deactivate() = 0; + + /** + * @brief Method to obtain the dock's staging pose. This method should likely + * be using TF and the dock's pose information to find the staging pose from + * a static or parameterized staging pose relative to the docking pose + * @param pose Dock pose + * @param frame Dock's frame of pose + * @return PoseStamped of staging pose in the specified frame + */ + virtual geometry_msgs::msg::PoseStamped getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame) = 0; + + /** + * @brief Method to obtain the refined pose of the dock, usually based on sensors + * @param pose The initial estimate of the dock pose. + * @param frame The frame of the initial estimate. + */ + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id) = 0; + + /** + * @brief Have we made contact with dock? This can be implemented in a variety + * of ways: by establishing communications with the dock, by monitoring the + * the drive motor efforts, etc. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + virtual bool isDocked() = 0; + + /** + * @brief Are we charging? If a charge dock requires any sort of negotiation + * to begin charging, that should happen inside this function as this function + * will be called repeatedly after the docking loop to check if successful. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + bool isCharging() final + { + throw std::runtime_error("This dock is not a charging dock!"); + } + + /** + * @brief Undocking while current is still flowing can damage a charge dock + * so some charge docks provide the ability to disable charging before the + * robot physically disconnects. The undocking action will not command the + * robot to move until this returns true. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + bool disableCharging() final + { + throw std::runtime_error("This dock is not a charging dock!"); + } + + /** + * @brief Similar to isCharging() but called when undocking. + */ + bool hasStoppedCharging() final + { + throw std::runtime_error("This dock is not a charging dock!"); + } + + /** + * @brief Gets if this is a charging-typed dock + */ + bool isCharger() final {return false;} +}; + +} // namespace opennav_docking_core + +#endif // OPENNAV_DOCKING_CORE__NON_CHARGING_DOCK_HPP_ diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index 4bfa0e9993..f0d782d8d3 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -2,7 +2,7 @@ opennav_docking_core - 1.3.2 + 1.3.3 A set of headers for plugins core to the opennav docking framework Steve Macenski Apache-2.0 diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 4c1918dd56..82173a877b 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.3.2 + 1.3.3 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index a24498b80c..68d858ce0c 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.3.2 + 1.3.3 DWB core interfaces package Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 17daf1433e..d79f8097ca 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.3.2 + 1.3.3 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 773463ab9e..3fc7a1e267 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.3.2 + 1.3.3 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp index 87ba77baf0..db5467cd48 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp @@ -147,6 +147,9 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator /// @brief the name of the overlying plugin ID std::string plugin_name_; + /// @brief Option to limit velocity in the trajectory generator by using current velocity + bool limit_vel_cmd_in_traj_; + /* Backwards Compatibility Parameter: include_last_point * * dwa had an off-by-one error built into it. diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 29b11f6f10..50926c98ed 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.3.2 + 1.3.3 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 49a62de302..3b57df08e1 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -75,6 +75,10 @@ void StandardTrajectoryGenerator::initialize( nh, plugin_name + ".include_last_point", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + nh, + plugin_name + ".limit_vel_cmd_in_traj", rclcpp::ParameterValue(false)); + /* * If discretize_by_time, then sim_granularity represents the amount of time that should be between * two successive points on the trajectory. @@ -89,6 +93,7 @@ void StandardTrajectoryGenerator::initialize( nh->get_parameter(plugin_name + ".linear_granularity", linear_granularity_); nh->get_parameter(plugin_name + ".angular_granularity", angular_granularity_); nh->get_parameter(plugin_name + ".include_last_point", include_last_point_); + nh->get_parameter(plugin_name + ".limit_vel_cmd_in_traj", limit_vel_cmd_in_traj_); } void StandardTrajectoryGenerator::initializeIterator( @@ -156,9 +161,14 @@ dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory( double running_time = 0.0; std::vector steps = getTimeSteps(cmd_vel); traj.poses.push_back(start_pose); + bool first_vel = false; for (double dt : steps) { // calculate velocities vel = computeNewVelocity(cmd_vel, vel, dt); + if (!first_vel && limit_vel_cmd_in_traj_) { + traj.velocity = vel; + first_vel = true; + } // update the position of the robot using the velocities passed in pose = computeNewPosition(pose, vel, dt); diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index d03c1936b0..9aae228a24 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.3.2 + 1.3.3 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index a0498c81a3..6ebf841ab6 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.3.2 + 1.3.3 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 74a447069d..cb6994259d 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.3.2 + 1.3.3 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 5c3f8a39fb..24f1b851e7 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -1,5 +1,5 @@ # Graceful Motion Controller -The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. +The graceful motion controller implements a controller based on the works of Jong Jin Park in "A Smooth Control Law for Graceful Motion of Differential Wheeled Mobile Robots in 2D Environment". In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index 23de7f646d..95ef103d77 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.3.2 + 1.3.3 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index cd717afc97..057552ef2f 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.3.2 + 1.3.3 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index 7339904090..6c8c599aa9 100644 --- a/nav2_loopback_sim/README.md +++ b/nav2_loopback_sim/README.md @@ -8,7 +8,7 @@ This was created by Steve Macenski of [Open Navigation LLC](https://opennav.org) It is drop-in replacable with AMR simulators and global localization by providing: - Map -> Odom transform -- Odom -> Base Link transform, `nav_msgs/Odometry` odometry +- Odom -> Base Link transform, `nav_msgs/Odometry` odometry - Accepts the standard `/initialpose` topic for transporting the robot to another location Note: This does not provide sensor data, so it is required that the global (and probably local) costmap contain the `StaticLayer` to avoid obstacles. @@ -33,12 +33,15 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated na ### Parameters -- `update_duration`: the duration between updates (default 0.01 -- 100hz) +- `update_duration`: The duration between updates (default 0.01 -- 100hz) - `base_frame_id`: The base frame to use (default `base_link`) - `odom_frame_id`: The odom frame to use (default `odom`) - `map_frame_id`: The map frame to use (default `map`) - `scan_frame_id`: The can frame to use to publish a scan to keep the collision monitor fed and happy (default `base_scan` for TB3, `rplidar_link` for TB4) - `enable_stamped_cmd_vel`: Whether cmd_vel is stamped or unstamped (i.e. Twist or TwistStamped). Default `false` for `Twist`. +- `scan_publish_dur`: : The duration between publishing scan (default 0.1s -- 10hz) +- `publish_map_odom_tf`: Whether or not to publish tf from `map_frame_id` to `odom_frame_id` (default `true`) +- `publish_clock`: Whether or not to publish simulated clock to `/clock` (default `true`) ### Topics @@ -47,6 +50,7 @@ This node subscribes to: - `cmd_vel`: Nav2's output twist to get the commanded velocity This node publishes: +- `clock`: To publish a simulation clock for all other nodes with `use_sim_time=True` - `odom`: To publish odometry from twist - `tf`: To publish map->odom and odom->base_link transforms -- `scan`: To publish a bogus max range laser scan sensor to make the collision monitor happy +- `scan`: To publish a range laser scan sensor based on the static map diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 7902affb13..71560c5aa8 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -24,6 +24,7 @@ from rclpy.duration import Duration from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from rosgraph_msgs.msg import Clock from sensor_msgs.msg import LaserScan from tf2_ros import Buffer, TransformBroadcaster, TransformListener import tf_transformations @@ -84,6 +85,9 @@ def __init__(self): self.publish_map_odom_tf = self.get_parameter( 'publish_map_odom_tf').get_parameter_value().bool_value + self.declare_parameter('publish_clock', True) + self.publish_clock = self.get_parameter('publish_clock').get_parameter_value().bool_value + self.t_map_to_odom = TransformStamped() self.t_map_to_odom.header.frame_id = self.map_frame_id self.t_map_to_odom.child_frame_id = self.odom_frame_id @@ -112,6 +116,10 @@ def __init__(self): depth=10) self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos) + if self.publish_clock: + self.clock_timer = self.create_timer(0.1, self.clockTimerCallback) + self.clock_pub = self.create_publisher(Clock, '/clock', 10) + self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) self.map_client = self.create_client(GetMap, '/map_server/map') @@ -139,6 +147,11 @@ def setupTimerCallback(self): if self.mat_base_to_laser is None: self.getBaseToLaserTf() + def clockTimerCallback(self): + msg = Clock() + msg.clock = self.get_clock().now().to_msg() + self.clock_pub.publish(msg) + def cmdVelCallback(self, msg): self.debug('Received cmd_vel') if self.initial_pose is None: diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index c018ae029a..0fffe1a4cb 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -2,7 +2,7 @@ nav2_loopback_sim - 1.3.2 + 1.3.3 A loopback simulator to replace physics simulation steve macenski Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index ede17e312d..f658ddbdf8 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.3.2 + 1.3.3 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 85428490ed..23f0f55b07 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -132,8 +132,10 @@ std::string expand_user_home_dir_if_needed( return yaml_filename; } if (home_variable_value.empty()) { - std::cout << "[INFO] [map_io]: Map yaml file name starts with '~/' but no HOME variable set. \n" - << "[INFO] [map_io] User home dir will be not expanded \n"; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Map yaml file name starts with '~/' but no HOME variable set. \n" + << "[INFO] [map_io] User home dir will be not expanded \n"); return yaml_filename; } const std::string prefix{home_variable_value}; @@ -181,15 +183,18 @@ LoadParameters loadMapYaml(const std::string & yaml_filename) load_parameters.negate = yaml_get_value(doc, "negate"); } - std::cout << "[DEBUG] [map_io]: resolution: " << load_parameters.resolution << std::endl; - std::cout << "[DEBUG] [map_io]: origin[0]: " << load_parameters.origin[0] << std::endl; - std::cout << "[DEBUG] [map_io]: origin[1]: " << load_parameters.origin[1] << std::endl; - std::cout << "[DEBUG] [map_io]: origin[2]: " << load_parameters.origin[2] << std::endl; - std::cout << "[DEBUG] [map_io]: free_thresh: " << load_parameters.free_thresh << std::endl; - std::cout << "[DEBUG] [map_io]: occupied_thresh: " << load_parameters.occupied_thresh << - std::endl; - std::cout << "[DEBUG] [map_io]: mode: " << map_mode_to_string(load_parameters.mode) << std::endl; - std::cout << "[DEBUG] [map_io]: negate: " << load_parameters.negate << std::endl; //NOLINT + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "resolution: " << load_parameters.resolution); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[0]: " << load_parameters.origin[0]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[1]: " << load_parameters.origin[1]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[2]: " << load_parameters.origin[2]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "free_thresh: " << load_parameters.free_thresh); + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "occupied_thresh: " << load_parameters.occupied_thresh); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), + "mode: " << map_mode_to_string(load_parameters.mode)); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "negate: " << load_parameters.negate); return load_parameters; } @@ -201,8 +206,9 @@ void loadMapFromFile( Magick::InitializeMagick(nullptr); nav_msgs::msg::OccupancyGrid msg; - std::cout << "[INFO] [map_io]: Loading image_file: " << - load_parameters.image_file_name << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), "Loading image_file: " << + load_parameters.image_file_name); Magick::Image img(load_parameters.image_file_name); // Copy the image data into the map structure @@ -290,9 +296,11 @@ void loadMapFromFile( msg.header.frame_id = "map"; msg.header.stamp = clock.now(); - std::cout << - "[DEBUG] [map_io]: Read map " << load_parameters.image_file_name << ": " << msg.info.width << - " X " << msg.info.height << " map @ " << msg.info.resolution << " m/cell" << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Read map " << load_parameters.image_file_name + << ": " << msg.info.width << " X " << msg.info.height << " map @ " + << msg.info.resolution << " m/cell"); map = msg; } @@ -302,30 +310,32 @@ LOAD_MAP_STATUS loadMapFromYaml( nav_msgs::msg::OccupancyGrid & map) { if (yaml_file.empty()) { - std::cerr << "[ERROR] [map_io]: YAML file name is empty, can't load!" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "YAML file name is empty, can't load!"); return MAP_DOES_NOT_EXIST; } - std::cout << "[INFO] [map_io]: Loading yaml file: " << yaml_file << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Loading yaml file: " << yaml_file); LoadParameters load_parameters; try { load_parameters = loadMapYaml(yaml_file); } catch (YAML::Exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed processing YAML file " << yaml_file << " at position (" << - e.mark.line << ":" << e.mark.column << ") for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Failed processing YAML file " << yaml_file << " at position (" << + e.mark.line << ":" << e.mark.column << ") for reason: " << e.what()); return INVALID_MAP_METADATA; } catch (std::exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed to parse map YAML loaded from file " << yaml_file << - " for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), "Failed to parse map YAML loaded from file " << yaml_file << + " for reason: " << e.what()); return INVALID_MAP_METADATA; } try { loadMapFromFile(load_parameters, map); } catch (std::exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name << - " for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Failed to load image file " << load_parameters.image_file_name << + " for reason: " << e.what()); return INVALID_MAP_DATA; } @@ -350,40 +360,46 @@ void checkSaveParameters(SaveParameters & save_parameters) rclcpp::Clock clock(RCL_SYSTEM_TIME); save_parameters.map_file_name = "map_" + std::to_string(static_cast(clock.now().seconds())); - std::cout << "[WARN] [map_io]: Map file unspecified. Map will be saved to " << - save_parameters.map_file_name << " file" << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Map file unspecified. Map will be saved to " << + save_parameters.map_file_name << " file"); } // Checking thresholds if (save_parameters.occupied_thresh == 0.0) { save_parameters.occupied_thresh = 0.65; - std::cout << "[WARN] [map_io]: Occupied threshold unspecified. Setting it to default value: " << - save_parameters.occupied_thresh << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "map_io"), "Occupied threshold unspecified. Setting it to default value: " << + save_parameters.occupied_thresh); } if (save_parameters.free_thresh == 0.0) { save_parameters.free_thresh = 0.25; - std::cout << "[WARN] [map_io]: Free threshold unspecified. Setting it to default value: " << - save_parameters.free_thresh << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Free threshold unspecified. Setting it to default value: " << + save_parameters.free_thresh); } if (1.0 < save_parameters.occupied_thresh) { - std::cerr << "[ERROR] [map_io]: Threshold_occupied must be 1.0 or less" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Threshold_occupied must be 1.0 or less"); throw std::runtime_error("Incorrect thresholds"); } if (save_parameters.free_thresh < 0.0) { - std::cerr << "[ERROR] [map_io]: Free threshold must be 0.0 or greater" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Free threshold must be 0.0 or greater"); throw std::runtime_error("Incorrect thresholds"); } if (save_parameters.occupied_thresh <= save_parameters.free_thresh) { - std::cerr << "[ERROR] [map_io]: Threshold_free must be smaller than threshold_occupied" << - std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Threshold_free must be smaller than threshold_occupied"); throw std::runtime_error("Incorrect thresholds"); } // Checking image format if (save_parameters.image_format == "") { save_parameters.image_format = save_parameters.mode == MapMode::Scale ? "png" : "pgm"; - std::cout << "[WARN] [map_io]: Image format unspecified. Setting it to: " << - save_parameters.image_format << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Image format unspecified. Setting it to: " << + save_parameters.image_format); } std::transform( @@ -406,24 +422,25 @@ void checkSaveParameters(SaveParameters & save_parameters) ss << "'" << format_name << "'"; first = false; } - std::cout << - "[WARN] [map_io]: Requested image format '" << save_parameters.image_format << - "' is not one of the recommended formats: " << ss.str() << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Requested image format '" << save_parameters.image_format << + "' is not one of the recommended formats: " << ss.str()); } const std::string FALLBACK_FORMAT = "png"; try { Magick::CoderInfo info(save_parameters.image_format); if (!info.isWritable()) { - std::cout << - "[WARN] [map_io]: Format '" << save_parameters.image_format << - "' is not writable. Using '" << FALLBACK_FORMAT << "' instead" << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Format '" << save_parameters.image_format << + "' is not writable. Using '" << FALLBACK_FORMAT << "' instead"); save_parameters.image_format = FALLBACK_FORMAT; } } catch (Magick::ErrorOption & e) { - std::cout << - "[WARN] [map_io]: Format '" << save_parameters.image_format << "' is not usable. Using '" << - FALLBACK_FORMAT << "' instead:" << std::endl << e.what() << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "map_io"), "Format '" << save_parameters.image_format << "' is not usable. Using '" << + FALLBACK_FORMAT << "' instead:" << std::endl << e.what()); save_parameters.image_format = FALLBACK_FORMAT; } @@ -434,10 +451,10 @@ void checkSaveParameters(SaveParameters & save_parameters) save_parameters.image_format == "jpg" || save_parameters.image_format == "jpeg")) { - std::cout << - "[WARN] [map_io]: Map mode 'scale' requires transparency, but format '" << - save_parameters.image_format << - "' does not support it. Consider switching image format to 'png'." << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Map mode 'scale' requires transparency, but format '" << + save_parameters.image_format << + "' does not support it. Consider switching image format to 'png'."); } } @@ -451,9 +468,10 @@ void tryWriteMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters) { - std::cout << - "[INFO] [map_io]: Received a " << map.info.width << " X " << map.info.height << " map @ " << - map.info.resolution << " m/pix" << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Received a " << map.info.width << " X " << map.info.height << " map @ " << + map.info.resolution << " m/pix"); std::string mapdatafile = save_parameters.map_file_name + "." + save_parameters.image_format; { @@ -509,14 +527,18 @@ void tryWriteMapToFile( pixel = Magick::Color(q, q, q); break; default: - std::cerr << "[ERROR] [map_io]: Map mode should be Trinary, Scale or Raw" << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Map mode should be Trinary, Scale or Raw"); throw std::runtime_error("Invalid map mode"); } image.pixelColor(x, y, pixel); } } - std::cout << "[INFO] [map_io]: Writing map occupancy data to " << mapdatafile << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), + "Writing map occupancy data to " << mapdatafile); image.write(mapdatafile); } @@ -545,15 +567,15 @@ void tryWriteMapToFile( e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh; if (!e.good()) { - std::cout << - "[WARN] [map_io]: YAML writer failed with an error " << e.GetLastError() << - ". The map metadata may be invalid." << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), "YAML writer failed with an error " << e.GetLastError() << + ". The map metadata may be invalid."); } - std::cout << "[INFO] [map_io]: Writing map metadata to " << mapmetadatafile << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Writing map metadata to " << mapmetadatafile); std::ofstream(mapmetadatafile) << e.c_str(); } - std::cout << "[INFO] [map_io]: Map saved" << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Map saved"); } bool saveMapToFile( @@ -569,7 +591,9 @@ bool saveMapToFile( tryWriteMapToFile(map, save_parameters_loc); } catch (std::exception & e) { - std::cout << "[ERROR] [map_io]: Failed to write map for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), + "Failed to write map for reason: " << e.what()); return false; } return true; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 82b850bc46..0816fcc32e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -106,7 +106,9 @@ class MPPIController : public nav2_core::Controller * @brief Visualize trajectories * @param transformed_plan Transformed input plan */ - void visualize(nav_msgs::msg::Path transformed_plan); + void visualize( + nav_msgs::msg::Path transformed_plan, + const builtin_interfaces::msg::Time & cmd_stamp); std::string name_; rclcpp_lifecycle::LifecycleNode::WeakPtr parent_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp index d9cdc95ce9..328424317d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp @@ -79,7 +79,9 @@ class TrajectoryVisualizer * @brief Add an optimal trajectory to visualize * @param trajectory Optimal trajectory */ - void add(const xt::xtensor & trajectory, const std::string & marker_namespace); + void add( + const xt::xtensor & trajectory, const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp); /** * @brief Add candidate trajectories to visualize @@ -103,7 +105,9 @@ class TrajectoryVisualizer std::shared_ptr> trajectories_publisher_; std::shared_ptr> transformed_path_pub_; + std::shared_ptr> optimal_path_pub_; + std::unique_ptr optimal_path_; std::unique_ptr points_; int marker_id_ = 0; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index ddde607765..4ad0dbfbd7 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -461,9 +461,8 @@ inline void savitskyGolayFilter( xt::xarray filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0}; filter /= 231.0; - const unsigned int num_sequences = control_sequence.vx.shape(0) - 1; - // Too short to smooth meaningfully + const unsigned int num_sequences = control_sequence.vx.shape(0) - 1; if (num_sequences < 20) { return; } @@ -473,137 +472,49 @@ inline void savitskyGolayFilter( }; auto applyFilterOverAxis = - [&](xt::xtensor & sequence, + [&](xt::xtensor & sequence, const xt::xtensor & initial_sequence, const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void { - unsigned int idx = 0; - sequence(idx) = applyFilter( - { - hist_0, - hist_1, - hist_2, - hist_3, - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); - - idx++; - sequence(idx) = applyFilter( - { - hist_1, - hist_2, - hist_3, - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); - - idx++; - sequence(idx) = applyFilter( - { - hist_2, - hist_3, - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); - - idx++; - sequence(idx) = applyFilter( - { - hist_3, - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); - - for (idx = 4; idx != num_sequences - 4; idx++) { - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); + float pt_m4 = hist_0; + float pt_m3 = hist_1; + float pt_m2 = hist_2; + float pt_m1 = hist_3; + float pt = initial_sequence(0); + float pt_p1 = initial_sequence(1); + float pt_p2 = initial_sequence(2); + float pt_p3 = initial_sequence(3); + float pt_p4 = initial_sequence(4); + + for (unsigned int idx = 0; idx != num_sequences; idx++) { + sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); + pt_m4 = pt_m3; + pt_m3 = pt_m2; + pt_m2 = pt_m1; + pt_m1 = pt; + pt = pt_p1; + pt_p1 = pt_p2; + pt_p2 = pt_p3; + pt_p3 = pt_p4; + + if (idx + 5 < num_sequences) { + pt_p4 = initial_sequence(idx + 5); + } else { + // Return the last point + pt_p4 = initial_sequence(num_sequences); + } } - - idx++; - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 3)}); - - idx++; - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 2), - sequence(idx + 2)}); - - idx++; - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 1), - sequence(idx + 1), - sequence(idx + 1)}); - - idx++; - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx), - sequence(idx), - sequence(idx), - sequence(idx)}); }; // Filter trajectories + const models::ControlSequence initial_control_sequence = control_sequence; applyFilterOverAxis( - control_sequence.vx, control_history[0].vx, + control_sequence.vx, initial_control_sequence.vx, control_history[0].vx, control_history[1].vx, control_history[2].vx, control_history[3].vx); applyFilterOverAxis( - control_sequence.vy, control_history[0].vy, + control_sequence.vy, initial_control_sequence.vy, control_history[0].vy, control_history[1].vy, control_history[2].vy, control_history[3].vy); applyFilterOverAxis( - control_sequence.wz, control_history[0].wz, + control_sequence.wz, initial_control_sequence.wz, control_history[0].wz, control_history[1].wz, control_history[2].wz, control_history[3].wz); // Update control history diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index d278185477..13afd1e58c 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.3.2 + 1.3.3 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 6c83131263..54eb1f57a0 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -99,16 +99,18 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( #endif if (visualize_) { - visualize(std::move(transformed_plan)); + visualize(std::move(transformed_plan), cmd.header.stamp); } return cmd; } -void MPPIController::visualize(nav_msgs::msg::Path transformed_plan) +void MPPIController::visualize( + nav_msgs::msg::Path transformed_plan, + const builtin_interfaces::msg::Time & cmd_stamp) { trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); - trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory"); + trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp); trajectory_visualizer_.visualize(std::move(transformed_plan)); } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index a6531b7d1d..7cb07c6858 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -28,6 +28,7 @@ void TrajectoryVisualizer::on_configure( trajectories_publisher_ = node->create_publisher("/trajectories", 1); transformed_path_pub_ = node->create_publisher("transformed_global_plan", 1); + optimal_path_pub_ = node->create_publisher("optimal_trajectory", 1); parameters_handler_ = parameters_handler; auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer"); @@ -42,22 +43,27 @@ void TrajectoryVisualizer::on_cleanup() { trajectories_publisher_.reset(); transformed_path_pub_.reset(); + optimal_path_pub_.reset(); } void TrajectoryVisualizer::on_activate() { trajectories_publisher_->on_activate(); transformed_path_pub_->on_activate(); + optimal_path_pub_->on_activate(); } void TrajectoryVisualizer::on_deactivate() { trajectories_publisher_->on_deactivate(); transformed_path_pub_->on_deactivate(); + optimal_path_pub_->on_deactivate(); } void TrajectoryVisualizer::add( - const xt::xtensor & trajectory, const std::string & marker_namespace) + const xt::xtensor & trajectory, + const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp) { auto & size = trajectory.shape()[0]; if (!size) { @@ -76,8 +82,21 @@ void TrajectoryVisualizer::add( auto marker = utils::createMarker( marker_id_++, pose, scale, color, frame_id_, marker_namespace); points_->markers.push_back(marker); + + // populate optimal path + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.frame_id = frame_id_; + pose_stamped.pose = pose; + + tf2::Quaternion quaternion_tf2; + quaternion_tf2.setRPY(0., 0., trajectory(i, 2)); + pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2); + + optimal_path_->poses.push_back(pose_stamped); }; + optimal_path_->header.stamp = cmd_stamp; + optimal_path_->header.frame_id = frame_id_; for (size_t i = 0; i < size; i++) { add_marker(i); } @@ -111,6 +130,7 @@ void TrajectoryVisualizer::reset() { marker_id_ = 0; points_ = std::make_unique(); + optimal_path_ = std::make_unique(); } void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) @@ -119,6 +139,10 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) trajectories_publisher_->publish(std::move(points_)); } + if (optimal_path_pub_->get_subscription_count() > 0) { + optimal_path_pub_->publish(std::move(optimal_path_)); + } + reset(); if (transformed_path_pub_->get_subscription_count() > 0) { diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 7ebada2a6a..2b7c8e0a90 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -81,7 +81,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); - vis.add(optimal_trajectory, "Optimal Trajectory"); + builtin_interfaces::msg::Time bogus_stamp; + vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); @@ -90,7 +91,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) // Now populated with content, should publish optimal_trajectory = xt::ones({20, 2}); - vis.add(optimal_trajectory, "Optimal Trajectory"); + vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); vis.visualize(bogus_path); rclcpp::spin_some(node->get_node_base_interface()); @@ -153,3 +154,65 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) // 40 * 4, for 5 trajectory steps + 3 point steps EXPECT_EQ(recieved_msg.markers.size(), 160u); } + +TEST(TrajectoryVisualizerTests, VisOptimalPath) +{ + auto node = std::make_shared("my_node"); + auto parameters_handler = std::make_unique(node); + builtin_interfaces::msg::Time cmd_stamp; + cmd_stamp.sec = 5; + cmd_stamp.nanosec = 10; + + nav_msgs::msg::Path recieved_path; + auto my_sub = node->create_subscription( + "optimal_trajectory", 10, + [&](const nav_msgs::msg::Path msg) {recieved_path = msg;}); + + // optimal_trajectory empty, should fail to publish + xt::xtensor optimal_trajectory; + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + nav_msgs::msg::Path bogus_path; + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_EQ(recieved_path.poses.size(), 0u); + + // Now populated with content, should publish + optimal_trajectory.resize({20, 2}); + for (unsigned int i = 0; i != optimal_trajectory.shape()[0] - 1; i++) { + optimal_trajectory(i, 0) = static_cast(i); + optimal_trajectory(i, 1) = static_cast(i); + } + vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + + // Should have a 20 points path in the map frame and with same stamp than velocity command + EXPECT_EQ(recieved_path.poses.size(), 20u); + EXPECT_EQ(recieved_path.header.frame_id, "fkmap"); + EXPECT_EQ(recieved_path.header.stamp.sec, cmd_stamp.sec); + EXPECT_EQ(recieved_path.header.stamp.nanosec, cmd_stamp.nanosec); + + tf2::Quaternion quat; + for (unsigned int i = 0; i != recieved_path.poses.size() - 1; i++) { + // Poses should be in map frame too + EXPECT_EQ(recieved_path.poses[i].header.frame_id, "fkmap"); + + // Check positions are correct + EXPECT_EQ(recieved_path.poses[i].pose.position.x, static_cast(i)); + EXPECT_EQ(recieved_path.poses[i].pose.position.y, static_cast(i)); + EXPECT_EQ(recieved_path.poses[i].pose.position.z, 0.06); + + // Check orientations are correct + quat.setRPY(0., 0., optimal_trajectory(i, 2)); + auto expected_orientation = tf2::toMsg(quat); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.x, expected_orientation.x); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.y, expected_orientation.y); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.z, expected_orientation.z); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.w, expected_orientation.w); + } +} diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 49f04e2139..80b0a1d4ef 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -16,6 +16,7 @@ uint16 INVALID_PATH=103 uint16 PATIENCE_EXCEEDED=104 uint16 FAILED_TO_MAKE_PROGRESS=105 uint16 NO_VALID_CONTROL=106 +uint16 CONTROLLER_TIMED_OUT=107 std_msgs/Empty result uint16 error_code diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index cc26330e90..cf4cdbf43f 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.3.2 + 1.3.3 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 903303223d..3277aea744 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.3.2 + 1.3.3 Nav2 NavFn planner Steve Macenski Carlos Orduno diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index bdffda281a..565d4c0dc0 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -243,6 +243,7 @@ class PlannerServer : public nav2_util::LifecycleNode std::vector planner_ids_; std::vector planner_types_; double max_planner_duration_; + rclcpp::Duration costmap_update_timeout_; std::string planner_ids_concat_; // TF buffer diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index ede0137f77..9bba6f0b2d 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.3.2 + 1.3.3 Nav2 planner server package Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6abeddd29b..c0eb744d00 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -45,6 +45,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), default_ids_{"GridBased"}, default_types_{"nav2_navfn_planner::NavfnPlanner"}, + costmap_update_timeout_(1s), costmap_(nullptr) { RCLCPP_INFO(get_logger(), "Creating"); @@ -53,6 +54,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); declare_parameter("action_server_result_timeout", 10.0); + declare_parameter("costmap_update_timeout", 1.0); get_parameter("planner_plugins", planner_ids_); if (planner_ids_ == default_ids_) { @@ -78,7 +80,7 @@ PlannerServer::~PlannerServer() } nav2_util::CallbackReturn -PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +PlannerServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -119,6 +121,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -151,6 +154,10 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + double costmap_update_timeout_dbl; + get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); + costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); + // Create the action servers for path planning to a pose and through poses action_server_pose_ = std::make_unique( shared_from_this(), @@ -284,7 +291,11 @@ void PlannerServer::waitForCostmap() { // Don't compute a plan until costmap is valid (after clear costmap) rclcpp::Rate r(100); + auto waiting_start = now(); while (!costmap_ros_->isCurrent()) { + if (now() - waiting_start > costmap_update_timeout_) { + throw nav2_core::PlannerTimedOut("Costmap timed out waiting for update"); + } r.sleep(); } } diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index caced767c1..f45f4dd48f 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.3.2 + 1.3.3 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index cb1b8ad7ed..03c1c9881c 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -152,6 +152,13 @@ class RotationShimController : public nav2_core::Controller const double & angular_distance_to_heading, const geometry_msgs::msg::PoseStamped & pose); + /** + * @brief Checks if the goal has changed based on the given path. + * @param path The path to compare with the current goal. + * @return True if the goal has changed, false otherwise. + */ + bool isGoalChanged(const nav_msgs::msg::Path & path); + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -175,7 +182,7 @@ class RotationShimController : public nav2_core::Controller double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; - bool rotate_to_goal_heading_, in_rotation_; + bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_; // Dynamic parameters handler std::mutex mutex_; diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index ae7412733e..c8930eddd6 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.3.2 + 1.3.3 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 4704c0b8eb..6b3d24a4ab 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -66,6 +66,8 @@ void RotationShimController::configure( node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_); @@ -81,6 +83,7 @@ void RotationShimController::configure( control_duration_ = 1.0 / control_frequency; node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_); try { primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); @@ -248,7 +251,10 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - return current_path_.poses.back(); + auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; + goal.header.stamp = clock_->now(); + return goal; } geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() @@ -258,6 +264,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() } auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; goal.header.stamp = clock_->now(); return goal; } @@ -332,9 +339,20 @@ void RotationShimController::isCollisionFree( } } +bool RotationShimController::isGoalChanged(const nav_msgs::msg::Path & path) +{ + // Return true if rotating or if the current path is empty + if (in_rotation_ || current_path_.poses.empty()) { + return true; + } + + // Check if the last pose of the current and new paths differ + return current_path_.poses.back().pose != path.poses.back().pose; +} + void RotationShimController::setPlan(const nav_msgs::msg::Path & path) { - path_updated_ = true; + path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true; current_path_ = path; primary_controller_->setPlan(path); } @@ -369,6 +387,8 @@ RotationShimController::dynamicParametersCallback(std::vector } else if (type == ParameterType::PARAMETER_BOOL) { if (name == plugin_name_ + ".rotate_to_goal_heading") { rotate_to_goal_heading_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".rotate_to_heading_once") { + rotate_to_heading_once_ = parameter.as_bool(); } } } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 1160a5a98a..1d63a77b47 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -62,6 +62,11 @@ class RotationShimShim : public nav2_rotation_shim_controller::RotationShimContr return getSampledPathPt(); } + bool isGoalChangedWrapper(const nav_msgs::msg::Path & path) + { + return isGoalChanged(path); + } + geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper(geometry_msgs::msg::PoseStamped pt) { return transformPoseToBaseFrame(pt); @@ -382,6 +387,57 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } +TEST(RotationShimControllerTest, isGoalChangedTest) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(node); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "base_link"; + transform.child_frame_id = "odom"; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + tf_broadcaster->sendTransform(transform); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter( + "PathFollower.rotate_to_heading_once", + true); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + nav_msgs::msg::Path path; + path.header.frame_id = "base_link"; + path.poses.resize(2); + path.poses.back().pose.position.x = 2.0; + path.poses.back().pose.position.y = 2.0; + + // Test: Current path is empty, should return true + EXPECT_EQ(controller->isGoalChangedWrapper(path), true); + + // Test: Last pose of the current path is the same, should return false + controller->setPlan(path); + EXPECT_EQ(controller->isGoalChangedWrapper(path), false); + + // Test: Last pose of the current path differs, should return true + path.poses.back().pose.position.x = 3.0; + EXPECT_EQ(controller->isGoalChangedWrapper(path), true); +} + TEST(RotationShimControllerTest, testDynamicParameter) { auto node = std::make_shared("ShimControllerTest"); @@ -412,7 +468,8 @@ TEST(RotationShimControllerTest, testDynamicParameter) rclcpp::Parameter("test.max_angular_accel", 7.0), rclcpp::Parameter("test.simulate_ahead_time", 7.0), rclcpp::Parameter("test.primary_controller", std::string("HI")), - rclcpp::Parameter("test.rotate_to_goal_heading", true)}); + rclcpp::Parameter("test.rotate_to_goal_heading", true), + rclcpp::Parameter("test.rotate_to_heading_once", true)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -424,4 +481,5 @@ TEST(RotationShimControllerTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true); } diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 6f1a0a96f3..b7a46b1ea9 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.3.2 + 1.3.3 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 54d7604433..4b604d5561 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -136,6 +136,10 @@ def destroy_node(self): self.spin_client.destroy() self.backup_client.destroy() self.drive_on_heading_client.destroy() + self.assisted_teleop_client.destroy() + self.follow_gps_waypoints_client.destroy() + self.docking_client.destroy() + self.undocking_client.destroy() super().destroy_node() def setInitialPose(self, initial_pose): diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index cd571e2e20..d369aa0093 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.3.2 + 1.3.3 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index ebb25bb52b..49ab250123 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -43,7 +43,7 @@ The `SmacPlanner2D` is designed to work with: - Circular, differential or omnidirectional robots - Relatively small robots with respect to environment size (e.g. RC car in a hallway or large robot in a convention center) that can be approximated by circular footprint. -## Features +## Features We further improve on Hybrid-A\* in the following ways: - Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio). @@ -90,7 +90,7 @@ We provide 3 nodes by default currently. The 2D node template (`Node2D`) which d In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path (not available for State Lattice due to the lattices generated are dependent on costmap resolution). For the `SmacPlannerHybrid` and `SmacPlannerLattice` plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. -We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. +We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. ## Parameters @@ -111,13 +111,13 @@ planner_server: max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance terminal_checking_interval: 5000 # number of iterations between checking if the goal has been cancelled or planner timed out max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. - motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp + motion_model_for_search: "DUBIN" # For Hybrid Dubin, Reeds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius - analytic_expansion_max_cost: true # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal) - analytic_expansion_max_cost_override: false # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + analytic_expansion_max_cost: 200 # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal) + analytic_expansion_max_cost_override: false # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required). If expansion is within 2*pi*min_r of the goal, then it will override the max cost if ``false``. minimum_turning_radius: 0.40 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 change_penalty: 0.0 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0 @@ -126,10 +126,10 @@ planner_server: retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0 rotation_penalty: 5.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings. lookup_table_size: 20.0 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. + cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. - debug_visualizations: True # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + debug_visualizations: True # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. smoother: max_iterations: 1000 w_smooth: 0.3 @@ -155,19 +155,19 @@ sudo apt-get install ros--nav2-smac-planner ### Potential Fields -Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. +Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. -Some of the most popular tuning guides for Navigation / Nav2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. +Some of the most popular tuning guides for Navigation / Nav2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be somewhat suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better. -So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. +So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. ### Hybrid-A* and State Lattice Turning Radius' A very reasonable and logical assumption would be to set the `minimum_turning_radius` to the kinematic limits of your vehicle. For an ackermann car, that's a physical quantity; while for differential or omni robots, its a bit of a dance around what kind of turns you'd like your robot to be able to make. Obviously setting this to something insanely small (like 20 cm) means you have alot of options, but also probably means the raw output plans won't be very straight and smooth when you have 2+ meter wide aisles to work in. -I assert that you should also consider the environment you operate within when setting this. While you should **absolutely not** set this to be any smaller than the actual limits of your vehicle, there are some useful side effects of increasing this value in practical use. If you work in an area wider than the turning circle of your robot, you have some options that will ultimately improve the performance of the planner (in terms of CPU and compute time) as well as generate paths that are more "smooth" directly out of the planner -- not requiring any explicit path smoothing. +I assert that you should also consider the environment you operate within when setting this. While you should **absolutely not** set this to be any smaller than the actual limits of your vehicle, there are some useful side effects of increasing this value in practical use. If you work in an area wider than the turning circle of your robot, you have some options that will ultimately improve the performance of the planner (in terms of CPU and compute time) as well as generate paths that are more "smooth" directly out of the planner -- not requiring any explicit path smoothing. By default, `0.4m` is the setting which I think is "reasonable" for the smaller scale industrial grade robots (think Simbe, the small Fetch, or Locus robots) resulting in faster plans and less "wobbly" motions that do not require post-smoothing -- further improving CPU performance. I selected `0.4m` as a trade off between practical robots mentioned above and hobbyist users with a tiny-little-turtlebot-3 which might still need to navigate around some smaller cavities. @@ -177,11 +177,11 @@ We provide for the Hybrid-A\*, State Lattice, and 2D A\* implementations a costm I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used. -Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. +Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. ### Penalty Function Tuning -The penalty function defaults are tuned for all of the planners based on a 5cm costmap. While some change in this should not largely impact default behavior, it may be good to tune for your specific application and needs. The default values were tuned to have decent out of the box behavior for a large number of platforms and resolutions. In most situations, you should not need to play with them. +The penalty function defaults are tuned for all of the planners based on a 5cm costmap. While some change in this should not largely impact default behavior, it may be good to tune for your specific application and needs. The default values were tuned to have decent out of the box behavior for a large number of platforms and resolutions. In most situations, you should not need to play with them. **However**, due to the nature of the State Lattice planner being able to use any number of custom generated minimum control sets, this planner may require more tuning to get good behavior. The defaults for State Lattice were generated using the 5cm Ackermann files you can find in this package as initial examples. After a change in formulation for the Hybrid-A* planner, the default of change penalty off seems to produce good results, but please tune to your application need and run-time speed requirements. @@ -197,13 +197,13 @@ Note that change penalty must be greater than 0.0. The non-straight, reverse, an Before addressing the section below, make sure you have an appropriately set max iterations parameter. If you have a 1 km2 sized warehouse, clearly 5000 expansions will be insufficient. Try increasing this value if you're unable to achieve goals or disable it with the `-1` value to see if you are now able to plan within a reasonable time period. If you still have issues, there is a secondary effect which could be happening that is good to be aware of. -In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. +In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. Seeing the figures below, you'll see an attempt to plan into a "U" shaped region across the map. The first figure shows the small gap in the map (from an imperfect SLAM session) which is nearly traversible, but not quite. From the starting location, that gap yeilds the shortest path to the goal, so the heuristics will attempt to route the paths in that direction. However, it is not possible to actually pass with a kinematically valid path with the footprint set. As a result, the planner expands all of its maximum 1,000,000 iterations attempting to fit through it (visualized in red). If an infinite number of iterations were allowed, eventually a valid path would be found, but might take significant time. By simply increasing the footprint (a bit hackier, the best solution is to edit the map to make this area impassible), then that gap is now properly blocked as un-navigable. In the second figure, you can see that the heuristics influence the expansion down a navigable route and is able to find a path in less than 10,000 iterations (or about 110ms). It is easy now! -As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isnt actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. +As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isnt actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. ![](media/A.png) ![](media/B.png) diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 86e09bb304..2077602b69 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.3.2 + 1.3.3 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 8b9e69aa77..a44acc518d 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -54,6 +54,16 @@ void GridCollisionChecker::setFootprint( const double & possible_collision_cost) { possible_collision_cost_ = static_cast(possible_collision_cost); + if (possible_collision_cost_ <= 0.0f) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + footprint_is_radius_ = radius; // Use radius, no caching required @@ -114,18 +124,8 @@ bool GridCollisionChecker::inCollision( footprint_cost_ = static_cast(costmap_->getCost( static_cast(x + 0.5f), static_cast(y + 0.5f))); - if (footprint_cost_ < possible_collision_cost_) { - if (possible_collision_cost_ > 0.0f) { - return false; - } else { - RCLCPP_ERROR_THROTTLE( - logger_, *clock_, 1000, - "Inflation layer either not found or inflation is not set sufficiently for " - "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" - " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " - "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" - " for full instructions. This will substantially impact run-time performance."); - } + if (footprint_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { + return false; } // If its inscribed, in collision, or unknown in the middle, diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 818613167f..963f52e377 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.3.2 + 1.3.3 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index e7baa5e483..fe462cb1c6 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -63,7 +63,7 @@ SmootherServer::~SmootherServer() } nav2_util::CallbackReturn -SmootherServer::on_configure(const rclcpp_lifecycle::State &) +SmootherServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring smoother server"); @@ -100,6 +100,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &) *costmap_sub_, *footprint_sub_, this->get_name()); if (!loadSmootherPlugins()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -162,7 +163,7 @@ bool SmootherServer::loadSmootherPlugins() } nav2_util::CallbackReturn -SmootherServer::on_activate(const rclcpp_lifecycle::State &) +SmootherServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 17fe63a926..2da176b12f 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -118,74 +118,44 @@ bool SavitzkyGolaySmoother::smoothImpl( }; auto applyFilterOverAxes = - [&](std::vector & plan_pts) -> void + [&](std::vector & plan_pts, + const std::vector & init_plan_pts) -> void { - // Handle initial boundary conditions, first point is fixed - unsigned int idx = 1; - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 1].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 2].pose.position, - plan_pts[idx + 3].pose.position}); - - idx++; - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 2].pose.position, - plan_pts[idx - 2].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 2].pose.position, - plan_pts[idx + 3].pose.position}); - - // Apply nominal filter - for (idx = 3; idx < path_size - 4; ++idx) { - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 3].pose.position, - plan_pts[idx - 2].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 2].pose.position, - plan_pts[idx + 3].pose.position}); + auto pt_m3 = init_plan_pts[0].pose.position; + auto pt_m2 = init_plan_pts[0].pose.position; + auto pt_m1 = init_plan_pts[0].pose.position; + auto pt = init_plan_pts[1].pose.position; + auto pt_p1 = init_plan_pts[2].pose.position; + auto pt_p2 = init_plan_pts[3].pose.position; + auto pt_p3 = init_plan_pts[4].pose.position; + + // First point is fixed + for (unsigned int idx = 1; idx != path_size - 1; idx++) { + plan_pts[idx].pose.position = applyFilter({pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3}); + pt_m3 = pt_m2; + pt_m2 = pt_m1; + pt_m1 = pt; + pt = pt_p1; + pt_p1 = pt_p2; + pt_p2 = pt_p3; + + if (idx + 4 < path_size - 1) { + pt_p3 = init_plan_pts[idx + 4].pose.position; + } else { + // Return the last point + pt_p3 = init_plan_pts[path_size - 1].pose.position; + } } - - // Handle terminal boundary conditions, last point is fixed - idx++; - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 3].pose.position, - plan_pts[idx - 2].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 2].pose.position, - plan_pts[idx + 2].pose.position}); - - idx++; - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 3].pose.position, - plan_pts[idx - 2].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 1].pose.position}); }; - applyFilterOverAxes(path.poses); + const auto initial_path_poses = path.poses; + applyFilterOverAxes(path.poses, initial_path_poses); // Lets do additional refinement, it shouldn't take more than a couple milliseconds if (do_refinement_) { for (int i = 0; i < refinement_num_; i++) { - applyFilterOverAxes(path.poses); + const auto reined_initial_path_poses = path.poses; + applyFilterOverAxes(path.poses, reined_initial_path_poses); } } diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 76e6d21bd5..36bdfd3f60 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.3.2 + 1.3.3 A sets of system-level tests for Nav2 usually involving full robot simulation Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index e0334d591c..417a74eab9 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.3.2 + 1.3.3 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 5df0e75102..1e1441234d 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.3.2 + 1.3.3 Nav2 utilities Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 3f51038157..d28af826b8 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.3.2 + 1.3.3 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 535b6b08f2..9519ab94b6 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -44,7 +44,7 @@ VelocitySmoother::~VelocitySmoother() } nav2_util::CallbackReturn -VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) +VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring velocity smoother"); auto node = shared_from_this(); @@ -76,24 +76,35 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) for (unsigned int i = 0; i != 3; i++) { if (max_decels_[i] > 0.0) { - throw std::runtime_error( - "Positive values set of deceleration! These should be negative to slow down!"); + RCLCPP_ERROR( + get_logger(), + "Positive values set of deceleration! These should be negative to slow down!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (max_accels_[i] < 0.0) { - throw std::runtime_error( - "Negative values set of acceleration! These should be positive to speed up!"); + RCLCPP_ERROR( + get_logger(), + "Negative values set of acceleration! These should be positive to speed up!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (min_velocities_[i] > 0.0) { - throw std::runtime_error( - "Positive values set of min_velocities! These should be negative!"); + RCLCPP_ERROR( + get_logger(), "Positive values set of min_velocities! These should be negative!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (max_velocities_[i] < 0.0) { - throw std::runtime_error( - "Negative values set of max_velocities! These should be positive!"); + RCLCPP_ERROR( + get_logger(), "Negative values set of max_velocities! These should be positive!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (min_velocities_[i] > max_velocities_[i]) { - throw std::runtime_error( - "Min velocities are higher than max velocities!"); + RCLCPP_ERROR(get_logger(), "Min velocities are higher than max velocities!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } } @@ -112,9 +123,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) if (max_velocities_.size() != 3 || min_velocities_.size() != 3 || max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3) { - throw std::runtime_error( - "Invalid setting of kinematic and/or deadband limits!" - " All limits must be size of 3 representing (x, y, theta)."); + RCLCPP_ERROR( + get_logger(), + "Invalid setting of kinematic and/or deadband limits!" + " All limits must be size of 3 representing (x, y, theta)."); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } // Get control type @@ -124,7 +138,11 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) open_loop_ = false; odom_smoother_ = std::make_unique(node, odom_duration_, odom_topic_); } else { - throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + RCLCPP_ERROR( + get_logger(), + "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } // Setup inputs / outputs @@ -144,6 +162,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 25754d0f35..76c5e0498b 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -40,7 +40,10 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother public: VelSmootherShim() : VelocitySmoother() {} - void configure(const rclcpp_lifecycle::State & state) {this->on_configure(state);} + nav2_util::CallbackReturn configure(const rclcpp_lifecycle::State & state) + { + return this->on_configure(state); + } void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);} void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);} void cleanup(const rclcpp_lifecycle::State & state) {this->on_cleanup(state);} @@ -594,10 +597,10 @@ TEST(VelocitySmootherTest, testInvalidParams) std::vector max_vels{0.0, 0.0}; // invalid size smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(max_vels)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("feedback", std::string("LAWLS"))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) @@ -613,13 +616,13 @@ TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel)); smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testDynamicParameter) diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index ce7fe04135..4fa5eba26a 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.3.2 + 1.3.3 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 128b2f2369..10a416a5be 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.3.2 + 1.3.3 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 02a4158d28..0014e79a15 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -54,7 +54,7 @@ WaypointFollower::~WaypointFollower() } nav2_util::CallbackReturn -WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) +WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -123,6 +123,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create waypoint_task_executor. Exception: %s", e.what()); + on_cleanup(state); } return nav2_util::CallbackReturn::SUCCESS; diff --git a/navigation2/package.xml b/navigation2/package.xml index 1f66feb583..eab0074b3c 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.3.2 + 1.3.3 ROS2 Navigation Stack