From 57093ecc475afce44869347a5dc0f56c567c3d60 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 24 Jan 2024 01:31:46 +0000 Subject: [PATCH 01/19] Iron Sync 4: Jan 23, 2024 (#4063) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * collision_monitor: dynamic polygon and source enable/disable (#3825) * Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * initial * fix * fix * remove unrelated change * reset * fix format * PR fixes * Add test * fix comments * add to params * publish only if enabled * Add source dynamic enable/disable * add enabled param to sources * fix * add same to collision detector * Update README.md: fix typo (#3842) * Update package.xml * fix typo (#3850) * adjust link to point to v3.8 of behavior tree docs (#3851) BT.CPP_v3 is used, thereby the correct docs should be linked * Fix bug in nav2_behavior_tree/bt_action_node (#3849) * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak --------- Signed-off-by: CihatAltiparmak * Fix action test failure due to rate after Rolling sync API change (#3852) * Update CMakeLists.txt (#3843) * add option for sse4 and avs512 (#3853) * Remove all exit(-1) crash conditions (#3846) * Update transform_available_condition.cpp * wrapping all examples of get_plugin_type_param in exceptions and making that throw instead of crash * some linting * fix typo * Update controller.cpp * Update nav2_params.yaml * Update nav2_params.yaml * simplication of lattice_generator.py, fixes #3858 (#3859) * simplification of equation to compute the max_value/outer edge of the lattice based on number of headings. * Stop error diagnostics when pausing nav (#3830) * Added nodestate enum and a variable to keep track of current state of managed nodes. * Updating state_of_managed_nodes_ when switching states and using it to determine an accurate diagnostics message. * Fixing bugs. * Updated/added docstrings. * Publishing OK status when nodes are unconfigured. Changed if-else chain to switch case. * Renamed NodeState PAUSED to INACTIVE, state_of_managed_nodes_ to managed_nodes_state_ and replaced system_active_ with an inline method. * Bugfix. --------- Co-authored-by: Pekka Myller * Add a timeout to the wait for transforms step of the costmap activation. (#3866) * Add a timeout to the wait for transforms step of the costmap activation. Signed-off-by: Fabian König * Rename wait_for_transforms_timeout to initial_transform_timeout * Activate costmap publishers only after transforms are checked * Check if controller server activation was succesful in planner_server * Add unittest for costmap activation Signed-off-by: Fabian König --------- Signed-off-by: Fabian König * Fix class doxygen * fix minor typos (#3892) Signed-off-by: Anton Kesy * Publish collision points for debug purposes (#3879) * Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * . * fixes * add to collision detector * fix * fix * . * fixes * add namespace to topic * fixes * fix use after free (#3910) * fix build mppi (#3927) Signed-off-by: kevin Co-authored-by: kevin * Removing old TODOs * protect invalid max_velocity min_velocity (#3953) Co-authored-by: Guillaume Doisy * protect properly max_accel and max_decel (#3952) Co-authored-by: Guillaume Doisy * Fixed links for install and build in README (#3963) Currently the readme is linking to an invalida page in the docs (404 error). * adding support for rotate in place cusps (#3934) * Fix linting error (#3969) * Fix linting error * Update regulated_pure_pursuit_controller.cpp * fix a few outdated comments in smac planners (#3978) * adding soft realtime prioritization for collision monitor and velocity smoother (#3979) * adding soft realtime prioritization for collision monitor and velocity smoother * refactor simple action server to use new utils API * Update README.md * Synchronize map size information during map initialization (#4015) * Update costmap size configuration This commit updates the costmap_2d.cpp file to fix a bug where the costmap size wasn't appropriately updated. Two new lines of code have been added to ensure the size of the costmap is correctly configured each time it's instantiated. * Refactor costmap size assignment in Costmap2D class The code refactor eliminates the direct mutation of the size_x_ and size_y_ attributes in the Costmap2D class. Instead, the class uses the size of cells provided during initialization and calculation from map coordinates for better encapsulation and clarity. * check width&height params (#4017) Co-authored-by: GoesM * Fix SimpleActionServer nullprt callback (#4025) * add check before calling completion_callback_ * Fix lint * footprint checks (#4030) * footprint checks Signed-off-by: gg * lint fix Signed-off-by: gg --------- Signed-off-by: gg * Is path valid doc (#4032) * docs Signed-off-by: gg * update Signed-off-by: gg --------- Signed-off-by: gg * Update vision_opencv's branch for rolling Signed-off-by: Steve Macenski * handle dynamically changes in parameters. (#4046) Signed-off-by: Sebastian Solarte * Add inflation_layer_name param (#4047) Signed-off-by: Renan Salles * missing urdf dep (#4050) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * bump to 1.2.6 for release --------- Signed-off-by: CihatAltiparmak Signed-off-by: Fabian König Signed-off-by: Anton Kesy Signed-off-by: kevin Signed-off-by: gg Signed-off-by: Steve Macenski Signed-off-by: Sebastian Solarte Signed-off-by: Renan Salles Signed-off-by: Guillaume Doisy Co-authored-by: Tony Najjar Co-authored-by: thandal Co-authored-by: Anton Kesy Co-authored-by: CihatAltiparmak Co-authored-by: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com> Co-authored-by: Plaqueoff <44152820+Plaqueoff@users.noreply.github.com> Co-authored-by: Pekka Myller Co-authored-by: Fabian König Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com> Co-authored-by: kevin Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Abiel Fernandez Co-authored-by: Michael Ferguson Co-authored-by: Hao-Li-Bachelorarbeit <141755843+Hao-Li-Bachelorarbeit@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Joshua Wallace Co-authored-by: Sebastian Solarte <89881453+Sunart24@users.noreply.github.com> Co-authored-by: Renan Salles --- README.md | 6 +- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/README.md | 2 +- .../nav2_behavior_tree/bt_action_node.hpp | 15 +- nav2_behavior_tree/package.xml | 2 +- .../transform_available_condition.cpp | 2 +- .../plugins/action/test_bt_action_node.cpp | 133 +++++++++++- nav2_behaviors/package.xml | 2 +- nav2_behaviors/src/behavior_server.cpp | 4 +- nav2_bringup/package.xml | 2 +- .../navigators/navigate_through_poses.hpp | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 9 +- nav2_collision_monitor/CMakeLists.txt | 2 + .../collision_detector_node.hpp | 4 + .../collision_monitor_node.hpp | 6 + .../nav2_collision_monitor/polygon.hpp | 16 ++ .../include/nav2_collision_monitor/source.hpp | 23 ++ nav2_collision_monitor/package.xml | 3 +- .../params/collision_detector_params.yaml | 3 + .../params/collision_monitor_params.yaml | 6 + .../src/collision_detector_node.cpp | 45 +++- .../src/collision_monitor_node.cpp | 61 +++++- nav2_collision_monitor/src/pointcloud.cpp | 1 + nav2_collision_monitor/src/polygon.cpp | 34 +++ nav2_collision_monitor/src/range.cpp | 1 + nav2_collision_monitor/src/scan.cpp | 1 + nav2_collision_monitor/src/source.cpp | 40 ++++ .../test/collision_detector_node_test.cpp | 60 ++++++ .../test/collision_monitor_node_test.cpp | 201 ++++++++++++++++++ nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_controller/src/controller_server.cpp | 7 +- nav2_core/package.xml | 2 +- .../include/nav2_costmap_2d/cost_values.hpp | 2 +- .../nav2_costmap_2d/costmap_2d_ros.hpp | 9 +- .../denoise/image_processing.hpp | 28 +-- nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/src/costmap_2d.cpp | 13 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 49 ++++- nav2_costmap_2d/test/module_tests.cpp | 4 +- nav2_costmap_2d/test/unit/CMakeLists.txt | 5 + .../test/unit/binary_filter_test.cpp | 18 +- .../test/unit/keepout_filter_test.cpp | 14 +- nav2_costmap_2d/test/unit/lifecycle_test.cpp | 54 +++++ .../test/unit/speed_filter_test.cpp | 18 +- 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 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- .../lifecycle_manager.hpp | 22 +- nav2_lifecycle_manager/package.xml | 2 +- .../src/lifecycle_manager.cpp | 60 ++++-- nav2_map_server/package.xml | 2 +- nav2_mppi_controller/CMakeLists.txt | 6 +- nav2_mppi_controller/README.md | 3 +- .../critics/obstacles_critic.hpp | 4 +- nav2_mppi_controller/package.xml | 4 +- .../src/critics/obstacles_critic.cpp | 6 +- nav2_mppi_controller/src/optimizer.cpp | 2 + .../test/noise_generator_test.cpp | 2 + nav2_mppi_controller/test/utils_test.cpp | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_navfn_planner/src/navfn_planner.cpp | 5 - .../include/nav2_planner/planner_server.hpp | 3 + nav2_planner/package.xml | 2 +- nav2_planner/src/planner_server.cpp | 47 +++- .../package.xml | 2 +- .../src/regulated_pure_pursuit_controller.cpp | 19 +- nav2_rotation_shim_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 3 +- nav2_simple_commander/package.xml | 2 +- .../include/nav2_smac_planner/a_star.hpp | 13 +- .../include/nav2_smac_planner/node_basic.hpp | 1 - .../nav2_smac_planner/node_lattice.hpp | 2 - .../nav2_smac_planner/thirdparty/robin_hood.h | 4 +- .../lattice_primitives/lattice_generator.py | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smoother/package.xml | 2 +- nav2_smoother/src/nav2_smoother.cpp | 2 +- nav2_system_tests/package.xml | 2 +- .../behaviors/spin/spin_behavior_tester.cpp | 4 +- .../src/planning/planner_tester.hpp | 3 + nav2_theta_star_planner/package.xml | 2 +- .../include/nav2_util/geometry_utils.hpp | 2 +- nav2_util/include/nav2_util/node_utils.hpp | 12 +- .../nav2_util/simple_action_server.hpp | 22 +- nav2_util/package.xml | 2 +- nav2_util/src/node_utils.cpp | 14 ++ nav2_util/test/test_actions.cpp | 1 - nav2_util/test/test_node_utils.cpp | 2 +- nav2_velocity_smoother/package.xml | 2 +- .../src/velocity_smoother.cpp | 52 ++++- .../include/nav2_voxel_grid/voxel_grid.hpp | 6 +- nav2_voxel_grid/package.xml | 2 +- .../plugins/photo_at_waypoint.hpp | 2 +- nav2_waypoint_follower/package.xml | 2 +- .../src/waypoint_follower.cpp | 4 +- navigation2/package.xml | 2 +- tools/underlay.repos | 2 +- 106 files changed, 1100 insertions(+), 209 deletions(-) create mode 100644 nav2_costmap_2d/test/unit/lifecycle_test.cpp diff --git a/README.md b/README.md index b8d9131e165..9eed0e712c9 100644 --- a/README.md +++ b/README.md @@ -9,8 +9,8 @@ For detailed instructions on how to: - [Getting Started](https://navigation.ros.org/getting_started/index.html) - [Concepts](https://navigation.ros.org/concepts/index.html) -- [Build](https://navigation.ros.org/build_instructions/index.html#build) -- [Install](https://navigation.ros.org/build_instructions/index.html#install) +- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build) +- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install) - [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html) - [Configure](https://navigation.ros.org/configuration/index.html) - [Navigation Plugins](https://navigation.ros.org/plugins/index.html) @@ -20,6 +20,8 @@ For detailed instructions on how to: Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). +If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org. + ## Our Sponsors Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index a9cd3a89bb1..b05101cb557 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.5 + 1.2.6

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index b2c30c432f0..84e16e7d4e9 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine. -For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/ +For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics 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 b76403ff31e..4e1029bf9ab 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 @@ -162,7 +162,7 @@ class BtActionNode : public BT::ActionNodeBase } /** - * @brief Function to perform some user-defined operation whe the action is aborted. + * @brief Function to perform some user-defined operation when the action is aborted. * @return BT::NodeStatus Returns FAILURE by default, user may override return another value */ virtual BT::NodeStatus on_aborted() @@ -266,7 +266,7 @@ class BtActionNode : public BT::ActionNodeBase // Action related failure that should not fail the tree, but the node return BT::NodeStatus::FAILURE; } else { - // Internal exception to propogate to the tree + // Internal exception to propagate to the tree throw e; } } @@ -300,6 +300,7 @@ class BtActionNode : public BT::ActionNodeBase void halt() override { if (should_cancel_goal()) { + auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) @@ -308,6 +309,16 @@ class BtActionNode : public BT::ActionNodeBase node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str()); } + + if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( + node_->get_logger(), + "Failed to get result for %s in node halt!", action_name_.c_str()); + } + + on_cancelled(); } setStatus(BT::NodeStatus::IDLE); diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index c5f60f7a28e..21f49e98eb0 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.5 + 1.2.6 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index ddc67534469..8daca5afede 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -39,7 +39,7 @@ TransformAvailableCondition::TransformAvailableCondition( RCLCPP_FATAL( node_->get_logger(), "Child frame (%s) or parent frame (%s) were empty.", child_frame_.c_str(), parent_frame_.c_str()); - exit(-1); + throw std::runtime_error("TransformAvailableCondition: Child or parent frames not provided!"); } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index d95cbd4a848..12c25d30d6f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -54,11 +54,17 @@ class FibonacciActionServer : public rclcpp::Node sleep_duration_ = sleep_duration; } + void setServerLoopRate(std::chrono::nanoseconds server_loop_rate) + { + server_loop_rate_ = server_loop_rate; + } + protected: rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID &, std::shared_ptr) { + RCLCPP_INFO(this->get_logger(), "Goal is received.."); if (sleep_duration_ > 0ms) { std::this_thread::sleep_for(sleep_duration_); } @@ -73,6 +79,13 @@ class FibonacciActionServer : public rclcpp::Node void handle_accepted( const std::shared_ptr> handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&FibonacciActionServer::execute, this, _1), handle}.detach(); + } + + void execute( + const std::shared_ptr> handle) { // this needs to return quickly to avoid blocking the executor, so spin up a new thread if (handle) { @@ -88,8 +101,17 @@ class FibonacciActionServer : public rclcpp::Node sequence.push_back(0); sequence.push_back(1); + rclcpp::Rate rate(server_loop_rate_); for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + if (handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Goal is canceling."); + handle->canceled(result); + return; + } + + RCLCPP_INFO(this->get_logger(), "Goal is feedbacking."); sequence.push_back(sequence[i] + sequence[i - 1]); + rate.sleep(); } handle->succeed(result); @@ -99,6 +121,7 @@ class FibonacciActionServer : public rclcpp::Node protected: rclcpp_action::Server::SharedPtr action_server_; std::chrono::milliseconds sleep_duration_; + std::chrono::nanoseconds server_loop_rate_; }; class FibonacciAction : public nav2_behavior_tree::BtActionNode @@ -121,6 +144,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset>("sequence", result_.result->sequence); + config().blackboard->set("on_cancelled_triggered", true); + return BT::NodeStatus::SUCCESS; + } + static BT::PortsList providedPorts() { return providedBasicPorts({BT::InputPort("order", "Fibonacci order")}); @@ -144,6 +174,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("on_cancelled_triggered", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) @@ -220,6 +251,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -255,15 +287,22 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // start a new execution cycle with the previous BT to ensure previous state doesn't leak into // the new cycle - // halt BT for a new execution cycle + // halt BT for a new execution cycle, + // get if the on_cancelled is triggered from blackboard and assert + // that the on_cancelled triggers after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a large action server goal handling duration action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -300,6 +339,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the action server will take 100ms before accepting the goal action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -327,14 +367,21 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the new cycle // halt BT for a new execution cycle + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancelled never can trigger after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(25ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -348,6 +395,90 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) EXPECT_EQ(result, BT::NodeStatus::SUCCESS); } +TEST_F(BTActionNodeTestFixture, test_server_cancel) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + // setting a server timeout smaller than the time the action server will take to accept the goal + // to simulate a server timeout scenario + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 50ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // to keep track of the number of ticks it took to reach expected tick count + int ticks = 0; + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + // BT loop execution rate + rclcpp::WallRate loopRate(100ms); + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 5 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 5); + + // send new goal to the action server for a new execution cycle + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 1000ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // reset state variable + ticks = 0; + config_->blackboard->set("on_cancelled_triggered", false); + result = BT::NodeStatus::RUNNING; + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 7 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 7); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 4799f7eb7b4..bae36129b5c 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.5 + 1.2.6 TODO Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index b5ec19ba626..c7beffd7862 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -105,13 +105,13 @@ BehaviorServer::loadBehaviorPlugins() auto node = shared_from_this(); for (size_t i = 0; i != behavior_ids_.size(); i++) { - behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]); try { + behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]); RCLCPP_INFO( get_logger(), "Creating behavior plugin %s of type %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str()); behaviors_.push_back(plugin_loader_.createUniqueInstance(behavior_types_[i])); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), "Failed to create behavior %s of type %s." " Exception: %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str(), diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index f84f4795e65..6ffb1376a72 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.5 + 1.2.6 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 2605fc86497..615ce5ef64b 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -32,7 +32,7 @@ namespace nav2_bt_navigator { /** - * @class NavigateToPoseNavigator + * @class NavigateThroughPosesNavigator * @brief A navigator for navigating to a a bunch of intermediary poses */ class NavigateThroughPosesNavigator diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 41a646af56a..57af0654514 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.5 + 1.2.6 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index a9b7af4cb61..e60e3c38a90 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -161,8 +161,8 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) // Load navigator plugins for (size_t i = 0; i != navigator_ids.size(); i++) { - std::string navigator_type = nav2_util::get_plugin_type_param(node, navigator_ids[i]); try { + std::string navigator_type = nav2_util::get_plugin_type_param(node, navigator_ids[i]); RCLCPP_INFO( get_logger(), "Creating navigator id %s of type %s", navigator_ids[i].c_str(), navigator_type.c_str()); @@ -173,11 +173,10 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) { return nav2_util::CallbackReturn::FAILURE; } - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( - get_logger(), "Failed to create navigator id %s of type %s." - " Exception: %s", navigator_ids[i].c_str(), navigator_type.c_str(), - ex.what()); + get_logger(), "Failed to create navigator id %s." + " Exception: %s", navigator_ids[i].c_str(), ex.what()); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 51cafe71dd0..b902cc4ea83 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(nav2_common REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) ### Header ### @@ -37,6 +38,7 @@ set(dependencies nav2_util nav2_costmap_2d nav2_msgs + visualization_msgs ) set(monitor_executable_name collision_monitor) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index c4c5906b22a..7791265179f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -28,6 +28,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/msg/collision_detector_state.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" @@ -147,6 +148,9 @@ class CollisionDetector : public nav2_util::LifecycleNode /// @brief collision monitor state publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; + /// @brief Collision points marker publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + collision_points_marker_pub_; /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; 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 0383dee3634..09a13658092 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 @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -107,6 +108,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @param cmd_vel_in_topic Output name of cmd_vel_in topic * @param cmd_vel_out_topic Output name of cmd_vel_out topic * is required. + * @param state_topic topic name for publishing collision monitor state * @return True if all parameters were obtained or false in failure case */ bool getParameters( @@ -210,6 +212,10 @@ class CollisionMonitor : public nav2_util::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; + /// @brief Collision points marker publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + collision_points_marker_pub_; + /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index a68fece7573..815694d83c6 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -85,6 +85,11 @@ class Polygon * @return Action type for current polygon */ ActionType getActionType() const; + /** + * @brief Obtains polygon enabled state + * @return Whether polygon is enabled + */ + bool getEnabled() const; /** * @brief Obtains polygon minimum points to enter inside polygon causing the action * @return Minimum number of data readings within a zone to trigger the action @@ -191,6 +196,13 @@ class Polygon */ void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + /** * @brief Checks if point is inside polygon * @param point Given point to check @@ -204,6 +216,8 @@ class Polygon nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Basic parameters /// @brief Name of polygon @@ -222,6 +236,8 @@ class Polygon double time_before_collision_; /// @brief Time step for robot movement simulation double simulation_time_step_; + /// @brief Whether polygon is enabled + bool enabled_; /// @brief Polygon subscription rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 8bc750cc717..30d58d53bc1 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -74,7 +74,19 @@ class Source const rclcpp::Time & curr_time, std::vector & data) const = 0; + /** + * @brief Obtains source enabled state + * @return Whether source is enabled + */ + bool getEnabled() const; + protected: + /** + * @brief Source configuration routine. + * @return True in case of everything is configured correctly, or false otherwise + */ + bool configure(); + /** * @brief Supporting routine obtaining ROS-parameters common for all data sources * @param source_topic Output name of source subscription topic @@ -91,12 +103,21 @@ class Source const rclcpp::Time & source_time, const rclcpp::Time & curr_time) const; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + // ----- Variables ----- /// @brief Collision Monitor node nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Basic parameters /// @brief Name of data source @@ -116,6 +137,8 @@ class Source /// @brief Whether to correct source data towards to base frame movement, /// considering the difference between current time and latest source time bool base_shift_correction_; + /// @brief Whether source is enabled + bool enabled_; }; // class Source } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index bdfef0ed00d..99ab6b3cf4b 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.5 + 1.2.6 Collision Monitor Alexey Merzlyakov Steve Macenski @@ -21,6 +21,7 @@ nav2_util nav2_costmap_2d nav2_msgs + visualization_msgs ament_cmake_gtest ament_lint_common diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 218ce452397..68b4cd7b507 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -14,12 +14,15 @@ collision_detector: min_points: 4 visualize: True polygon_pub_topic: "polygon_front" + enabled: True observation_sources: ["scan"] scan: type: "scan" topic: "scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 239960e5934..3e506ed1d7f 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -22,6 +22,7 @@ collision_monitor: min_points: 4 visualize: True polygon_pub_topic: "polygon_stop" + enabled: True PolygonSlow: type: "polygon" points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] @@ -30,6 +31,7 @@ collision_monitor: slowdown_ratio: 0.3 visualize: True polygon_pub_topic: "polygon_slowdown" + enabled: True PolygonLimit: type: "polygon" points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] @@ -39,6 +41,7 @@ collision_monitor: angular_limit: 0.5 visualize: True polygon_pub_topic: "polygon_limit" + enabled: True FootprintApproach: type: "polygon" action_type: "approach" @@ -47,12 +50,15 @@ collision_monitor: simulation_time_step: 0.1 min_points: 6 visualize: False + enabled: True observation_sources: ["scan"] scan: type: "scan" topic: "scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 98433ead04c..d5cf99a9de7 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -55,6 +55,9 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) state_pub_ = this->create_publisher( "collision_detector_state", rclcpp::SystemDefaultsQoS()); + collision_points_marker_pub_ = this->create_publisher( + "~/collision_points_marker", 1); + // Obtaining ROS parameters if (!getParameters()) { return nav2_util::CallbackReturn::FAILURE; @@ -70,6 +73,7 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) // Activating lifecycle publisher state_pub_->on_activate(); + collision_points_marker_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -97,6 +101,7 @@ CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Deactivating lifecycle publishers state_pub_->on_deactivate(); + collision_points_marker_pub_->on_deactivate(); // Deactivating polygons for (std::shared_ptr polygon : polygons_) { @@ -115,6 +120,7 @@ CollisionDetector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Cleaning up"); state_pub_.reset(); + collision_points_marker_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -224,7 +230,6 @@ bool CollisionDetector::configurePolygons( polygon_name.c_str()); return false; } - } } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); @@ -304,13 +309,45 @@ void CollisionDetector::process() // Fill collision_points array from different data sources for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); + if (source->getEnabled()) { + source->getData(curr_time, collision_points); + } + } + + 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); + + 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)); } std::unique_ptr state_msg = std::make_unique(); for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } state_msg->polygons.push_back(polygon->getName()); state_msg->detections.push_back( polygon->getPointsInside( @@ -326,7 +363,9 @@ void CollisionDetector::process() void CollisionDetector::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 9bd7f1eb2f8..115a6ffef88 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -68,11 +68,29 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1)); cmd_vel_out_pub_ = this->create_publisher( cmd_vel_out_topic, 1); + if (!state_topic.empty()) { state_pub_ = this->create_publisher( state_topic, 1); } + collision_points_marker_pub_ = this->create_publisher( + "~/collision_points_marker", 1); + + auto node = shared_from_this(); + nav2_util::declare_parameter_if_not_declared( + node, "use_realtime_priority", rclcpp::ParameterValue(false)); + bool use_realtime_priority = false; + node->get_parameter("use_realtime_priority", use_realtime_priority); + if (use_realtime_priority) { + try { + nav2_util::setSoftRealTimePriority(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } + } + return nav2_util::CallbackReturn::SUCCESS; } @@ -86,6 +104,7 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/) if (state_pub_) { state_pub_->on_activate(); } + collision_points_marker_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -126,6 +145,7 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/) if (state_pub_) { state_pub_->on_deactivate(); } + collision_points_marker_pub_->on_deactivate(); // Destroying bond connection destroyBond(); @@ -141,6 +161,7 @@ CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/) cmd_vel_in_sub_.reset(); cmd_vel_out_pub_.reset(); state_pub_.reset(); + collision_points_marker_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -372,7 +393,36 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) // Fill collision_points array from different data sources for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); + if (source->getEnabled()) { + source->getData(curr_time, collision_points); + } + } + + 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); + + 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)); } // By default - there is no action @@ -381,6 +431,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) std::shared_ptr action_polygon; for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } if (robot_action.action_type == STOP) { // If robot already should stop, do nothing break; @@ -408,7 +461,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) notifyActionState(robot_action, action_polygon); } - // Publish requred robot velocity + // Publish required robot velocity publishVelocity(robot_action); // Publish polygons for better visualization @@ -544,7 +597,9 @@ void CollisionMonitor::notifyActionState( void CollisionMonitor::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 4582703b2f4..f23f5e0bfea 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -49,6 +49,7 @@ PointCloud::~PointCloud() void PointCloud::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 8b7a3c7c002..248bdcec6de 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -48,6 +48,7 @@ Polygon::~Polygon() polygon_sub_.reset(); polygon_pub_.reset(); poly_.clear(); + dyn_params_handler_.reset(); } bool Polygon::configure() @@ -102,6 +103,10 @@ bool Polygon::configure() polygon_pub_topic, polygon_qos); } + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Polygon::dynamicParametersCallback, this, std::placeholders::_1)); + return true; } @@ -129,6 +134,11 @@ ActionType Polygon::getActionType() const return action_type_; } +bool Polygon::getEnabled() const +{ + return enabled_; +} + int Polygon::getMinPoints() const { return min_points_; @@ -302,6 +312,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) return false; } + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool(); + nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".min_points", rclcpp::ParameterValue(4)); min_points_ = node->get_parameter(polygon_name_ + ".min_points").as_int(); @@ -487,6 +501,26 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m polygon_ = *msg; } +rcl_interfaces::msg::SetParametersResult +Polygon::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == polygon_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) { RCLCPP_INFO( diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index cd070d70be6..56c0a4750fc 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -49,6 +49,7 @@ Range::~Range() void Range::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 731ee8baa8b..603e2f04e69 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -47,6 +47,7 @@ Scan::~Scan() void Scan::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index df539495f25..8dcf0c85e2d 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -43,6 +43,17 @@ Source::~Source() { } +bool Source::configure() +{ + auto node = node_.lock(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Source::dynamicParametersCallback, this, std::placeholders::_1)); + + return true; +} + void Source::getCommonParameters(std::string & source_topic) { auto node = node_.lock(); @@ -54,6 +65,10 @@ void Source::getCommonParameters(std::string & source_topic) node, source_name_ + ".topic", rclcpp::ParameterValue("scan")); // Set deafult topic for laser scanner source_topic = node->get_parameter(source_name_ + ".topic").as_string(); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool(); } bool Source::sourceValid( @@ -75,4 +90,29 @@ bool Source::sourceValid( return true; } +bool Source::getEnabled() const +{ + return enabled_; +} + +rcl_interfaces::msg::SetParametersResult +Source::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == source_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 3400f1d50d4..c6af2d7a23b 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -32,6 +32,7 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "tf2_ros/transform_broadcaster.h" @@ -50,6 +51,7 @@ static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; static const char STATE_TOPIC[]{"collision_detector_state"}; +static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_detector/collision_points_marker"}; static const int MIN_POINTS{1}; static const double SIMULATION_TIME_STEP{0.01}; static const double TRANSFORM_TOLERANCE{0.5}; @@ -145,6 +147,8 @@ class Tester : public ::testing::Test const rclcpp::Time & stamp); bool waitState(const std::chrono::nanoseconds & timeout); void stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg); + bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout); + void collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg); protected: // CollisionDetector node @@ -157,6 +161,11 @@ class Tester : public ::testing::Test rclcpp::Subscription::SharedPtr state_sub_; nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; + + // CollisionMonitor collision points markers + rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; + }; // Tester Tester::Tester() @@ -173,6 +182,10 @@ Tester::Tester() state_sub_ = cd_->create_subscription( STATE_TOPIC, rclcpp::SystemDefaultsQoS(), std::bind(&Tester::stateCallback, this, std::placeholders::_1)); + + collision_points_marker_sub_ = cd_->create_subscription( + COLLISION_POINTS_MARKERS_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::collisionPointsMarkerCallback, this, std::placeholders::_1)); } Tester::~Tester() @@ -180,6 +193,7 @@ Tester::~Tester() scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); + collision_points_marker_sub_.reset(); cd_.reset(); } @@ -197,11 +211,30 @@ bool Tester::waitState(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) +{ + collision_points_marker_msg_ = nullptr; + rclcpp::Time start_time = cd_->now(); + while (rclcpp::ok() && cd_->now() - start_time <= rclcpp::Duration(timeout)) { + if (collision_points_marker_msg_) { + return true; + } + rclcpp::spin_some(cd_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg) { state_msg_ = msg; } +void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg) +{ + collision_points_marker_msg_ = msg; +} + void Tester::setCommonParameters() { cd_->declare_parameter( @@ -679,6 +712,33 @@ TEST_F(Tester, testPointcloudDetection) cd_->stop(); } +TEST_F(Tester, testCollisionPointsMarkers) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + addSource(SCAN_NAME, SCAN); + setVectors({}, {SCAN_NAME}); + + // Start Collision Monitor node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_NE(collision_points_marker_msg_->markers[0].points.size(), 0u); + // Stop Collision Monitor node + cd_->stop(); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index e11f3b1b3d9..2ceb34fff6a 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -32,6 +32,7 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "tf2_ros/transform_broadcaster.h" @@ -48,6 +49,7 @@ static const char ODOM_FRAME_ID[]{"odom"}; static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; static const char STATE_TOPIC[]{"collision_monitor_state"}; +static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_monitor/collision_points_marker"}; static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; @@ -160,11 +162,16 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp); bool waitCmdVel(const std::chrono::nanoseconds & timeout); + bool waitFuture( + rclcpp::Client::SharedFuture, + const std::chrono::nanoseconds & timeout); bool waitActionState(const std::chrono::nanoseconds & timeout); + bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout); protected: void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg); void actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPtr msg); + void collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg); // CollisionMonitor node std::shared_ptr cm_; @@ -186,6 +193,13 @@ class Tester : public ::testing::Test // CollisionMonitor Action state rclcpp::Subscription::SharedPtr action_state_sub_; nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; + + // CollisionMonitor collision points markers + rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; + + // Service client for setting CollisionMonitor parameters + rclcpp::Client::SharedPtr parameters_client_; }; // Tester Tester::Tester() @@ -211,6 +225,13 @@ Tester::Tester() action_state_sub_ = cm_->create_subscription( STATE_TOPIC, rclcpp::SystemDefaultsQoS(), std::bind(&Tester::actionStateCallback, this, std::placeholders::_1)); + collision_points_marker_sub_ = cm_->create_subscription( + COLLISION_POINTS_MARKERS_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::collisionPointsMarkerCallback, this, std::placeholders::_1)); + parameters_client_ = + cm_->create_client( + std::string( + cm_->get_name()) + "/set_parameters"); } Tester::~Tester() @@ -225,6 +246,7 @@ Tester::~Tester() cmd_vel_out_sub_.reset(); action_state_sub_.reset(); + collision_points_marker_sub_.reset(); cm_.reset(); } @@ -308,6 +330,11 @@ void Tester::addPolygon( rclcpp::Parameter(polygon_name + ".type", "unknown")); } + cm_->declare_parameter( + polygon_name + ".enabled", rclcpp::ParameterValue(true)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".enabled", true)); + cm_->declare_parameter( polygon_name + ".action_type", rclcpp::ParameterValue(at)); cm_->set_parameter( @@ -539,6 +566,7 @@ void Tester::publishCmdVel(const double x, const double y, const double tw) // Reset cmd_vel_out_ before calling CollisionMonitor::process() cmd_vel_out_ = nullptr; action_state_ = nullptr; + collision_points_marker_msg_ = nullptr; std::unique_ptr msg = std::make_unique(); @@ -579,6 +607,22 @@ bool Tester::waitCmdVel(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitFuture( + rclcpp::Client::SharedFuture result_future, + const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) { rclcpp::Time start_time = cm_->now(); @@ -592,6 +636,19 @@ bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (collision_points_marker_msg_) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg) { cmd_vel_out_ = msg; @@ -602,6 +659,11 @@ void Tester::actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPt action_state_ = msg; } +void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg) +{ + collision_points_marker_msg_ = msg; +} + TEST_F(Tester, testProcessStopSlowdownLimit) { rclcpp::Time curr_time = cm_->now(); @@ -961,6 +1023,116 @@ TEST_F(Tester, testCeasePublishZeroVel) cm_->stop(); } +TEST_F(Tester, testPolygonNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when polygon is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, STOP); + ASSERT_EQ(action_state_->polygon_name, "Stop"); + + // Disable polygon by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = "Stop.enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when polygon is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testSourceNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when source is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, STOP); + ASSERT_EQ(action_state_->polygon_name, "Stop"); + + // Disable source by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = std::string(SCAN_NAME) + ".enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when source is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + TEST_F(Tester, testProcessNonActive) { rclcpp::Time curr_time = cm_->now(); @@ -1029,6 +1201,35 @@ TEST_F(Tester, testSourcesNotSet) cm_->cant_configure(); } +TEST_F(Tester, testCollisionPointsMarkers) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + addSource(SCAN_NAME, SCAN); + setVectors({}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + + publishCmdVel(0.5, 0.2, 0.1); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_NE(collision_points_marker_msg_->markers[0].points.size(), 0u); + // Stop Collision Monitor node + cm_->stop(); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_common/package.xml b/nav2_common/package.xml index cd06143ce3b..bc40b382fd2 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.5 + 1.2.6 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 f24b0ad23f9..f8e740c9aed 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.5 + 1.2.6 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 2f05758b95e..3289376f9cd 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.5 + 1.2.6 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 00854cd44b9..c6908bb5115 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -158,7 +158,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) progress_checker_ids_[i].c_str(), progress_checker_types_[i].c_str()); progress_checker->initialize(node, progress_checker_ids_[i]); progress_checkers_.insert({progress_checker_ids_[i], progress_checker}); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); @@ -264,7 +264,10 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_ros_->activate(); + const auto costmap_ros_state = costmap_ros_->activate(); + if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + return nav2_util::CallbackReturn::FAILURE; + } ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 7610bd2d665..8dc4896b61e 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.5 + 1.2.6 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/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp index dca22c9f7ff..edd521bf306 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp @@ -44,5 +44,5 @@ static constexpr unsigned char LETHAL_OBSTACLE = 254; static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; static constexpr unsigned char MAX_NON_OBSTACLE = 252; static constexpr unsigned char FREE_SPACE = 0; -} +} // namespace nav2_costmap_2d #endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 46a71c8e76d..f319e52f330 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -360,7 +360,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode bool always_send_full_costmap_{false}; std::string footprint_; float footprint_padding_{0}; - std::string global_frame_; ///< The global frame for the costmap + std::string global_frame_; ///< The global frame for the costmap int map_height_meters_{0}; double map_publish_frequency_{0}; double map_update_frequency_{0}; @@ -374,11 +374,12 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector filter_names_; std::vector filter_types_; double resolution_{0}; - std::string robot_base_frame_; ///< The frame_id of the robot base + std::string robot_base_frame_; ///< The frame_id of the robot base double robot_radius_; - bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap + bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap bool track_unknown_space_{false}; - double transform_tolerance_{0}; ///< The timeout before transform errors + double transform_tolerance_{0}; ///< The timeout before transform errors + double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors // Derived parameters bool use_radius_{false}; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp index 4527e07fdd3..96a13b8c7c9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp @@ -21,6 +21,8 @@ #include #include #include +#include +#include namespace nav2_costmap_2d { @@ -77,7 +79,7 @@ void morphologyOperation( const Image & input, Image & output, const Image & shape, AggregateFn aggregate); -using ShapeBuffer3x3 = std::array; +using ShapeBuffer3x3 = std::array; // NOLINT inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity); } // namespace imgproc_impl @@ -95,7 +97,7 @@ inline void dilate( const Image & input, Image & output, ConnectivityType connectivity, Max && max_function) { - using namespace imgproc_impl; + using namespace imgproc_impl; // NOLINT ShapeBuffer3x3 shape_buffer; Image shape = createShape(shape_buffer, connectivity); morphologyOperation(input, output, shape, max_function); @@ -376,7 +378,7 @@ struct EquivalenceLabelTreesBase struct LabelOverflow : public std::runtime_error { - LabelOverflow(const std::string & message) + explicit LabelOverflow(const std::string & message) : std::runtime_error(message) {} }; @@ -464,7 +466,6 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase { Label k = 1; for (Label i = 1; i < next_free_; ++i) { - if (labels_[i] < i) { labels_[i] = labels_[labels_[i]]; } else { @@ -504,7 +505,7 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase * '~' - row continuation in the same style */ max_labels = (rows * columns) / 3 + 1; } - ++max_labels; // add zero label + ++max_labels; // add zero label max_labels = std::min(max_labels, size_t(std::numeric_limits

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index dd03b3be022..87b95592234 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -325,13 +325,17 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages global_loc_srv_.reset(); + initial_guess_srv_.reset(); nomotion_update_srv_.reset(); + executor_thread_.reset(); // to make sure initial_pose_sub_ completely exit initial_pose_sub_.reset(); laser_scan_connection_.disconnect(); + tf_listener_.reset(); // listener may access lase_scan_filter_, so it should be reset earlier laser_scan_filter_.reset(); laser_scan_sub_.reset(); // Map + map_sub_.reset(); // map_sub_ may access map_, so it should be reset earlier if (map_ != NULL) { map_free(map_); map_ = nullptr; @@ -341,7 +345,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // Transforms tf_broadcaster_.reset(); - tf_listener_.reset(); tf_buffer_.reset(); // PubSub @@ -493,6 +496,15 @@ AmclNode::globalLocalizationCallback( pf_init_ = false; } +void +AmclNode::initialPoseReceivedSrv( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + std::shared_ptr/*res*/) +{ + initialPoseReceived(std::make_shared(req->pose)); +} + // force nomotion updates (amcl updating without requiring motion) void AmclNode::nomotionUpdateCallback( @@ -1102,20 +1114,20 @@ AmclNode::initParameters() // Semantic checks if (laser_likelihood_max_dist_ < 0) { RCLCPP_WARN( - get_logger(), "You've set laser_likelihood_max_dist to be negtive," + get_logger(), "You've set laser_likelihood_max_dist to be negative," " this isn't allowed so it will be set to default value 2.0."); laser_likelihood_max_dist_ = 2.0; } if (max_particles_ < 0) { RCLCPP_WARN( - get_logger(), "You've set max_particles to be negtive," + get_logger(), "You've set max_particles to be negative," " this isn't allowed so it will be set to default value 2000."); max_particles_ = 2000; } if (min_particles_ < 0) { RCLCPP_WARN( - get_logger(), "You've set min_particles to be negtive," + get_logger(), "You've set min_particles to be negative," " this isn't allowed so it will be set to default value 500."); min_particles_ = 500; } @@ -1129,7 +1141,7 @@ AmclNode::initParameters() if (resample_interval_ <= 0) { RCLCPP_WARN( - get_logger(), "You've set resample_interval to be zero or negtive," + get_logger(), "You've set resample_interval to be zero or negative," " this isn't allowed so it will be set to default value to 1."); resample_interval_ = 1; } @@ -1542,6 +1554,10 @@ AmclNode::initServices() "reinitialize_global_localization", std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3)); + initial_guess_srv_ = create_service( + "set_initial_pose", + std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3)); + nomotion_update_srv_ = create_service( "request_nomotion_update", std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3)); diff --git a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp index 491e64ecf1e..1148c2828db 100644 --- a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp +++ b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp @@ -53,6 +53,17 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) self = reinterpret_cast(data->laser); + // Pre-compute a couple of things + double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; + double z_rand_mult = 1.0 / data->range_max; + + step = (data->range_count - 1) / (self->max_beams_ - 1); + + // Step size must be at least 1 + if (step < 1) { + step = 1; + } + total_weight = 0.0; // Compute the sample weights @@ -65,17 +76,6 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) p = 1.0; - // Pre-compute a couple of things - double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; - double z_rand_mult = 1.0 / data->range_max; - - step = (data->range_count - 1) / (self->max_beams_ - 1); - - // Step size must be at least 1 - if (step < 1) { - step = 1; - } - for (i = 0; i < data->range_count; i += step) { obs_range = data->ranges[i][0]; obs_bearing = data->ranges[i][1]; diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 96fc1540135..fe539b0965f 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -204,6 +204,9 @@ list(APPEND plugin_libs nav2_smoother_selector_bt_node) add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp) list(APPEND plugin_libs nav2_goal_checker_selector_bt_node) +add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp) +list(APPEND plugin_libs nav2_progress_checker_selector_bt_node) + add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp) list(APPEND plugin_libs nav2_goal_updated_controller_bt_node) 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 4e1029bf9ab..a8e3aa26e68 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 @@ -95,7 +95,7 @@ class BtActionNode : public BT::ActionNodeBase RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); if (!action_client_->wait_for_action_server(1s)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", + node_->get_logger(), "\"%s\" action server not available after waiting for 1s", action_name.c_str()); throw std::runtime_error( std::string("Action server ") + action_name + 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 026fdb4cbc8..5c4fb4b1739 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 @@ -64,6 +64,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("action_server_result_timeout")) { node->declare_parameter("action_server_result_timeout", 900.0); } + if (!node->has_parameter("wait_for_service_timeout")) { + node->declare_parameter("wait_for_service_timeout", 1000); + } std::vector error_code_names = { "follow_path_error_code", diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp new file mode 100644 index 00000000000..e996bda55bf --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp @@ -0,0 +1,96 @@ +// 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 NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_ + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The ProgressCheckerSelector behavior is used to switch the progress checker + * of the controller server. It subscribes to a topic "progress_checker_selector" + * to get the decision about what progress_checker must be used. It is usually used before of + * the FollowPath. The selected_progress_checker output port is passed to progress_checker_id + * input port of the FollowPath + */ +class ProgressCheckerSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ProgressCheckerSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + ProgressCheckerSelector( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "default_progress_checker", + "the default progress_checker to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "progress_checker_selector", + "the input topic name to select the progress_checker"), + + BT::OutputPort( + "selected_progress_checker", + "Selected progress_checker by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the progress_checker_selector topic + * + * @param msg the message with the id of the progress_checker_selector + */ + void callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr progress_checker_selector_sub_; + + std::string last_selected_progress_checker_; + + rclcpp::Node::SharedPtr node_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 27ba3e9a579..3e23aafbd0a 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -182,6 +182,12 @@ Name of the selected goal checker received from the topic subcription + + Name of the topic to receive progress checker selection commands + Default progress checker of the controller selector + Name of the selected progress checker received from the topic subcription + + Spin distance Allowed time for spinning diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 21f49e98eb0..53ca2b0dcae 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.6 + 1.2.7 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp new file mode 100644 index 00000000000..e8c418ddc17 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp @@ -0,0 +1,81 @@ +// 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 + +#include "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +ProgressCheckerSelector::ProgressCheckerSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + progress_checker_selector_sub_ = node_->create_subscription( + topic_name_, qos, std::bind(&ProgressCheckerSelector::callbackProgressCheckerSelect, this, _1)); +} + +BT::NodeStatus ProgressCheckerSelector::tick() +{ + rclcpp::spin_some(node_); + + // This behavior always use the last selected progress checker received from the topic input. + // When no input is specified it uses the default goaprogressl checker. + // If the default progress checker is not specified then we work in + // "required progress checker mode": In this mode, the behavior returns failure if the progress + // checker selection is not received from the topic input. + if (last_selected_progress_checker_.empty()) { + std::string default_progress_checker; + getInput("default_progress_checker", default_progress_checker); + if (default_progress_checker.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_progress_checker_ = default_progress_checker; + } + } + + setOutput("selected_progress_checker", last_selected_progress_checker_); + + return BT::NodeStatus::SUCCESS; +} + +void +ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_progress_checker_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ProgressCheckerSelector"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index c640e824244..4aaf185306b 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -107,3 +107,7 @@ ament_target_dependencies(test_smoother_selector_node ${dependencies}) ament_add_gtest(test_goal_checker_selector_node test_goal_checker_selector_node.cpp) target_link_libraries(test_goal_checker_selector_node nav2_goal_checker_selector_bt_node) ament_target_dependencies(test_goal_checker_selector_node ${dependencies}) + +ament_add_gtest(test_progress_checker_selector_node test_progress_checker_selector_node.cpp) +target_link_libraries(test_progress_checker_selector_node nav2_progress_checker_selector_bt_node) +ament_target_dependencies(test_progress_checker_selector_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp new file mode 100644 index 00000000000..574a514886b --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp @@ -0,0 +1,187 @@ +// 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 +#include +#include + +#include "utils/test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class ProgressCheckerSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("progress_checker_selector_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, config); + }; + + factory_->registerBuilder( + "ProgressCheckerSelector", + builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ProgressCheckerSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * ProgressCheckerSelectorTestFixture::config_ = nullptr; +std::shared_ptr ProgressCheckerSelectorTestFixture::factory_ = nullptr; +std::shared_ptr ProgressCheckerSelectorTestFixture::tree_ = nullptr; + +TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_progress_checker_result; + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + + EXPECT_EQ(selected_progress_checker_result, "SimpleProgressCheck"); + + std_msgs::msg::String selected_progress_checker_cmd; + + selected_progress_checker_cmd.data = "AngularProgressChecker"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto progress_checker_selector_pub = + node_->create_publisher( + "progress_checker_selector_custom_topic_name", qos); + + // publish a few updates of the selected_progress_checker + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + progress_checker_selector_pub->publish(selected_progress_checker_cmd); + + rclcpp::spin_some(node_); + } + + // check progress_checker updated + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + EXPECT_EQ("AngularProgressChecker", selected_progress_checker_result); +} + +TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_progress_checker_result; + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + + EXPECT_EQ(selected_progress_checker_result, "GridBased"); + + std_msgs::msg::String selected_progress_checker_cmd; + + selected_progress_checker_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto progress_checker_selector_pub = + node_->create_publisher("progress_checker_selector", qos); + + // publish a few updates of the selected_progress_checker + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + progress_checker_selector_pub->publish(selected_progress_checker_cmd); + + rclcpp::spin_some(node_); + } + + // check goal_checker updated + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + EXPECT_EQ("RRT", selected_progress_checker_result); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index bae36129b5c..6ab1b283d21 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.6 + 1.2.7 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 6ffb1376a72..208e6882186 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.6 + 1.2.7 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 57af0654514..f4a3dbe6554 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.6 + 1.2.7 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 815694d83c6..940c53148ac 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -238,6 +238,8 @@ class Polygon double simulation_time_step_; /// @brief Whether polygon is enabled bool enabled_; + /// @brief Wether the subscription to polygon topic has transient local QoS durability + bool polygon_subscribe_transient_local_; /// @brief Polygon subscription rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 99ab6b3cf4b..c0ea7fea457 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.6 + 1.2.7 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 248bdcec6de..c215cdf2da8 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -70,6 +70,9 @@ bool Polygon::configure() "[%s]: Subscribing on %s topic for polygon", polygon_name_.c_str(), polygon_sub_topic.c_str()); rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + if (polygon_subscribe_transient_local_) { + polygon_qos.transient_local(); + } polygon_sub_ = node->create_subscription( polygon_sub_topic, polygon_qos, std::bind(&Polygon::polygonCallback, this, std::placeholders::_1)); @@ -452,6 +455,10 @@ bool Polygon::getParameters( footprint_topic = node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); } + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false)); + polygon_subscribe_transient_local_ = + node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool(); } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, diff --git a/nav2_common/package.xml b/nav2_common/package.xml index bc40b382fd2..74a7e69a3f6 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.6 + 1.2.7 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 f8e740c9aed..c90edc7c828 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.6 + 1.2.7 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 3289376f9cd..48a7359614d 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.6 + 1.2.7 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c6908bb5115..447d8534819 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -472,6 +472,8 @@ void ControllerServer::computeControl() last_valid_cmd_time_ = now(); rclcpp::WallRate loop_rate(controller_frequency_); while (rclcpp::ok()) { + auto start_time = this->now(); + if (action_server_ == nullptr || !action_server_->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; @@ -499,10 +501,12 @@ void ControllerServer::computeControl() break; } + auto cycle_duration = this->now() - start_time; if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), "Control loop missed its desired rate of %.4fHz", - controller_frequency_); + get_logger(), + "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", + controller_frequency_, 1 / cycle_duration.seconds()); } } } catch (nav2_core::InvalidController & e) { diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 8dc4896b61e..37e4a6a4c10 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.6 + 1.2.7 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index f039cf5d42b..3be30b28539 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -26,17 +26,10 @@ find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) -find_package(Eigen3 REQUIRED) +find_package(Eigen3 3.3 REQUIRED) nav2_package() -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} -) - -add_definitions(${EIGEN3_DEFINITIONS}) - add_library(nav2_costmap_2d_core SHARED src/array_parser.cpp src/costmap_2d.cpp @@ -52,6 +45,13 @@ add_library(nav2_costmap_2d_core SHARED src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_core ALIAS nav2_costmap_2d_core) + +target_include_directories(nav2_costmap_2d_core + PUBLIC + "$" + "$" +) set(dependencies geometry_msgs @@ -79,6 +79,7 @@ set(dependencies ament_target_dependencies(nav2_costmap_2d_core ${dependencies} ) +target_link_libraries(nav2_costmap_2d_core Eigen3::Eigen) add_library(layers SHARED plugins/inflation_layer.cpp @@ -89,11 +90,12 @@ add_library(layers SHARED plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp ) +add_library(${PROJECT_NAME}::layers ALIAS layers) ament_target_dependencies(layers ${dependencies} ) target_link_libraries(layers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(filters SHARED @@ -101,11 +103,14 @@ add_library(filters SHARED plugins/costmap_filters/speed_filter.cpp plugins/costmap_filters/binary_filter.cpp ) +add_library(${PROJECT_NAME}::filters ALIAS filters) + + ament_target_dependencies(filters ${dependencies} ) target_link_libraries(filters - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(nav2_costmap_2d_client SHARED @@ -113,18 +118,19 @@ add_library(nav2_costmap_2d_client SHARED src/costmap_subscriber.cpp src/costmap_topic_collision_checker.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_client ALIAS nav2_costmap_2d_client) ament_target_dependencies(nav2_costmap_2d_client ${dependencies} ) target_link_libraries(nav2_costmap_2d_client - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp) target_link_libraries(nav2_costmap_2d_markers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_target_dependencies(nav2_costmap_2d_markers @@ -133,7 +139,7 @@ ament_target_dependencies(nav2_costmap_2d_markers add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp) target_link_libraries(nav2_costmap_2d_cloud - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d src/costmap_2d_node.cpp) @@ -142,26 +148,24 @@ ament_target_dependencies(nav2_costmap_2d ) target_link_libraries(nav2_costmap_2d - nav2_costmap_2d_core - layers - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers + ${PROJECT_NAME}::filters ) install(TARGETS - nav2_costmap_2d_core layers filters + nav2_costmap_2d_core nav2_costmap_2d_client + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(TARGETS - nav2_costmap_2d - nav2_costmap_2d_markers - nav2_costmap_2d_cloud - RUNTIME DESTINATION lib/${PROJECT_NAME} +install(TARGETS nav2_costmap_2d_markers nav2_costmap_2d_cloud nav2_costmap_2d + DESTINATION lib/${PROJECT_NAME} ) install(FILES costmap_plugins.xml @@ -169,7 +173,7 @@ install(FILES costmap_plugins.xml ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -184,8 +188,7 @@ if(BUILD_TESTING) pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml) endif() -ament_export_include_directories(include) -ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index aa3803a3156..f13d8c0c5cd 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.2.6 + 1.2.7 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_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 0768972a0bb..6e0e32a3bd4 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -1,79 +1,58 @@ ament_add_gtest_executable(footprint_tests_exec footprint_tests.cpp ) -ament_target_dependencies(footprint_tests_exec - ${dependencies} -) target_link_libraries(footprint_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(test_collision_checker_exec test_costmap_topic_collision_checker.cpp ) -ament_target_dependencies(test_collision_checker_exec - ${dependencies} -) target_link_libraries(test_collision_checker_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(inflation_tests_exec inflation_tests.cpp ) -ament_target_dependencies(inflation_tests_exec - ${dependencies} -) target_link_libraries(inflation_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(obstacle_tests_exec obstacle_tests.cpp ) -ament_target_dependencies(obstacle_tests_exec - ${dependencies} -) target_link_libraries(obstacle_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(range_tests_exec range_tests.cpp ) -ament_target_dependencies(range_tests_exec - ${dependencies} -) target_link_libraries(range_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest(dyn_params_tests dyn_params_tests.cpp ) -ament_target_dependencies(dyn_params_tests - ${dependencies} -) target_link_libraries(dyn_params_tests - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest_executable(test_costmap_publisher_exec test_costmap_2d_publisher.cpp ) -ament_target_dependencies(test_costmap_publisher_exec - ${dependencies} -) target_link_libraries(test_costmap_publisher_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client + ${PROJECT_NAME}::layers ) ament_add_test(test_collision_checker @@ -145,6 +124,6 @@ ament_add_test(test_costmap_publisher_exec # ${dependencies} # ) # target_link_libraries(costmap_tester -# nav2_costmap_2d_core +# ${PROJECT_NAME}::nav2_costmap_2d_core # layers # ) diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index c9a52aa8af7..ab9f01ee4ad 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,7 +1,7 @@ # Bresenham2D corner cases test ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) target_link_libraries(costmap_bresenham_2d - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) # OrderLayer for checking Costmap2D plugins API calling order @@ -10,6 +10,10 @@ add_library(order_layer SHARED ament_target_dependencies(order_layer ${dependencies} ) + +target_link_libraries(order_layer + ${PROJECT_NAME}::nav2_costmap_2d_core +) install(TARGETS order_layer ARCHIVE DESTINATION lib @@ -20,5 +24,5 @@ install(TARGETS # Costmap2D plugins API calling order test ament_add_gtest(plugin_api_order plugin_api_order.cpp) target_link_libraries(plugin_api_order - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index b9480fdffab..a4148ad7d26 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -5,58 +5,59 @@ target_link_libraries(array_parser_test ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) target_link_libraries(costmap_convesion_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(declare_parameter_test declare_parameter_test.cpp) target_link_libraries(declare_parameter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_test costmap_filter_test.cpp) target_link_libraries(costmap_filter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(keepout_filter_test keepout_filter_test.cpp) target_link_libraries(keepout_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(speed_filter_test speed_filter_test.cpp) target_link_libraries(speed_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(binary_filter_test binary_filter_test.cpp) target_link_libraries(binary_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(copy_window_test copy_window_test.cpp) target_link_libraries(copy_window_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_service_test costmap_filter_service_test.cpp) target_link_libraries(costmap_filter_service_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_processing_test.cpp) target_link_libraries(denoise_layer_test - nav2_costmap_2d_core layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest(lifecycle_test lifecycle_test.cpp) target_link_libraries(lifecycle_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) \ No newline at end of file diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt index 4641d0b5471..3cb0d967f88 100644 --- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt +++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt @@ -12,7 +12,7 @@ include_directories( include ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/costmap_queue.cpp src/limited_costmap_queue.cpp ) diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 13177acd3f4..b03c8096aa9 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.2.6 + 1.2.7 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 7c1fd743163..279f4264f4a 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.2.6 + 1.2.7 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index ffe3027910f..f3b7abf4811 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.2.6 + 1.2.7 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 203f1ff4169..0a596b77be6 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.2.6 + 1.2.7 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 0a472fd7ef9..3526def3f22 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.2.6 + 1.2.7 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 3641265db1a..8b5c02d4f79 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.2.6 + 1.2.7 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 af09ece7af9..1ed2f94a325 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.2.6 + 1.2.7 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 a5237a446d6..0b9425c70bb 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.2.6 + 1.2.7 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index eedd360803b..93062f985ca 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.2.6 + 1.2.7 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 2050e91cc55..24f5315d0c6 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.6 + 1.2.7 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1501e4321a1..d2bb6d9c9da 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -79,6 +79,7 @@ add_library(mppi_controller SHARED add_library(mppi_critics SHARED src/critics/obstacles_critic.cpp + src/critics/cost_critic.cpp src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 987b4bc6c07..740dcae1b6a 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -98,6 +98,9 @@ This process is then repeated a number of times and returns a converged solution #### Obstacles Critic + +Uses estimated distances from obstacles using cost and inflation parameters to avoid obstacles + | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | @@ -109,6 +112,20 @@ This process is then repeated a number of times and returns a converged solution | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | +#### Cost Critic + +Uses inflated costmap cost directly to avoid obstacles + + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | + | cost_weight | double | Default 3.81. Wight to apply to critic to avoid obstacles. | + | cost_power | int | Default 1. Power order to apply to term. | + | collision_cost | double | Default 1000000.0. Cost to apply to a true collision in a trajectory. | + | critical_cost | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles. | + | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. + | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | + #### Path Align Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -184,7 +201,7 @@ controller_server: time_step: 3 AckermannConstraints: min_turning_r: 0.2 - critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] ConstraintCritic: enabled: true cost_power: 1 @@ -204,15 +221,24 @@ controller_server: cost_power: 1 cost_weight: 5.0 threshold_to_consider: 0.5 - ObstaclesCritic: + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + CostCritic: enabled: true cost_power: 1 - repulsion_weight: 1.5 - critical_weight: 20.0 - consider_footprint: false - collision_cost: 10000.0 - collision_margin_distance: 0.1 - near_goal_distance: 0.5 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + PathAlignCritic: PathAlignCritic: enabled: true cost_power: 1 diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 790568e7967..d578d23a9ec 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -5,6 +5,10 @@ mppi critic for obstacle avoidance + + mppi critic for obstacle avoidance using costmap score + + mppi critic for driving towards the goal diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp new file mode 100644 index 00000000000..77df20adcb5 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2023 Robocc Brice Renaudeau +// +// 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 NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::CostCritic + * @brief Critic objective function for avoiding obstacles using costmap's inflated cost + */ +class CostCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to obstacle avoidance + * + * @param costs [out] add obstacle cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + /** + * @brief Checks if cost represents a collision + * @param cost Point cost at pose center + * @param x X of pose + * @param y Y of pose + * @param theta theta of pose + * @return bool if in collision + */ + bool inCollision(float cost, float x, float y, float theta); + + /** + * @brief cost at a robot pose + * @param x X of pose + * @param y Y of pose + * @return Collision information at pose + */ + float costAtPose(float x, float y); + + /** + * @brief Find the min cost of the inflation decay function for which the robot MAY be + * in collision in any orientation + * @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) + * @return double circumscribed cost, any higher than this and need to do full footprint collision checking + * since some element of the robot could be in collision + */ + float findCircumscribedCost(std::shared_ptr costmap); + +protected: + nav2_costmap_2d::FootprintCollisionChecker + collision_checker_{nullptr}; + float possibly_inscribed_cost_; + + bool consider_footprint_{true}; + float circumscribed_radius_{0}; + float circumscribed_cost_{0}; + float collision_cost_{0}; + float critical_cost_{0}; + float weight_{0}; + + float near_goal_distance_; + std::string inflation_layer_name_; + + unsigned int power_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 5fe6022b554..dd6142c52fa 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.2.6 + 1.2.7 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp new file mode 100644 index 00000000000..b91ab075b6a --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -0,0 +1,212 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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_mppi_controller/critics/cost_critic.hpp" + +namespace mppi::critics +{ + +void CostCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 3.81); + getParam(critical_cost_, "critical_cost", 300.0); + getParam(collision_cost_, "collision_cost", 1000000.0); + getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + + // Normalized by cost value to put in same regime as other weights + weight_ /= 254.0f; + + collision_checker_.setCostmap(costmap_); + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + + if (possibly_inscribed_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "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."); + } + + RCLCPP_INFO( + logger_, + "InflationCostCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_cost_, weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +float CostCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + bool inflation_layer_found = false; + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin(); + layer != costmap->getLayeredCostmap()->getPlugins()->end(); + ++layer) + { + auto inflation_layer = std::dynamic_pointer_cast(*layer); + if (!inflation_layer || + (!inflation_layer_name_.empty() && + inflation_layer->getName() != inflation_layer_name_)) + { + continue; + } + + inflation_layer_found = true; + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + } + + if (!inflation_layer_found) { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +void CostCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + if (!enabled_) { + return; + } + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + } + + // If near the goal, don't apply the preferential term since the goal is near obstacles + bool near_goal = false; + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) { + near_goal = true; + } + + auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + repulsive_cost.fill(0.0); + + const size_t traj_len = data.trajectories.x.shape(1); + bool all_trajectories_collide = true; + for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { + bool trajectory_collide = false; + const auto & traj = data.trajectories; + float pose_cost; + + for (size_t j = 0; j < traj_len; j++) { + // The costAtPose doesn't use orientation + // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it + // So the center point has more information than the footprint + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j)); + if (pose_cost < 1.0f) {continue;} // In free space + + if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) { + trajectory_collide = true; + break; + } + + // Let near-collision trajectory points be punished severely + // Note that we collision check based on the footprint actual, + // but score based on the center-point cost regardless + using namespace nav2_costmap_2d; // NOLINT + if (pose_cost >= INSCRIBED_INFLATED_OBSTACLE) { + repulsive_cost[i] += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + repulsive_cost[i] += pose_cost; + } + } + + if (!trajectory_collide) { + all_trajectories_collide = false; + } else { + repulsive_cost[i] = collision_cost_; + } + } + + data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_); + data.fail_flag = all_trajectories_collide; +} + +/** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ +bool CostCritic::inCollision(float cost, float x, float y, float theta) +{ + bool is_tracking_unknown = + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + + // If consider_footprint_ check footprint scort for collision + if (consider_footprint_ && + (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + } + + switch (static_cast(cost)) { + using namespace nav2_costmap_2d; // NOLINT + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint_ ? false : true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + + return false; +} + +float CostCritic::costAtPose(float x, float y) +{ + using namespace nav2_costmap_2d; // NOLINT + unsigned int x_i, y_i; + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { + return nav2_costmap_2d::NO_INFORMATION; + } + + return collision_checker_.pointCost(x_i, y_i); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::CostCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index f88249aed14..af2a61e20e7 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -24,6 +24,7 @@ #include "nav2_mppi_controller/critics/goal_angle_critic.hpp" #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" +#include "nav2_mppi_controller/critics/cost_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" #include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index d694496395d..9a77f6d8b06 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -103,7 +103,7 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( "DiffDrive", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"CostCritic"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index e588ff0fa5f..317117e9bdf 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -37,6 +37,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ManageLifecycleNodes.srv" "srv/LoadMap.srv" "srv/SaveMap.srv" + "srv/SetInitialPose.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 713a5e1bd80..3261672ed70 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.2.6 + 1.2.7 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_msgs/srv/SetInitialPose.srv b/nav2_msgs/srv/SetInitialPose.srv new file mode 100644 index 00000000000..ec1e50b65f1 --- /dev/null +++ b/nav2_msgs/srv/SetInitialPose.srv @@ -0,0 +1,3 @@ +geometry_msgs/PoseWithCovarianceStamped pose +--- + diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index e3da25fcd01..97fe29232be 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.2.6 + 1.2.7 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 5ea2e322431..58b8343666a 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.2.6 + 1.2.7 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6e7199deeb1..1278656cf32 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -412,7 +412,10 @@ void PlannerServer::computePlanThroughPoses() if (i == 0) { curr_start = start; } else { - curr_start = goal->goals[i - 1]; + // pick the end of the last planning task as the start for the next one + // to allow for path tolerance deviations + curr_start = concat_path.poses.back(); + curr_start.header = concat_path.header; } curr_goal = goal->goals[i]; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 6b51d87cdbb..72b5c09665d 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.2.6 + 1.2.7 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index b3e65e7667c..97989ee9834 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.2.6 + 1.2.7 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index f5a781a262f..659058f84f4 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.2.6 + 1.2.7 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 76a6c3d79be..d6c87b0169c 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -420,7 +420,7 @@ def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False) def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): """Send a `ComputePathThroughPoses` action request.""" - rtn = self.__getPathThroughPosesImpl(start, goals, planner_id, use_start) + rtn = self._getPathThroughPosesImpl(start, goals, planner_id, use_start) if self.status != GoalStatus.STATUS_SUCCEEDED: self.warn(f'Getting path failed with status code: {self.status}') diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index a72545bed1d..d6557a2c7a6 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.2.6 + 1.2.7 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 5f437ce546a..e8a88cb7b50 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -131,7 +131,7 @@ planner_server: max_iterations: 1000 w_smooth: 0.3 w_data: 0.2 - tolerance: 1e-10 + tolerance: 1.0e-10 do_refinement: true # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further ``` diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index 18bf38576fc..831ba0b776b 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -393,7 +393,7 @@ class NodeHybrid static float getObstacleHeuristic( const Coordinates & node_coords, const Coordinates & goal_coords, - const double & cost_penalty); + const float & cost_penalty); /** * @brief Compute the Distance heuristic diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 84cb33aae0b..a5406bc37b3 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.2.6 + 1.2.7 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 20d288809da..549570fb262 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -62,6 +62,7 @@ void GridCollisionChecker::setFootprint( return; } + oriented_footprints_.clear(); oriented_footprints_.reserve(angles_.size()); double sin_th, cos_th; geometry_msgs::msg::Point new_pt; diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 73f774bf735..0e86e7c5706 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -433,7 +433,7 @@ void NodeHybrid::resetObstacleHeuristic( float NodeHybrid::getObstacleHeuristic( const Coordinates & node_coords, const Coordinates & goal_coords, - const double & cost_penalty) + const float & cost_penalty) { // If already expanded, return the cost const unsigned int size_x = sampled_costmap->getSizeInCellsX(); diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 80577fcb559..42dbd4efe0b 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -304,6 +304,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } // Set collision checker and costmap information + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 8b46fc3ae74..75c54517a6c 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -243,6 +243,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( std::unique_lock lock(*(_costmap->getMutex())); // Set collision checker and costmap information + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 90b8f32cade..d55321b3f34 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.2.6 + 1.2.7 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 7fa094ff735..bbdef57a8fb 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.2.6 + 1.2.7 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index f84ff994c23..b58cd83095f 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.2.6 + 1.2.7 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index eec9c2dd9f7..c8b1660840f 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -315,7 +315,7 @@ class SimpleActionServer if (steady_clock::now() - start_time >= server_timeout_) { terminate_all(); if (completion_callback_) {completion_callback_();} - throw std::runtime_error("Action callback is still running and missed deadline to stop"); + error_msg("Action callback is still running and missed deadline to stop"); } } diff --git a/nav2_util/package.xml b/nav2_util/package.xml index d1898925a9a..298b94eedc5 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.6 + 1.2.7 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 52b90c48618..c9a9775135a 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.2.6 + 1.2.7 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 8f153aeed24..d981fac4e71 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.2.6 + 1.2.7 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 3d94808a97a..c489b9f2a69 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.2.6 + 1.2.7 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 83661291937..b726fb2c272 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.6 + 1.2.7 ROS2 Navigation Stack From 33d605c30c44d0b82fa3ae136bf95b0cbfac330f Mon Sep 17 00:00:00 2001 From: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com> Date: Sun, 21 Apr 2024 23:44:53 +0200 Subject: [PATCH 07/19] Backport/wait for server timeout (#4261) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * WIP: Make BT nodes have configurable wait times. (#3960) * Make BT nodes have configurable wait times. Previous solution provided hardcoded 1s value. Right now the value can be configured for BT Action, Cancel and Service nodes. [#3920] Signed-off-by: Adam Galecki * Make BT nodes have configurable wait times. Previous solution provided hardcoded 1s value. Right now the value can be configured for BT Action, Cancel and Service nodes. [#3920] Signed-off-by: Adam Galecki * Fix typos, linting errors and value type from float to int * Fix extra underscores * Fix extra underscore * Update unit tests with blackboard parameter Signed-off-by: Adam Galecki * Fix formatting errors Signed-off-by: Adam Galecki * Update system tests to match new parameter Signed-off-by: Adam Galecki --------- Signed-off-by: Adam Galecki Signed-off-by: Kemal Bektas * chore(nav2_behavior_tree): log actual wait period in bt_action_node (#4178) Signed-off-by: Felix Co-authored-by: Felix Signed-off-by: Kemal Bektas --------- Signed-off-by: Adam Galecki Signed-off-by: Kemal Bektas Signed-off-by: Felix Co-authored-by: Adam Gałecki <73710544+embeddedadam@users.noreply.github.com> Co-authored-by: bi0ha2ard Co-authored-by: Felix --- .../include/nav2_behavior_tree/bt_action_node.hpp | 12 +++++++++--- .../include/nav2_behavior_tree/bt_action_server.hpp | 3 +++ .../nav2_behavior_tree/bt_action_server_impl.hpp | 9 +++++++++ .../nav2_behavior_tree/bt_cancel_action_node.hpp | 6 +++++- .../include/nav2_behavior_tree/bt_service_node.hpp | 7 ++++++- .../plugins/action/test_assisted_teleop_action.cpp | 3 +++ .../action/test_assisted_teleop_cancel_node.cpp | 3 +++ .../test/plugins/action/test_back_up_action.cpp | 3 +++ .../test/plugins/action/test_back_up_cancel_node.cpp | 3 +++ .../test/plugins/action/test_bt_action_node.cpp | 1 + .../plugins/action/test_clear_costmap_service.cpp | 9 +++++++++ .../test_compute_path_through_poses_action.cpp | 3 +++ .../action/test_compute_path_to_pose_action.cpp | 3 +++ .../plugins/action/test_controller_cancel_node.cpp | 3 +++ .../plugins/action/test_drive_on_heading_action.cpp | 3 +++ .../action/test_drive_on_heading_cancel_node.cpp | 3 +++ .../test/plugins/action/test_follow_path_action.cpp | 3 +++ .../action/test_navigate_through_poses_action.cpp | 3 +++ .../plugins/action/test_navigate_to_pose_action.cpp | 3 +++ ...test_reinitialize_global_localization_service.cpp | 3 +++ .../test/plugins/action/test_smooth_path_action.cpp | 3 +++ .../test/plugins/action/test_spin_action.cpp | 3 +++ .../test/plugins/action/test_spin_cancel_node.cpp | 3 +++ .../test/plugins/action/test_wait_action.cpp | 3 +++ .../test/plugins/action/test_wait_cancel_node.cpp | 3 +++ nav2_bringup/params/nav2_multirobot_params_1.yaml | 1 + nav2_bringup/params/nav2_multirobot_params_2.yaml | 1 + nav2_bringup/params/nav2_multirobot_params_all.yaml | 1 + nav2_bringup/params/nav2_params.yaml | 1 + .../src/behavior_tree/test_behavior_tree_node.cpp | 2 ++ .../src/gps_navigation/nav2_no_map_params.yaml | 1 + 31 files changed, 103 insertions(+), 5 deletions(-) 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 a8e3aa26e68..d34f6097245 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 @@ -61,6 +61,8 @@ class BtActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); // Initialize the input and output messages goal_ = typename ActionT::Goal(); @@ -93,10 +95,11 @@ class BtActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(1s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for 1s", - action_name.c_str()); + node_->get_logger(), "\"%s\" action server not available after waiting for %f s", + action_name.c_str(), + wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string("Action server ") + action_name + std::string(" not available")); @@ -460,6 +463,9 @@ class BtActionNode : public BT::ActionNodeBase // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_duration_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // To track the action server acknowledgement when a new goal is sent std::shared_ptr::SharedPtr>> future_goal_handle_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 2dcb21e42bb..4e98fea4908 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -247,6 +247,9 @@ class BtActionServer // Default timeout value while waiting for response from a server std::chrono::milliseconds default_server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; 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 5c4fb4b1739..fa5c5502249 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 @@ -161,6 +161,9 @@ bool BtActionServer::on_configure() int default_server_timeout; node->get_parameter("default_server_timeout", default_server_timeout); default_server_timeout_ = std::chrono::milliseconds(default_server_timeout); + int wait_for_service_timeout; + node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); + wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -175,6 +178,9 @@ bool BtActionServer::on_configure() blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT + blackboard_->set( + "wait_for_service_timeout", + wait_for_service_timeout_); return true; } @@ -238,6 +244,9 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); + blackboard->set( + "wait_for_service_timeout", + wait_for_service_timeout_); } } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); 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 e87ffbfac48..7831f533bc3 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 @@ -59,6 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); std::string remapped_action_name; if (getInput("server_name", remapped_action_name)) { @@ -89,7 +91,7 @@ class BtCancelActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(1s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", action_name.c_str()); @@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase // The timeout value while waiting for response from a server when a // new action goal is canceled std::chrono::milliseconds server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; }; } // namespace nav2_behavior_tree 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 cd0cd524b97..4a04a147698 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 @@ -62,6 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); @@ -77,7 +79,7 @@ class BtServiceNode : public BT::ActionNodeBase RCLCPP_DEBUG( node_->get_logger(), "Waiting for \"%s\" service", service_name_.c_str()); - if (!service_client_->wait_for_service(1s)) { + if (!service_client_->wait_for_service(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", service_node_name.c_str()); @@ -249,6 +251,9 @@ class BtServiceNode : public BT::ActionNodeBase // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_duration_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // To track the server response when a new request is sent std::shared_future future_result_; bool request_sent_{false}; diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp index 38033598d01..4bb99057591 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -68,6 +68,9 @@ class AssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index a7af3efef6a..40f6f65c88a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "assisted_teleop"); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 6f0d54e7ca9..85e3d3764dc 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -68,6 +68,9 @@ class BackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp index 35ab9d2f88d..99e6eb1b45d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelBackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "back_up"); diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index 12c25d30d6f..a283dee33e7 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -173,6 +173,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("node", node_); config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); + config_->blackboard->set("wait_for_service_timeout", 1000ms); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("on_cancelled_triggered", false); diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index 4351152acfb..88173e84205 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -53,6 +53,9 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -139,6 +142,9 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -231,6 +237,9 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 40bbeb1a375..ec619b01205 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -76,6 +76,9 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index ca5bcea666c..d726186cd31 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -74,6 +74,9 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index 5128391d03b..290c9133a52 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelControllerActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "follow_path"); diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index 043f1aedb62..06478df588f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -69,6 +69,9 @@ class DriveOnHeadingActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp index cc2e7f0693f..5378123a461 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelDriveOnHeadingTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "drive_on_heading_cancel"); diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 4fa291c4b78..c4fcc575193 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -67,6 +67,9 @@ class FollowPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index bd028827af8..5fa2c9aa036 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -70,6 +70,9 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); std::vector poses; config_->blackboard->set>( diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index 13e3a2a2ca9..264b775a680 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -68,6 +68,9 @@ class NavigateToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 122e6d0e0b1..2f7a36e7fa7 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -53,6 +53,9 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index d1c930b5e40..c2e43890212 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -67,6 +67,9 @@ class SmoothPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index 56f5cdae746..a6d9f36d57a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -68,6 +68,9 @@ class SpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index d773e88bcad..ea3b01b983f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelSpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "spin"); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 75a115b8f58..9ab4d1c1af4 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -59,6 +59,9 @@ class WaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp index e8d4d053c7d..0773c72c6da 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelWaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "wait"); diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 692ca6ec803..d0bdce21db2 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 2d57aef3830..cd51b1332e4 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 6f7e66cd617..b6e5712092f 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 1474f33a675..356118f19db 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 48ba35038cd..48b97848f8f 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -138,6 +138,8 @@ class BehaviorTreeHandler "server_timeout", std::chrono::milliseconds(20)); // NOLINT blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT + blackboard->set( + "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 2539cef7b59..7b3125afbae 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -5,6 +5,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" From 4e16d3c7e3cfac08c4b79841f19570c40ebbe14b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 22 Apr 2024 15:36:00 -0700 Subject: [PATCH 08/19] [Iron] Fixing 3240 Signed-off-by: Steve Macenski --- nav2_mppi_controller/src/critics/path_align_critic.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index cd913e5b0ee..b103f5b5f64 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -98,6 +98,7 @@ void PathAlignCritic::score(CriticData & data) traj_integrated_distance = 0.0f; summed_path_dist = 0.0f; num_samples = 0.0f; + path_pt = 0u; const auto T_x = xt::view(data.trajectories.x, t, xt::all()); const auto T_y = xt::view(data.trajectories.y, t, xt::all()); for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { From 4cb6aba4ac9b98caa78cd0e91539b6e9f8f85818 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 23 May 2024 16:34:28 -0700 Subject: [PATCH 09/19] Iron Sync 6: May 23, 2024 (#4366) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Scale cost critic's weight when dynamically updated (#4246) * Scale cost critic's weight when dynamically updated Signed-off-by: pepisg * sign off Signed-off-by: pepisg --------- Signed-off-by: pepisg * Add expanding the ~/ to the full home dir of user in the path to the map yaml. (#4258) * Add user home expander of home sequence Signed-off-by: Wiktor Bajor * Add passing home dir as string instead of const char* Signed-off-by: Wiktor Bajor * Add docs Signed-off-by: Wiktor Bajor * Fix function declaration Signed-off-by: Wiktor Bajor * Fix linter issues Signed-off-by: Wiktor Bajor * Uncrustify linter Signed-off-by: Wiktor Bajor * Uncrustify linter Signed-off-by: Wiktor Bajor * Uncrustify linter: remove remove whitespace Signed-off-by: Wiktor Bajor --------- Signed-off-by: Wiktor Bajor * Implement Critic for Velocity Deadband Hardware Constraints (#4256) * Adding new velocity deadband critic. - add some tests - cast double to float - add new features from "main" branch - fix formating - add cost test - fix linting issue - add README Signed-off-by: Denis Sokolov * Remove velocity deadband critic from defaults Signed-off-by: Denis Sokolov * remove old weight Signed-off-by: Denis Sokolov * fix velocity deadband critic tests Signed-off-by: Denis Sokolov --------- Signed-off-by: Denis Sokolov * removing clearable layer param (unused) (#4280) * provide message validation check API (#4276) * provide validation_message.hpp Signed-off-by: goes * fix typo Signed-off-by: goes * add test_validation_messages.cpp Signed-off-by: goes * change include-order Signed-off-by: goes * reformat Signed-off-by: goes * update test Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes * [RotationShimController] rotate also on short paths (#4290) When the path is shorter than ´forward_sampling_distance´ the rotatitonShimController would directly give control to the primary controller to navigate to the goal. This would lead to the exact behavior that was tried to be fixed by the rotationShimController: "The result is an awkward, stuttering, or whipping around behavior" [1]. It often resulted in navigation failure. In this case, the controller should try to rotate towards the last point of the path, so that the primary controller can have a better starting orientation. [1] https://navigation.ros.org/tuning/index.html#rotate-in-place-behavior * Add footprint clearing for static layer (#4282) * Add footprint clearing for static layer Signed-off-by: Tony Najjar * fix flckering --------- Signed-off-by: Tony Najjar * Fix #4268 (#4296) Signed-off-by: Steve Macenski * Fix undefined symbols in `libpf_lib.so` (#4312) When I build `nav2_amcl` with `-Wl,--no-undefined` I noticed `libpf_lib.so` has undefined symbols. This PR correctly links `libpf_lib.so` to `libm` so all symbols can be found. You can verify this by executing the following command: ``` ldd -r ./build/nav2_amcl/src/pf/libpf_lib.so linux-vdso.so.1 (0x00007ffd1f8c0000) libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x000074e909a00000) /lib64/ld-linux-x86-64.so.2 (0x000074e909e60000) undefined symbol: ceil (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: atan2 (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sin (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: hypot (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: cos (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: log (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sqrt (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: floor (./build/nav2_amcl/src/pf/libpf_lib.so) ``` Signed-off-by: Ramon Wijnands * msg validation check for `/initialpose` in `nav2_amcl` (#4301) * add validation check for PoseWithCovarianceStamped Signed-off-by: goes * remove rebundant check before Signed-off-by: goes * reformat Signed-off-by: goes * typo fixed Signed-off-by: goes * change the type-name Signed-off-by: goes * update test Signed-off-by: goes * reformat Signed-off-by: goes * . Signed-off-by: goes * add comment Signed-off-by: goes * update comment Signed-off-by: goes * change header Signed-off-by: goes * update test Signed-off-by: goes * typo fixed Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes * 4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. (#4324) Signed-off-by: Krzysztof Pawełczyk Co-authored-by: Krzysztof Pawełczyk * [LifecycleNode] add bond_heartbeat_period (#4342) * add bond_heartbeat_period Signed-off-by: Guillaume Doisy * lint Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * Update path_longer_on_approach.cpp (#4344) Fix the bug that isPathUpdated function will return false for the reason that it compare the timestaped between new path and old path's last pose Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> * [LifecycleManagerClient] clean set_initial_pose and navigate_to_pose (#4346) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * Move projectState after getPointsInside (#4356) * Modify test to check fix Signed-off-by: Brice * Add static polygon check before simulation Signed-off-by: Brice --------- Signed-off-by: Brice * adding final pose in analytic expansion to check (#4353) * bump iron to 1.2.8 * fix merge conflict resolution --------- Signed-off-by: pepisg Signed-off-by: Wiktor Bajor Signed-off-by: Denis Sokolov Signed-off-by: goes Signed-off-by: Tony Najjar Signed-off-by: Steve Macenski Signed-off-by: Ramon Wijnands Signed-off-by: Krzysztof Pawełczyk Signed-off-by: Guillaume Doisy Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> Signed-off-by: Brice Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Co-authored-by: Sokolov Denis <52282102+perchess@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: goes Co-authored-by: Saitama Co-authored-by: Tony Najjar Co-authored-by: Ramon Wijnands Co-authored-by: AzaelCicero Co-authored-by: Krzysztof Pawełczyk Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: StetroF <120172218+StetroF@users.noreply.github.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> --- nav2_amcl/package.xml | 2 +- nav2_amcl/src/amcl_node.cpp | 12 +- nav2_amcl/src/pf/CMakeLists.txt | 1 + nav2_behavior_tree/package.xml | 2 +- .../decorator/path_longer_on_approach.cpp | 2 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_collision_monitor/src/polygon.cpp | 7 +- .../test/collision_monitor_node_test.cpp | 3 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- .../nav2_costmap_2d/clear_costmap_service.hpp | 1 - .../include/nav2_costmap_2d/static_layer.hpp | 12 + nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/plugins/static_layer.cpp | 35 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 2 - nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 - 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 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- .../lifecycle_manager_client.hpp | 17 - nav2_lifecycle_manager/package.xml | 2 +- .../include/nav2_map_server/map_io.hpp | 11 + nav2_map_server/package.xml | 2 +- nav2_map_server/src/map_io.cpp | 28 +- nav2_map_server/test/unit/test_map_io.cpp | 27 ++ nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/README.md | 14 + nav2_mppi_controller/critics.xml | 4 + .../critics/velocity_deadband_critic.hpp | 54 +++ nav2_mppi_controller/package.xml | 2 +- .../src/critics/cost_critic.cpp | 6 + .../src/critics/velocity_deadband_critic.cpp | 104 +++++ nav2_mppi_controller/test/critics_tests.cpp | 92 +---- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- .../package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- .../src/nav2_rotation_shim_controller.cpp | 5 +- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- nav2_smac_planner/src/collision_checker.cpp | 2 +- nav2_smac_planner/src/node_hybrid.cpp | 3 +- nav2_smac_planner/test/test_nodehybrid.cpp | 15 +- nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- .../include/nav2_util/lifecycle_node.hpp | 1 + .../include/nav2_util/validate_messages.hpp | 179 +++++++++ nav2_util/package.xml | 2 +- nav2_util/src/lifecycle_node.cpp | 31 +- nav2_util/test/CMakeLists.txt | 4 + nav2_util/test/test_validation_messages.cpp | 368 ++++++++++++++++++ nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 69 files changed, 958 insertions(+), 164 deletions(-) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp create mode 100644 nav2_util/include/nav2_util/validate_messages.hpp create mode 100644 nav2_util/test/test_validation_messages.cpp diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 93c45609825..27d1e917c70 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.7 + 1.2.8

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 87b95592234..6e92626fc93 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -49,6 +49,7 @@ #pragma GCC diagnostic pop #include "nav2_amcl/portable_utils.hpp" +#include "nav2_util/validate_messages.hpp" using namespace std::placeholders; using rcl_interfaces::msg::ParameterType; @@ -523,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha RCLCPP_INFO(get_logger(), "initialPoseReceived"); - if (msg->header.frame_id == "") { - // This should be removed at some point - RCLCPP_WARN( - get_logger(), - "Received initial pose with empty frame_id. You should always supply a frame_id."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting."); return; } if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) { @@ -1390,6 +1388,10 @@ void AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { RCLCPP_DEBUG(get_logger(), "AmclNode: A new map was received."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received map message is malformed. Rejecting."); + return; + } if (first_map_only_ && first_map_received_) { return; } diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt index c6f16e6a7dc..3b4b2fa5ca1 100644 --- a/nav2_amcl/src/pf/CMakeLists.txt +++ b/nav2_amcl/src/pf/CMakeLists.txt @@ -15,6 +15,7 @@ target_include_directories(pf_lib PRIVATE ../include) if(HAVE_DRAND48) target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") endif() +target_link_libraries(pf_lib m) install(TARGETS pf_lib diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 53ca2b0dcae..4354d1fc946 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.7 + 1.2.8 TODO Michael Jeronimo Carlos Orduno 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 d4b40b0964e..b8c5bdbb46c 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -36,7 +36,7 @@ bool PathLongerOnApproach::isPathUpdated( { return new_path != old_path && old_path.poses.size() != 0 && new_path.poses.size() != 0 && - old_path.poses.back() == new_path.poses.back(); + old_path.poses.back().pose == new_path.poses.back().pose; } bool PathLongerOnApproach::isRobotInGoalProximity( diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 6ab1b283d21..51b422dbc62 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.7 + 1.2.8 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 208e6882186..d9ac165a605 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.7 + 1.2.8 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index f4a3dbe6554..308a0ff0ad0 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.7 + 1.2.8 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index c0ea7fea457..9c493ffac6a 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.7 + 1.2.8 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index c215cdf2da8..416e3e1cd5c 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -248,7 +248,12 @@ double Polygon::getCollisionTime( Velocity vel = velocity; // Array of points transformed to the frame concerned with pose on each simulation step - std::vector points_transformed; + std::vector points_transformed = collision_points; + + // Check static polygon + if (getPointsInside(points_transformed) >= min_points_) { + return 0.0; + } // Robot movement simulation for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) { diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 2ceb34fff6a..baee9e49031 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -792,7 +792,8 @@ TEST_F(Tester, testProcessApproach) // 3. Obstacle is inside robot footprint publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); - publishCmdVel(0.5, 0.2, 0.0); + // Publish impossible cmd_vel to ensure robot footprint is checked + publishCmdVel(1000000000.0, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 74a7e69a3f6..19aa6d0bd15 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.7 + 1.2.8 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 c90edc7c828..8c9c87484c4 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.7 + 1.2.8 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 48a7359614d..2d9416db329 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.7 + 1.2.8 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 37e4a6a4c10..535be634c6f 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.7 + 1.2.8 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/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 46d79bd9df5..2d74530b436 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -67,7 +67,6 @@ class ClearCostmapService // Clearing parameters unsigned char reset_value_; - std::vector clearable_layers_; // Server for clearing the costmap rclcpp::Service::SharedPtr clear_except_service_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index dd1a65cef0c..967a6c3789a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -48,6 +48,7 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/footprint.hpp" namespace nav2_costmap_2d { @@ -160,6 +161,17 @@ class StaticLayer : public CostmapLayer rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + std::vector transformed_footprint_; + bool footprint_clearing_enabled_; + /** + * @brief Clear costmap layer info below the robot's footprint + */ + void updateFootprint( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, + double * max_x, + double * max_y); + std::string global_frame_; ///< @brief The global frame for the costmap std::string map_frame_; /// @brief frame that map is located in diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index f13d8c0c5cd..a49554cc440 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.2.7 + 1.2.8 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_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 18cd51164f2..729530d82b7 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -45,6 +45,7 @@ #include "pluginlib/class_list_macros.hpp" #include "tf2/convert.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_util/validate_messages.hpp" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) @@ -132,6 +133,7 @@ StaticLayer::getParameters() declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); declareParameter("map_topic", rclcpp::ParameterValue("")); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false)); auto node = node_.lock(); if (!node) { @@ -140,6 +142,7 @@ StaticLayer::getParameters() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); std::string private_map_topic, global_map_topic; node->get_parameter(name_ + "." + "map_topic", private_map_topic); node->get_parameter("map_topic", global_map_topic); @@ -277,6 +280,10 @@ StaticLayer::interpretValue(unsigned char value) void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { + if (!nav2_util::validateMsg(*new_map)) { + RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting."); + return; + } if (!map_received_) { processMap(*new_map); map_received_ = true; @@ -328,7 +335,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u void StaticLayer::updateBounds( - double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x, + double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) @@ -366,6 +373,24 @@ StaticLayer::updateBounds( *max_y = std::max(wy, *max_y); has_updated_data_ = false; + + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void +StaticLayer::updateFootprint( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, + double * max_y) +{ + if (!footprint_clearing_enabled_) {return;} + + transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + + for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { + touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); + } } void @@ -387,6 +412,10 @@ StaticLayer::updateCosts( return; } + if (footprint_clearing_enabled_) { + setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE); + } + if (!layered_costmap_->isRolling()) { // if not rolling, the layered costmap (master_grid) has same coordinates as this layer if (!use_maximum_) { @@ -469,6 +498,10 @@ StaticLayer::dynamicParametersCallback( has_updated_data_ = true; current_ = false; } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } } } result.successful = true; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 18039f4298f..857278b8416 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -40,8 +40,6 @@ ClearCostmapService::ClearCostmapService( logger_ = node->get_logger(); reset_value_ = costmap_.getCostmap()->getDefaultValue(); - node->get_parameter("clearable_layers", clearable_layers_); - clear_except_service_ = node->create_service( "clear_except_" + costmap_.getName(), std::bind( diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 8bcd39683c6..8934ee44222 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -108,8 +108,6 @@ void Costmap2DROS::init() { RCLCPP_INFO(get_logger(), "Creating Costmap"); - std::vector clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"}; - declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false)); declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]"))); @@ -134,7 +132,6 @@ void Costmap2DROS::init() declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); declare_parameter("use_maximum", rclcpp::ParameterValue(false)); - declare_parameter("clearable_layers", rclcpp::ParameterValue(clearable_layers)); } Costmap2DROS::~Costmap2DROS() diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index b03c8096aa9..07081954314 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.2.7 + 1.2.8 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 279f4264f4a..0c1d9c2257e 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.2.7 + 1.2.8 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index f3b7abf4811..ad4b5f0fec8 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.2.7 + 1.2.8 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 0a596b77be6..9ada111110d 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.2.7 + 1.2.8 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 3526def3f22..b0b9eb77b1f 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.2.7 + 1.2.8 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 8b5c02d4f79..b7005a361a2 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.2.7 + 1.2.8 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 1ed2f94a325..15c5520368f 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.2.7 + 1.2.8 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 0b9425c70bb..ff94df6cb32 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.2.7 + 1.2.8 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index c5f2be8bb62..076e848902c 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -84,23 +84,6 @@ class LifecycleManagerClient */ SystemStatus is_active(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - // A couple convenience methods to facilitate scripting tests - /** - * @brief Set initial pose with covariance - * @param x X position - * @param y Y position - * @param theta orientation - */ - void set_initial_pose(double x, double y, double theta); - /** - * @brief Send goal pose to NavigationToPose action server - * @param x X position - * @param y Y position - * @param theta orientation - * @return true or false - */ - bool navigate_to_pose(double x, double y, double theta); - protected: using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes; diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 93062f985ca..e222d526e84 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.2.7 + 1.2.8 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/include/nav2_map_server/map_io.hpp b/nav2_map_server/include/nav2_map_server/map_io.hpp index bee7df73132..97dc81821c9 100644 --- a/nav2_map_server/include/nav2_map_server/map_io.hpp +++ b/nav2_map_server/include/nav2_map_server/map_io.hpp @@ -98,6 +98,17 @@ bool saveMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters); +/** + * @brief Expand ~/ to home user dir. + * @param yaml_filename Name of input YAML file. + * @param home_dir Expanded `~/`home dir or empty string if HOME not set + * + * @return Expanded string or input string if `~/` not expanded + */ +std::string expand_user_home_dir_if_needed( + std::string yaml_filename, + std::string home_dir); + } // namespace nav2_map_server #endif // NAV2_MAP_SERVER__MAP_IO_HPP_ diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 24f5315d0c6..98e08c8b80f 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.7 + 1.2.8 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 4803b6d3f64..85428490edb 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include "Magick++.h" #include "nav2_util/geometry_utils.hpp" @@ -115,9 +116,33 @@ T yaml_get_value(const YAML::Node & node, const std::string & key) } } +std::string get_home_dir() +{ + if (const char * home_dir = std::getenv("HOME")) { + return std::string{home_dir}; + } + return std::string{}; +} + +std::string expand_user_home_dir_if_needed( + std::string yaml_filename, + std::string home_variable_value) +{ + if (yaml_filename.size() < 2 || !(yaml_filename[0] == '~' && yaml_filename[1] == '/')) { + 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"; + return yaml_filename; + } + const std::string prefix{home_variable_value}; + return yaml_filename.replace(0, 1, prefix); +} + LoadParameters loadMapYaml(const std::string & yaml_filename) { - YAML::Node doc = YAML::LoadFile(yaml_filename); + YAML::Node doc = YAML::LoadFile(expand_user_home_dir_if_needed(yaml_filename, get_home_dir())); LoadParameters load_parameters; auto image_file_name = yaml_get_value(doc, "image"); @@ -295,7 +320,6 @@ LOAD_MAP_STATUS loadMapFromYaml( " for reason: " << e.what() << std::endl; return INVALID_MAP_METADATA; } - try { loadMapFromFile(load_parameters, map); } catch (std::exception & e) { diff --git a/nav2_map_server/test/unit/test_map_io.cpp b/nav2_map_server/test/unit/test_map_io.cpp index 419ce669ef5..aed2a2dde38 100644 --- a/nav2_map_server/test/unit/test_map_io.cpp +++ b/nav2_map_server/test/unit/test_map_io.cpp @@ -311,3 +311,30 @@ TEST_F(MapIOTester, loadInvalidYAML) LoadParameters loadParameters; ASSERT_ANY_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path("invalid_file.yaml"))); } + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenShorterThanTwo) +{ + const std::string emptyFileName{}; + ASSERT_EQ(emptyFileName, expand_user_home_dir_if_needed(emptyFileName, "/home/user")); +} + +TEST( + HomeUserExpanderTestSuite, + homeUserExpanderShouldNotChangeInputStringWhenInputStringDoesNotStartWithHomeSequence) +{ + const std::string fileName{"valid_file.yaml"}; + ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "/home/user")); +} + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenHomeVariableNotFound) +{ + const std::string fileName{"~/valid_file.yaml"}; + ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "")); +} + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldExpandHomeSequenceWhenHomeVariableSet) +{ + const std::string fileName{"~/valid_file.yaml"}; + const std::string expectedOutputFileName{"/home/user/valid_file.yaml"}; + ASSERT_EQ(expectedOutputFileName, expand_user_home_dir_if_needed(fileName, "/home/user")); +} diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index d2bb6d9c9da..3ca6735e40b 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -89,6 +89,7 @@ add_library(mppi_critics SHARED src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp src/critics/constraint_critic.cpp + src/critics/velocity_deadband_critic.cpp ) set(libraries mppi_controller mppi_critics) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 740dcae1b6a..f8a90f1cbc0 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -172,6 +172,15 @@ Note: There is a "Legacy" version of this critic also available with the same pa | cost_weight | double | Default 10.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | + +#### Velocity Deadband Critic + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 35.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | deadband_velocities | double[] | Default [0.0, 0.0, 0.0]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action. | + + ### XML configuration example ``` controller_server: @@ -262,6 +271,11 @@ controller_server: threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 forward_preference: true + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] # TwirlingCritic: # enabled: true # twirling_cost_power: 1 diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index d578d23a9ec..6085dbec885 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -45,5 +45,9 @@ mppi critic for incentivizing moving within kinematic and dynamic bounds + + mppi critic for restricting command velocities in deadband range + + diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp new file mode 100644 index 00000000000..76e1dbd670f --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp @@ -0,0 +1,54 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ + +#include + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::VelocityDeadbandCritic + * @brief Critic objective function for enforcing feasible constraints + */ +class VelocityDeadbandCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to goal following + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + std::vector deadband_velocities_{0.0f, 0.0f, 0.0f}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index dd6142c52fa..ff0683cd403 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.2.7 + 1.2.8 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index b91ab075b6a..a9dd9e9e814 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -33,6 +33,12 @@ void CostCritic::initialize() // Normalized by cost value to put in same regime as other weights weight_ /= 254.0f; + // Normalize weight when parameter is changed dynamically as well + auto weightDynamicCb = [&](const rclcpp::Parameter & weight) { + weight_ = weight.as_double() / 254.0f; + }; + parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb); + collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp new file mode 100644 index 00000000000..84d3aba303b --- /dev/null +++ b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "nav2_mppi_controller/critics/velocity_deadband_critic.hpp" + +namespace mppi::critics +{ + +void VelocityDeadbandCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 35.0); + + // Recast double to float + std::vector deadband_velocities{0.0, 0.0, 0.0}; + getParam(deadband_velocities, "deadband_velocities", std::vector{0.0, 0.0, 0.0}); + std::transform( + deadband_velocities.begin(), deadband_velocities.end(), deadband_velocities_.begin(), + [](double d) {return static_cast(d);}); + + RCLCPP_INFO_STREAM( + logger_, "VelocityDeadbandCritic instantiated with " + << power_ << " power, " << weight_ << " weight, deadband_velocity [" + << deadband_velocities_.at(0) << "," << deadband_velocities_.at(1) << "," + << deadband_velocities_.at(2) << "]"); +} + +void VelocityDeadbandCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + + if (!enabled_) { + return; + } + + auto & vx = data.state.vx; + auto & wz = data.state.wz; + + if (data.motion_model->isHolonomic()) { + auto & vy = data.state.vy; + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; + } + + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS(mppi::critics::VelocityDeadbandCritic, mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index af2a61e20e7..eb2a79fab7d 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -31,6 +31,7 @@ #include "nav2_mppi_controller/critics/path_follow_critic.hpp" #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" #include "nav2_mppi_controller/critics/twirling_critic.hpp" +#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp" #include "utils_test.cpp" // NOLINT // Tests the various critic plugin functions @@ -609,106 +610,51 @@ TEST(CriticTests, PathAlignCritic) EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } -TEST(CriticTests, PathAlignLegacyCritic) +TEST(CriticTests, VelocityDeadbandCritic) { // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", "dummy_costmap", true); ParametersHandler param_handler(node); + auto getParam = param_handler.getParamGetter("critic"); + std::vector deadband_velocities_; + getParam(deadband_velocities_, "deadband_velocities", std::vector{0.08, 0.08, 0.08}); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; - generated_trajectories.reset(1000, 30); models::Path path; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; - data.motion_model = std::make_shared(); - TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally - data.goal_checker = &goal_checker; + data.motion_model = std::make_shared(); // Initialization testing - // Make sure initializes correctly - PathAlignLegacyCritic critic; + // Make sure initializes correctly and that defaults are reasonable + VelocityDeadbandCritic critic; critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); EXPECT_EQ(critic.getName(), "critic"); // Scoring testing - // provide state poses and path close within positional tolerances - state.pose.pose.position.x = 1.0; - path.reset(10); - path.x(9) = 0.85; - critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); - - // provide state pose and path far enough to enable - // but data furthest point reached is 0 and offset default is 20, so returns - path.x(9) = 0.15; - critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); - - // provide state pose and path far enough to enable, with data to pass condition - // but with empty trajectories and paths, should still be zero - *data.furthest_reached_path_point = 21; - path.x(9) = 0.15; + // provide velocities out of deadband bounds, should not have any costs + state.vx = 0.80 * xt::ones({1000, 30}); + state.vy = 0.60 * xt::ones({1000, 30}); + state.wz = 0.80 * xt::ones({1000, 30}); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); - - // provide state pose and path far enough to enable, with data to pass condition - // and with a valid path to pass invalid path condition - state.pose.pose.position.x = 0.0; - data.path_pts_valid.reset(); // Recompute on new path - path.reset(22); - path.x(0) = 0; - path.x(1) = 0.1; - path.x(2) = 0.2; - path.x(3) = 0.3; - path.x(4) = 0.4; - path.x(5) = 0.5; - path.x(6) = 0.6; - path.x(7) = 0.7; - path.x(8) = 0.8; - path.x(9) = 0.9; - path.x(10) = 0.9; - path.x(11) = 0.9; - path.x(12) = 0.9; - path.x(13) = 0.9; - path.x(14) = 0.9; - path.x(15) = 0.9; - path.x(16) = 0.9; - path.x(17) = 0.9; - path.x(18) = 0.9; - path.x(19) = 0.9; - path.x(20) = 0.9; - path.x(21) = 0.9; - generated_trajectories.x = 0.66 * xt::ones({1000, 30}); - critic.score(data); - // 0.04 * 1000 * 10 weight * 6 num pts eval / 6 normalization term - EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2); - - // provide state pose and path far enough to enable, with data to pass condition - // but path is blocked in collision - auto * costmap = costmap_ros->getCostmap(); - // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution - for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m - for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m - costmap->setCost(i, j, 254); - } - } + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); - data.path_pts_valid.reset(); // Recompute on new path - costs = xt::zeros({1000}); - path.x = 1.5 * xt::ones({22}); - path.y = 1.5 * xt::ones({22}); + // Test cost value + state.vx = 0.01 * xt::ones({1000, 30}); + state.vy = 0.02 * xt::ones({1000, 30}); + state.wz = 0.021 * xt::ones({1000, 30}); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 + EXPECT_NEAR(costs(1), 19.845, 0.01); } diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 3261672ed70..2bfd8b716f7 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.2.7 + 1.2.8 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 97fe29232be..70f129d0adf 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.2.7 + 1.2.8 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 58b8343666a..667dac75c56 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.2.7 + 1.2.8 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 72b5c09665d..047e5dc9024 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.2.7 + 1.2.8 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 97989ee9834..cd9a5400843 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.2.7 + 1.2.8 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 afd32381459..5097a1fc9cd 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -195,10 +195,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - throw nav2_core::ControllerException( - std::string( - "Unable to find a sampling point at least %0.2f from the robot," - "passing off to primary controller plugin.", forward_sampling_distance_)); + return current_path_.poses.back(); } geometry_msgs::msg::Pose diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 659058f84f4..5512f763dde 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.2.7 + 1.2.8 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index d6557a2c7a6..e6f908ce3d1 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.2.7 + 1.2.8 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index a5406bc37b3..e8c3d43625a 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.2.7 + 1.2.8 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 831bccbd9e0..d0e068c634b 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -161,7 +161,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionmotion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); reals = s.reals(); // Make sure in range [0, 2PI) diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 549570fb262..59e7f6760bb 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -97,7 +97,7 @@ bool GridCollisionChecker::inCollision( if (outsideRange(costmap_->getSizeInCellsX(), x) || outsideRange(costmap_->getSizeInCellsY(), y)) { - return false; + return true; } // Assumes setFootprint already set diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 0e86e7c5706..304578de3df 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -250,7 +250,8 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(static_cast(theta) / bin_size)); + return static_cast(floor(theta / static_cast(bin_size))) % + num_angle_quantization; } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 5580e64d04e..0050b0a0abd 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -13,6 +13,7 @@ // limitations under the License. Reserved. #include +#include #include #include #include @@ -312,6 +313,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = 3.1415926; + motion_table.num_angle_quantization = 2; double test_theta = 3.1415926; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); @@ -320,17 +322,28 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; double test_theta = M_PI; - unsigned int expected_angular_bin = 1; + unsigned int expected_angular_bin = 0; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; float test_theta = M_PI; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } + + { + motion_table.bin_size = 0.0872664675; + motion_table.num_angle_quantization = 72; + double test_theta = 6.28318526567925; + unsigned int expected_angular_bin = 71; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } } diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index d55321b3f34..1fea5a11bc6 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.2.7 + 1.2.8 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index bbdef57a8fb..105446946ae 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.2.7 + 1.2.8 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index b58cd83095f..c945c59494f 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.2.7 + 1.2.8 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 6da348c5f1a..7f1ef071570 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -205,6 +205,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode // Connection to tell that server is still up std::unique_ptr bond_{nullptr}; + double bond_heartbeat_period; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/validate_messages.hpp b/nav2_util/include/nav2_util/validate_messages.hpp new file mode 100644 index 00000000000..58594f1ecd5 --- /dev/null +++ b/nav2_util/include/nav2_util/validate_messages.hpp @@ -0,0 +1,179 @@ +// Copyright (c) 2024 GoesM +// +// 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 NAV2_UTIL__VALIDATE_MESSAGES_HPP_ +#define NAV2_UTIL__VALIDATE_MESSAGES_HPP_ + +#include +#include + +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + + +// @brief Validation Check +// Check recieved message is safe or not for the nav2-system +// For each msg-type known in nav2, we could check it as following: +// if(!validateMsg()) RCLCPP_ERROR(,"malformed msg. Rejecting.") +// +// Workflow of validateMsg(): +// if here's a sub-msg-type in the recieved msg, +// the content of sub-msg would be checked as sub-msg-type +// then, check the whole recieved msg. +// +// Following conditions are involved in check: +// 1> Value Check: to avoid damaged value like like `nan`, `INF`, empty string and so on +// 2> Logic Check: to avoid value with bad logic, +// like the size of `map` should be equal to `height*width` +// 3> Any other needed condition could be joint here in future + +namespace nav2_util +{ + + +bool validateMsg(const double & num) +{ + /* @brief double/float value check + * if here'a need to check message validation + * it should be avoid to use double value like `nan`, `inf` + * otherwise, we regard it as an invalid message + */ + if (std::isinf(num)) {return false;} + if (std::isnan(num)) {return false;} + return true; +} + +template +bool validateMsg(const std::array & msg) +{ + /* @brief value check for double-array + * like the field `covariance` used in the msg-type: + * geometry_msgs::msg::PoseWithCovarianceStamped + */ + for (const auto & element : msg) { + if (!validateMsg(element)) {return false;} + } + + return true; +} + +const int NSEC_PER_SEC = 1e9; // 1 second = 1e9 nanosecond +bool validateMsg(const builtin_interfaces::msg::Time & msg) +{ + if (msg.nanosec >= NSEC_PER_SEC) { + return false; // invalid nanosec-stamp + } + return true; +} + +bool validateMsg(const std_msgs::msg::Header & msg) +{ + // check sub-type + if (!validateMsg(msg.stamp)) {return false;} + + /* @brief frame_id check + * if here'a need to check message validation + * it should at least have a non-empty frame_id + * otherwise, we regard it as an invalid message + */ + if (msg.frame_id.empty()) {return false;} + return true; +} + +bool validateMsg(const geometry_msgs::msg::Point & msg) +{ + // check sub-type + if (!validateMsg(msg.x)) {return false;} + if (!validateMsg(msg.y)) {return false;} + if (!validateMsg(msg.z)) {return false;} + return true; +} + +const double epsilon = 1e-4; +bool validateMsg(const geometry_msgs::msg::Quaternion & msg) +{ + // check sub-type + if (!validateMsg(msg.x)) {return false;} + if (!validateMsg(msg.y)) {return false;} + if (!validateMsg(msg.z)) {return false;} + if (!validateMsg(msg.w)) {return false;} + + if (abs(msg.x * msg.x + msg.y * msg.y + msg.z * msg.z + msg.w * msg.w - 1.0) >= epsilon) { + return false; + } + + return true; +} + +bool validateMsg(const geometry_msgs::msg::Pose & msg) +{ + // check sub-type + if (!validateMsg(msg.position)) {return false;} + if (!validateMsg(msg.orientation)) {return false;} + return true; +} + +bool validateMsg(const geometry_msgs::msg::PoseWithCovariance & msg) +{ + // check sub-type + if (!validateMsg(msg.pose)) {return false;} + if (!validateMsg(msg.covariance)) {return false;} + + return true; +} + +bool validateMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + if (!validateMsg(msg.pose)) {return false;} + return true; +} + + +// Function to verify map meta information +bool validateMsg(const nav_msgs::msg::MapMetaData & msg) +{ + // check sub-type + if (!validateMsg(msg.origin)) {return false;} + if (!validateMsg(msg.resolution)) {return false;} + + // logic check + // 1> we don't need an empty map + if (msg.height == 0 || msg.width == 0) {return false;} + return true; +} + +// for msg-type like map, costmap and others as `OccupancyGrid` +bool validateMsg(const nav_msgs::msg::OccupancyGrid & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + // msg.data : @todo any check for it ? + if (!validateMsg(msg.info)) {return false;} + + // check logic + if (msg.data.size() != msg.info.width * msg.info.height) { + return false; // check map-size + } + return true; +} + + +} // namespace nav2_util + + +#endif // NAV2_UTIL__VALIDATE_MESSAGES_HPP_ diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 298b94eedc5..544360d5ad3 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.7 + 1.2.8 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index e09ae54b5d6..5976d098a86 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -19,6 +19,7 @@ #include #include "lifecycle_msgs/msg/state.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_util { @@ -35,6 +36,10 @@ LifecycleNode::LifecycleNode( rclcpp::Parameter( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); + nav2_util::declare_parameter_if_not_declared( + this, "bond_heartbeat_period", rclcpp::ParameterValue(0.1)); + this->get_parameter("bond_heartbeat_period", bond_heartbeat_period); + printLifecycleNodeNotification(); register_rcl_preshutdown_callback(); @@ -55,16 +60,18 @@ LifecycleNode::~LifecycleNode() void LifecycleNode::createBond() { - RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); + if (bond_heartbeat_period > 0.0) { + RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); - bond_ = std::make_unique( - std::string("bond"), - this->get_name(), - shared_from_this()); + bond_ = std::make_unique( + std::string("bond"), + this->get_name(), + shared_from_this()); - bond_->setHeartbeatPeriod(0.10); - bond_->setHeartbeatTimeout(4.0); - bond_->start(); + bond_->setHeartbeatPeriod(bond_heartbeat_period); + bond_->setHeartbeatTimeout(4.0); + bond_->start(); + } } void LifecycleNode::runCleanups() @@ -110,10 +117,12 @@ void LifecycleNode::register_rcl_preshutdown_callback() void LifecycleNode::destroyBond() { - RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); + if (bond_heartbeat_period > 0.0) { + RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); - if (bond_) { - bond_.reset(); + if (bond_) { + bond_.reset(); + } } } diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 9f1ae99bbcb..ad0a5c0c41c 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,3 +41,7 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) + +ament_add_gtest(test_validation_messages test_validation_messages.cpp) +ament_target_dependencies(test_validation_messages rclcpp_lifecycle) +target_link_libraries(test_validation_messages ${library_name}) diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp new file mode 100644 index 00000000000..a7d8ed89041 --- /dev/null +++ b/nav2_util/test/test_validation_messages.cpp @@ -0,0 +1,368 @@ +// Copyright (c) 2024 GoesM +// +// 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/validate_messages.hpp" + +TEST(ValidateMessagesTest, DoubleValueCheck) { + // Test valid double value + EXPECT_TRUE(nav2_util::validateMsg(3.14)); + // Test invalid double value (infinity) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::infinity())); + // Test invalid double value (NaN) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::quiet_NaN())); +} + +TEST(ValidateMessagesTest, TimeStampCheck) +{ + // Test valid time stamp + builtin_interfaces::msg::Time valid_time_stamp; + valid_time_stamp.sec = 123; + valid_time_stamp.nanosec = 456789; + EXPECT_TRUE(nav2_util::validateMsg(valid_time_stamp)); + // Test invalid time stamp (nanosec out of range) + builtin_interfaces::msg::Time invalid_time_stamp; + invalid_time_stamp.sec = 123; + invalid_time_stamp.nanosec = 1e9; // 1 second = 1e9 nanoseconds + EXPECT_FALSE(nav2_util::validateMsg(invalid_time_stamp)); +} + +TEST(ValidateMessagesTest, HeaderCheck) +{ + // Test valid header with non-empty frame_id + std_msgs::msg::Header valid_header; + valid_header.stamp.sec = 123; + valid_header.stamp.nanosec = 456789; + valid_header.frame_id = "map"; + EXPECT_TRUE(nav2_util::validateMsg(valid_header)); + // Test invalid header with empty frame_id + std_msgs::msg::Header invalid_header; + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 456789; + invalid_header.frame_id = ""; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 1e9; + invalid_header.frame_id = "map"; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); +} + +TEST(ValidateMessagesTest, PointCheck) +{ + // Test valid Point message + geometry_msgs::msg::Point valid_point; + valid_point.x = 1.0; + valid_point.y = 2.0; + valid_point.z = 3.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_point)); + // Test invalid Point message with NaN value + geometry_msgs::msg::Point invalid_point; + invalid_point.x = 1.0; + invalid_point.y = std::numeric_limits::quiet_NaN(); + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = std::numeric_limits::quiet_NaN(); + invalid_point.y = 2.0; + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = 1.0; + invalid_point.y = 2.0; + invalid_point.z = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); +} + +TEST(ValidateMessagesTest, QuaternionCheck) +{ + // Test valid Quaternion message + geometry_msgs::msg::Quaternion valid_quaternion; + valid_quaternion.x = 0.0; + valid_quaternion.y = 0.0; + valid_quaternion.z = 0.0; + valid_quaternion.w = 1.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_quaternion)); + // Test invalid Quaternion message with invalid magnitude + geometry_msgs::msg::Quaternion invalid_quaternion; + invalid_quaternion.x = 0.1; + invalid_quaternion.y = 0.2; + invalid_quaternion.z = 0.3; + invalid_quaternion.w = 0.5; // Invalid magnitude (should be 1.0) + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + + // One NaN value + invalid_quaternion.x = 0.0; + invalid_quaternion.y = std::numeric_limits::quiet_NaN(); + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = std::numeric_limits::quiet_NaN(); + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = std::numeric_limits::quiet_NaN(); + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 1.0; + invalid_quaternion.w = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); +} + +TEST(ValidateMessagesTest, PoseCheck) +{ + // Test valid Pose message + geometry_msgs::msg::Pose valid_pose; + valid_pose.position.x = 1.0; + valid_pose.position.y = 2.0; + valid_pose.position.z = 3.0; + valid_pose.orientation.x = 1.0; + valid_pose.orientation.y = 0.0; + valid_pose.orientation.z = 0.0; + valid_pose.orientation.w = 0.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_pose)); + // Test invalid Pose message with invalid position + geometry_msgs::msg::Pose invalid_pose; + invalid_pose.position.x = 1.0; + invalid_pose.position.y = std::numeric_limits::quiet_NaN(); + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 1.0; + invalid_pose.orientation.y = 0.0; + invalid_pose.orientation.z = 0.0; + invalid_pose.orientation.w = 0.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); + // Test invalid Pose message with invalid orientation + invalid_pose.position.x = 1.0; + invalid_pose.position.y = 2.0; + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 0.1; + invalid_pose.orientation.y = 0.2; + invalid_pose.orientation.z = 0.3; + invalid_pose.orientation.w = 0.4; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); +} + + +TEST(ValidateMessagesTest, MapMetaDataCheck) { + // Test valid MapMetaData message + nav_msgs::msg::MapMetaData valid_map_meta_data; + valid_map_meta_data.resolution = 0.05; + valid_map_meta_data.width = 100; + valid_map_meta_data.height = 100; + geometry_msgs::msg::Pose valid_origin; + valid_origin.position.x = 0.0; + valid_origin.position.y = 0.0; + valid_origin.position.z = 0.0; + valid_origin.orientation.x = 0.0; + valid_origin.orientation.y = 0.0; + valid_origin.orientation.z = 0.0; + valid_origin.orientation.w = 1.0; + valid_map_meta_data.origin = valid_origin; + EXPECT_TRUE(nav2_util::validateMsg(valid_map_meta_data)); + + // Test invalid origin message + nav_msgs::msg::MapMetaData invalid_map_meta_data; + invalid_map_meta_data.resolution = 100.0; + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + geometry_msgs::msg::Pose invalid_origin; + invalid_origin.position.x = 0.0; + invalid_origin.position.y = 0.0; + invalid_origin.position.z = 0.0; + invalid_origin.orientation.x = 0.0; + invalid_origin.orientation.y = 0.0; + invalid_origin.orientation.z = 1.0; + invalid_origin.orientation.w = 1.0; + invalid_map_meta_data.origin = invalid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid resolution message + invalid_map_meta_data.resolution = std::numeric_limits::quiet_NaN(); + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid MapMetaData message with zero width + invalid_map_meta_data.resolution = 0.05; + invalid_map_meta_data.width = 0; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); +} + +TEST(ValidateMessagesTest, OccupancyGridCheck) { + // Test valid OccupancyGrid message + nav_msgs::msg::OccupancyGrid valid_occupancy_grid; + valid_occupancy_grid.header.frame_id = "map"; + valid_occupancy_grid.info.resolution = 0.05; + valid_occupancy_grid.info.width = 100; + valid_occupancy_grid.info.height = 100; + std::vector data(100 * 100, 0); // Initialize with zeros + valid_occupancy_grid.data = data; + EXPECT_TRUE(nav2_util::validateMsg(valid_occupancy_grid)); + + // Test invalid header message with wrong data size + nav_msgs::msg::OccupancyGrid invalid_occupancy_grid; + invalid_occupancy_grid.header.frame_id = ""; // Incorrect id + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid info message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 0; // Incorrect width + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid OccupancyGrid message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + std::vector invalid_data(100 * 99, 0); // Incorrect data size + invalid_occupancy_grid.data = invalid_data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); +} + +TEST(ValidateMessagesTest, PoseWithCovarianceCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovariance validate_msg; + validate_msg.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.covariance[35] = 0.06853891909122467; + + validate_msg.pose.position.x = 0.50010401010515571; + validate_msg.pose.position.y = 1.7468730211257935; + validate_msg.pose.position.z = 0.0; + + validate_msg.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.orientation.y = 0.0; + validate_msg.pose.orientation.z = 0.0; + validate_msg.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovariance invalidate_msg1; + invalidate_msg1.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.covariance[7] = NAN; + invalidate_msg1.covariance[9] = NAN; + invalidate_msg1.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.position.z = 0.0; + + invalidate_msg1.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.orientation.y = 0.0; + invalidate_msg1.pose.orientation.z = 0.0; + invalidate_msg1.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovariance invalidate_msg2; + invalidate_msg2.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.position.x = NAN; + invalidate_msg2.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.position.z = 0.0; + + invalidate_msg2.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.orientation.y = 0.0; + invalidate_msg2.pose.orientation.z = 0.0; + invalidate_msg2.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + +TEST(ValidateMessagesTest, PoseWithCovarianceStampedCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovarianceStamped validate_msg; + validate_msg.header.frame_id = "map"; + validate_msg.header.stamp.sec = 1711029956; + validate_msg.header.stamp.nanosec = 146734875; + + validate_msg.pose.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.pose.covariance[35] = 0.06853891909122467; + + validate_msg.pose.pose.position.x = 0.50010401010515571; + validate_msg.pose.pose.position.y = 1.7468730211257935; + validate_msg.pose.pose.position.z = 0.0; + + validate_msg.pose.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.pose.orientation.y = 0.0; + validate_msg.pose.pose.orientation.z = 0.0; + validate_msg.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg1; + invalidate_msg1.header.frame_id = "map"; + invalidate_msg1.header.stamp.sec = 1711029956; + invalidate_msg1.header.stamp.nanosec = 146734875; + + invalidate_msg1.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.pose.covariance[7] = NAN; + invalidate_msg1.pose.covariance[9] = NAN; + invalidate_msg1.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.pose.position.z = 0.0; + + invalidate_msg1.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.pose.orientation.y = 0.0; + invalidate_msg1.pose.pose.orientation.z = 0.0; + invalidate_msg1.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg2; + invalidate_msg2.header.frame_id = ""; + invalidate_msg2.header.stamp.sec = 1711029956; + invalidate_msg2.header.stamp.nanosec = 146734875; + + invalidate_msg2.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.pose.position.x = 0.50010401010515571; + invalidate_msg2.pose.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.pose.position.z = 0.0; + + invalidate_msg2.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.pose.orientation.y = 0.0; + invalidate_msg2.pose.pose.orientation.z = 0.0; + invalidate_msg2.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + + +// Add more test cases for other validateMsg functions if needed diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index c9a9775135a..9928a77022f 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.2.7 + 1.2.8 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index d981fac4e71..1f602689c97 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.2.7 + 1.2.8 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 c489b9f2a69..d53e79afed1 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.2.7 + 1.2.8 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index b726fb2c272..9a0d7012b36 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.7 + 1.2.8 ROS2 Navigation Stack From ff88c487d21a95e99d56eb6f195da4166cde2758 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 29 May 2024 11:08:50 -0700 Subject: [PATCH 10/19] adding mutex lock around map resizing due to dynamic parameter changes and associated processes (#4373) (#4377) (cherry picked from commit b0abc78ace48d21ca5b8b58afb0482d759733cb0) Co-authored-by: Steve Macenski --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 1 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index f319e52f330..ac3d1d57afc 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -348,6 +348,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::atomic stop_updates_{false}; std::atomic initialized_{false}; std::atomic stopped_{true}; + std::mutex _dynamic_parameter_mutex; std::unique_ptr map_update_thread_; ///< @brief A thread for updating the map rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME}; rclcpp::Duration publish_cycle_{1, 0}; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 8934ee44222..a6f207885a4 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -508,6 +508,9 @@ Costmap2DROS::mapUpdateLoop(double frequency) // Execute after start() will complete plugins activation if (!stopped_) { + // Lock while modifying layered costmap and publishing values + std::scoped_lock lock(_dynamic_parameter_mutex); + // Measure the execution time of the updateMap method timer.start(); updateMap(); @@ -708,6 +711,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter { auto result = rcl_interfaces::msg::SetParametersResult(); bool resize_map = false; + std::lock_guard lock_reinit(_dynamic_parameter_mutex); for (auto parameter : parameters) { const auto & type = parameter.get_type(); @@ -803,6 +807,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter layered_costmap_->resizeMap( (unsigned int)(map_width_meters_ / resolution_), (unsigned int)(map_height_meters_ / resolution_), resolution_, origin_x_, origin_y_); + updateMap(); } result.successful = true; From 3884d39d74487af84e753df5dda5333c22ac729f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 30 May 2024 17:59:28 -0700 Subject: [PATCH 11/19] Offcycle Iron Sync 7: May 30, 2024 (#4383) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add configure and cleanup transitions to lifecycle manager and client (#4371) Signed-off-by: Joni Pöllänen * [RotationShimController] Rotate to goal heading (#4332) When arriving in the goal xy tolerance, the rotation shim controller takes back the control to command the robot to rotate in the goal heading orientation. The initial goal of the rotationShimController was to rotate the robot at the beginning of a navigation towards the paths orientation because some controllers are not good at performing in place rotations. For the same reason, the rotationShimController should be able to rotate the robot towards the goal heading. Signed-off-by: Antoine Gennart * bump to 1.2.9 for release --------- Signed-off-by: Joni Pöllänen Signed-off-by: Antoine Gennart Co-authored-by: Joni Pöllänen Co-authored-by: Saitama --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/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 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- .../lifecycle_manager.hpp | 10 +++ .../lifecycle_manager_client.hpp | 10 +++ nav2_lifecycle_manager/package.xml | 2 +- .../src/lifecycle_manager.cpp | 34 ++++++++++ .../src/lifecycle_manager_client.cpp | 12 ++++ .../test/test_lifecycle_manager.cpp | 2 + nav2_map_server/package.xml | 2 +- nav2_mppi_controller/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_msgs/srv/ManageLifecycleNodes.srv | 2 + nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- .../package.xml | 2 +- nav2_rotation_shim_controller/README.md | 4 ++ .../nav2_rotation_shim_controller.hpp | 8 +++ .../tools/utils.hpp | 60 +++++++++++++++++ nav2_rotation_shim_controller/package.xml | 2 +- .../src/nav2_rotation_shim_controller.cpp | 55 ++++++++++++++++ .../test/test_shim_controller.cpp | 65 ++++++++++++++++++- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 49 files changed, 299 insertions(+), 39 deletions(-) create mode 100644 nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 27d1e917c70..3effde7d189 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.8 + 1.2.9

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 4354d1fc946..48c8bd24176 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.8 + 1.2.9 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 51b422dbc62..9f2b9ad3ff4 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.8 + 1.2.9 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index d9ac165a605..5bf57af203f 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.8 + 1.2.9 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 308a0ff0ad0..4e708a9e396 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.8 + 1.2.9 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 9c493ffac6a..f59f2f77381 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.8 + 1.2.9 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 19aa6d0bd15..615d8ce78ac 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.8 + 1.2.9 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 8c9c87484c4..172572b73ad 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.8 + 1.2.9 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 2d9416db329..4ebb396c7d4 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.8 + 1.2.9 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 535be634c6f..2e6a02f6483 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.8 + 1.2.9 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index a49554cc440..fd720fd24fd 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.2.8 + 1.2.9 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_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 07081954314..83c0619c522 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.2.8 + 1.2.9 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 0c1d9c2257e..61e4679cdfb 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.2.8 + 1.2.9 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index ad4b5f0fec8..491ee4ecb76 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.2.8 + 1.2.9 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 9ada111110d..a987b73763c 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.2.8 + 1.2.9 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index b0b9eb77b1f..44497477362 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.2.8 + 1.2.9 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index b7005a361a2..f0e5f2832bf 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.2.8 + 1.2.9 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 15c5520368f..5e0a9e990c0 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.2.8 + 1.2.9 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 ff94df6cb32..305ff7b75e1 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.2.8 + 1.2.9 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index 8d302907c4e..0728df7d071 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -104,6 +104,16 @@ class LifecycleManager : public rclcpp::Node * @return true or false */ bool startup(); + /** + * @brief Configures the managed nodes. + * @return true or false + */ + bool configure(); + /** + * @brief Cleanups the managed nodes + * @return true or false + */ + bool cleanup(); /** * @brief Deactivate, clean up and shut down all the managed nodes. * @return true or false diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index 076e848902c..80e9305e9e0 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -78,6 +78,16 @@ class LifecycleManagerClient * @return true or false */ bool reset(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /** + * @brief Make configure service call + * @return true or false + */ + bool configure(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /** + * @brief Make cleanup service call + * @return true or false + */ + bool cleanup(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Check if lifecycle node manager server is active * @return ACTIVE or INACTIVE or TIMEOUT diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index e222d526e84..85258fe2306 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.2.8 + 1.2.9 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index cea3a182602..e03dfd4701f 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -125,6 +125,12 @@ LifecycleManager::managerCallback( case ManageLifecycleNodes::Request::STARTUP: response->success = startup(); break; + case ManageLifecycleNodes::Request::CONFIGURE: + response->success = configure(); + break; + case ManageLifecycleNodes::Request::CLEANUP: + response->success = cleanup(); + break; case ManageLifecycleNodes::Request::RESET: response->success = reset(); break; @@ -319,6 +325,34 @@ LifecycleManager::startup() return true; } +bool +LifecycleManager::configure() +{ + message("Configuring managed nodes..."); + if (!changeStateForAllNodes(Transition::TRANSITION_CONFIGURE)) { + RCLCPP_ERROR(get_logger(), "Failed to configure all requested nodes. Aborting bringup."); + managed_nodes_state_ = NodeState::UNKNOWN; + return false; + } + message("Managed nodes are now configured"); + managed_nodes_state_ = NodeState::INACTIVE; + return true; +} + +bool +LifecycleManager::cleanup() +{ + message("Cleaning up managed nodes..."); + if (!changeStateForAllNodes(Transition::TRANSITION_CLEANUP)) { + RCLCPP_ERROR(get_logger(), "Failed to cleanup all requested nodes. Aborting cleanup."); + managed_nodes_state_ = NodeState::UNKNOWN; + return false; + } + message("Managed nodes have been cleaned up"); + managed_nodes_state_ = NodeState::UNCONFIGURED; + return true; +} + bool LifecycleManager::shutdown() { diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 632eb1aa8e6..123c69b58eb 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -73,6 +73,18 @@ LifecycleManagerClient::reset(const std::chrono::nanoseconds timeout) return callService(ManageLifecycleNodes::Request::RESET, timeout); } +bool +LifecycleManagerClient::configure(const std::chrono::nanoseconds timeout) +{ + return callService(ManageLifecycleNodes::Request::CONFIGURE, timeout); +} + +bool +LifecycleManagerClient::cleanup(const std::chrono::nanoseconds timeout) +{ + return callService(ManageLifecycleNodes::Request::CLEANUP, timeout); +} + SystemStatus LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) { diff --git a/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp b/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp index 96114f98a57..c2c0cb75f0b 100644 --- a/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp @@ -100,6 +100,8 @@ TEST(LifecycleClientTest, BasicTest) client.is_active(std::chrono::nanoseconds(1000000000))); EXPECT_TRUE(client.resume()); EXPECT_TRUE(client.reset()); + EXPECT_TRUE(client.configure()); + EXPECT_TRUE(client.cleanup()); EXPECT_TRUE(client.shutdown()); } diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 98e08c8b80f..a7ba7c62815 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.8 + 1.2.9 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index ff0683cd403..33c9af4085c 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.2.8 + 1.2.9 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 2bfd8b716f7..120a592e71a 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.2.8 + 1.2.9 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_msgs/srv/ManageLifecycleNodes.srv b/nav2_msgs/srv/ManageLifecycleNodes.srv index 8f37cbf819b..bb84f3e231a 100644 --- a/nav2_msgs/srv/ManageLifecycleNodes.srv +++ b/nav2_msgs/srv/ManageLifecycleNodes.srv @@ -3,6 +3,8 @@ uint8 PAUSE = 1 uint8 RESUME = 2 uint8 RESET = 3 uint8 SHUTDOWN = 4 +uint8 CONFIGURE = 5 +uint8 CLEANUP = 6 uint8 command --- diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 70f129d0adf..6fcdf170dbf 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.2.8 + 1.2.9 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 667dac75c56..ed091d614b7 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.2.8 + 1.2.9 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 047e5dc9024..2b2a6ff96b1 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.2.8 + 1.2.9 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 9129e9af619..6802ab7dc42 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -12,6 +12,8 @@ This is useful for situations when working with plugins that are either too spec As such, this controller will check the rough heading difference with respect to the robot and a newly received path. If within a threshold, it will pass the request onto the controller to execute. If it is outside of the threshold, this controller will rotate the robot towards that path heading. Once it is within the tolerance, it will then pass off control-execution from this rotation shim controller onto the primary controller plugin. At this point, the robot is still going to be rotating, allowing the current plugin to take control for a smooth hand off into path tracking. It is recommended to be more generous than strict in the angular threshold to allow for a smoother transition, but should be tuned for a specific application's desired behaviors. +When the `rotate_to_goal_heading` parameter is set to true, this controller is also able to take back control of the robot when reaching the XY goal tolerance of the goal checker. In this case, the robot will rotate towards the goal heading until the goal checker validate the goal and ends the current navigation task. + The Rotation Shim Controller is suitable for: - Robots that can rotate in place, such as differential and omnidirectional robots. - Preference to rotate in place rather than 'spiral out' when starting to track a new path that is at a significantly different heading than the robot's current heading. @@ -35,6 +37,7 @@ See its [Configuration Guide Page](https://navigation.ros.org/configuration/pack | `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | | `max_angular_accel` | Maximum angular acceleration for rotation to heading | | `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | +| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading | Example fully-described XML with default parameter values: @@ -66,6 +69,7 @@ controller_server: rotate_to_heading_angular_vel: 1.8 max_angular_accel: 3.2 simulate_ahead_time: 1.0 + rotate_to_goal_heading: false # DWB parameters ... 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 c3812ec1d3c..6d4e8b86b33 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 @@ -115,6 +115,13 @@ class RotationShimController : public nav2_core::Controller */ geometry_msgs::msg::PoseStamped getSampledPathPt(); + /** + * @brief Find the goal point in path + * May throw exception if the path is empty + * @return pt location of the output point + */ + geometry_msgs::msg::PoseStamped getSampledPathGoal(); + /** * @brief Uses TF to find the location of the sampled path point in base frame * @param pt location of the sampled path point @@ -168,6 +175,7 @@ class RotationShimController : public nav2_core::Controller double forward_sampling_distance_, angular_dist_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; + bool rotate_to_goal_heading_; // Dynamic parameters handler std::mutex mutex_; diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp new file mode 100644 index 00000000000..0a4ff4ac163 --- /dev/null +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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 NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_ +#define NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_ + +#include "nav2_core/goal_checker.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace nav2_rotation_shim_controller::utils +{ + +/** +* @brief get the current pose of the robot +* @param goal_checker goal checker to get tolerances +* @param robot robot pose +* @param goal goal pose +* @return bool Whether the robot is in the distance tolerance ignoring rotation and speed +*/ +inline bool withinPositionGoalTolerance( + nav2_core::GoalChecker * goal_checker, + const geometry_msgs::msg::Pose & robot, + const geometry_msgs::msg::Pose & goal) +{ + if (goal_checker) { + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist velocity_tolerance; + goal_checker->getTolerances(pose_tolerance, velocity_tolerance); + + const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x; + + auto dx = robot.position.x - goal.position.x; + auto dy = robot.position.y - goal.position.y; + + auto dist_sq = dx * dx + dy * dy; + + if (dist_sq < pose_tolerance_sq) { + return true; + } + } + + return false; +} + +} // namespace nav2_rotation_shim_controller::utils + +#endif // NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index cd9a5400843..59bc9b0fda9 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.2.8 + 1.2.9 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 5097a1fc9cd..dde73151fda 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -19,6 +19,7 @@ #include #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" +#include "nav2_rotation_shim_controller/tools/utils.hpp" using rcl_interfaces::msg::ParameterType; @@ -60,6 +61,8 @@ void RotationShimController::configure( node, plugin_name_ + ".simulate_ahead_time", rclcpp::ParameterValue(1.0)); nav2_util::declare_parameter_if_not_declared( 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)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_); @@ -73,6 +76,8 @@ void RotationShimController::configure( node->get_parameter("controller_frequency", control_frequency); control_duration_ = 1.0 / control_frequency; + node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_); + try { primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); RCLCPP_INFO( @@ -139,6 +144,41 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker) { + // Rotate to goal heading when in goal xy tolerance + if (rotate_to_goal_heading_) { + std::lock_guard lock_reinit(mutex_); + + try { + geometry_msgs::msg::PoseStamped sampled_pt_goal = getSampledPathGoal(); + + if (!nav2_util::transformPoseInTargetFrame( + sampled_pt_goal, sampled_pt_goal, *tf_, + pose.header.frame_id)) + { + throw nav2_core::ControllerTFError("Failed to transform pose to base frame!"); + } + + if (utils::withinPositionGoalTolerance( + goal_checker, + pose.pose, + sampled_pt_goal.pose)) + { + double pose_yaw = tf2::getYaw(pose.pose.orientation); + double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation); + + double angular_distance_to_heading = angles::shortest_angular_distance(pose_yaw, goal_yaw); + + return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); + } + } catch (const std::runtime_error & e) { + RCLCPP_INFO( + logger_, + "Rotation Shim Controller was unable to find a goal point," + " a rotational collision was detected, or TF failed to transform" + " into base frame! what(): %s", e.what()); + } + } + if (path_updated_) { nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock lock(*(costmap->getMutex())); @@ -198,6 +238,17 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() return current_path_.poses.back(); } +geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() +{ + if (current_path_.poses.empty()) { + throw nav2_core::InvalidPath("Path is empty - cannot find a goal point"); + } + + auto goal = current_path_.poses.back(); + goal.header.stamp = clock_->now(); + return goal; +} + geometry_msgs::msg::Pose RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt) { @@ -302,6 +353,10 @@ RotationShimController::dynamicParametersCallback(std::vector } else if (name == plugin_name_ + ".simulate_ahead_time") { simulate_ahead_time_ = parameter.as_double(); } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".rotate_to_goal_heading") { + rotate_to_goal_heading_ = 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 e1bc30f80ec..307f4385d90 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -309,6 +309,67 @@ TEST(RotationShimControllerTest, computeVelocityTests) EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); } +TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { + 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_goal_heading", + true); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + // Test state update and path setting + nav_msgs::msg::Path path; + path.header.frame_id = "fake_frame"; + path.poses.resize(4); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "base_link"; + geometry_msgs::msg::Twist velocity; + nav2_controller::SimpleGoalChecker checker; + checker.initialize(node, "checker", costmap); + + path.header.frame_id = "base_link"; + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = 0.0; + path.poses[1].pose.position.x = 0.05; + path.poses[1].pose.position.y = 0.05; + path.poses[2].pose.position.x = 0.10; + path.poses[2].pose.position.y = 0.10; + path.poses[3].pose.position.x = 0.20; + path.poses[3].pose.position.y = 0.20; + path.poses[3].header.frame_id = "base_link"; + + // this should make the goal checker to validated the fact that the robot is in range + // of the goal. The rotation shim controller should rotate toward the goal heading + // then it will throw an exception because the costmap is bogus + controller->setPlan(path); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); +} + TEST(RotationShimControllerTest, testDynamicParameter) { auto node = std::make_shared("ShimControllerTest"); @@ -338,7 +399,8 @@ TEST(RotationShimControllerTest, testDynamicParameter) rclcpp::Parameter("test.rotate_to_heading_angular_vel", 7.0), 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.primary_controller", std::string("HI")), + rclcpp::Parameter("test.rotate_to_goal_heading", true)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -349,4 +411,5 @@ TEST(RotationShimControllerTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.rotate_to_heading_angular_vel").as_double(), 7.0); 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); } diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 5512f763dde..7fb66af57ad 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.2.8 + 1.2.9 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index e6f908ce3d1..f6c056fdd4f 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.2.8 + 1.2.9 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index e8c3d43625a..c0b088c03ce 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.2.8 + 1.2.9 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 1fea5a11bc6..93842a507e8 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.2.8 + 1.2.9 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 105446946ae..87d6024c083 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.2.8 + 1.2.9 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index c945c59494f..167441d3945 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.2.8 + 1.2.9 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 544360d5ad3..3e922c515ef 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.8 + 1.2.9 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 9928a77022f..17def7f4b6d 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.2.8 + 1.2.9 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 1f602689c97..0b67699b733 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.2.8 + 1.2.9 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 d53e79afed1..e96d215fcf7 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.2.8 + 1.2.9 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 9a0d7012b36..a4b681828fc 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.8 + 1.2.9 ROS2 Navigation Stack From b4f0795de18375fd19d3cbc816f3e0d220aff3e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 11 Jun 2024 16:20:00 +0200 Subject: [PATCH 12/19] Enable reloading BT xml file with same name (#4209) (#4407) * Let BtActionServer overwrite xml * Make a ROS parameter for it * Rename flag to always reload BT xml file --------- Signed-off-by: Johannes Huemer Signed-off-by: Christoph Froehlich Co-authored-by: Johannes Huemer --- .../include/nav2_behavior_tree/bt_action_server.hpp | 3 +++ .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 8 ++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 4e98fea4908..f35c56a3137 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -250,6 +250,9 @@ class BtActionServer // The timeout value for waiting for a service to response std::chrono::milliseconds wait_for_service_timeout_; + // should the BT be reloaded even if the same xml filename is requested? + bool always_reload_bt_xml_ = false; + // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; 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 fa5c5502249..63b4b7a04e3 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 @@ -64,6 +64,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("action_server_result_timeout")) { node->declare_parameter("action_server_result_timeout", 900.0); } + if (!node->has_parameter("always_reload_bt_xml")) { + node->declare_parameter("always_reload_bt_xml", false); + } if (!node->has_parameter("wait_for_service_timeout")) { node->declare_parameter("wait_for_service_timeout", 1000); } @@ -164,6 +167,7 @@ bool BtActionServer::on_configure() int wait_for_service_timeout; node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); + node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -223,8 +227,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena // Empty filename is default for backward compatibility auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; - // Use previous BT if it is the existing one - if (current_bt_xml_filename_ == filename) { + // Use previous BT if it is the existing one and always reload flag is not set to true + if (!always_reload_bt_xml_ && current_bt_xml_filename_ == filename) { RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); return true; } From 5e8d7985c0f36d46699c2c6ea52da0ea3c50c8c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 24 Jun 2024 19:44:56 +0200 Subject: [PATCH 13/19] bt_service_node and bt_action_node: Don't block BT loop (backport #4214) (#4408) * bt_service_node and bt_action_node: Don't block BT loop (#4214) * Set smaller timeout for service node Signed-off-by: Christoph Froehlich * Fix timeout calculation for service node Signed-off-by: Christoph Froehlich * Add a feasible timeout also for action node Signed-off-by: Christoph Froehlich --------- Signed-off-by: Christoph Froehlich * Increasing test count from timeout handling changes (#4234) Signed-off-by: Steve Macenski --------- Signed-off-by: Christoph Froehlich Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- .../include/nav2_behavior_tree/bt_action_node.hpp | 9 ++++++--- .../include/nav2_behavior_tree/bt_service_node.hpp | 9 ++++++--- .../test/plugins/action/test_bt_action_node.cpp | 8 +++++--- 3 files changed, 17 insertions(+), 9 deletions(-) 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 d34f6097245..b9246f16c0a 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 @@ -56,7 +56,7 @@ class BtActionNode : public BT::ActionNodeBase callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); // Get the required items from the blackboard - bt_loop_duration_ = + auto bt_loop_duration = config().blackboard->template get("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); @@ -64,6 +64,9 @@ class BtActionNode : public BT::ActionNodeBase wait_for_service_timeout_ = config().blackboard->template get("wait_for_service_timeout"); + // timeout should be less than bt_loop_duration to be able to finish the current tick + max_timeout_ = std::chrono::duration_cast(bt_loop_duration * 0.5); + // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); @@ -405,7 +408,7 @@ class BtActionNode : public BT::ActionNodeBase return false; } - auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; + auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining; auto result = callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout); elapsed += timeout; @@ -461,7 +464,7 @@ class BtActionNode : public BT::ActionNodeBase std::chrono::milliseconds server_timeout_; // The timeout value for BT loop execution - std::chrono::milliseconds bt_loop_duration_; + std::chrono::milliseconds max_timeout_; // The timeout value for waiting for a service to response std::chrono::milliseconds 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 4a04a147698..0f7c52bd3bb 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 @@ -57,7 +57,7 @@ class BtServiceNode : public BT::ActionNodeBase callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); // Get the required items from the blackboard - bt_loop_duration_ = + auto bt_loop_duration = config().blackboard->template get("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); @@ -65,6 +65,9 @@ class BtServiceNode : public BT::ActionNodeBase wait_for_service_timeout_ = config().blackboard->template get("wait_for_service_timeout"); + // timeout should be less than bt_loop_duration to be able to finish the current tick + max_timeout_ = std::chrono::duration_cast(bt_loop_duration * 0.5); + // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); service_client_ = node_->create_client( @@ -189,7 +192,7 @@ class BtServiceNode : public BT::ActionNodeBase auto remaining = server_timeout_ - elapsed; if (remaining > std::chrono::milliseconds(0)) { - auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; + auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining; rclcpp::FutureReturnCode rc; rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout); @@ -249,7 +252,7 @@ class BtServiceNode : public BT::ActionNodeBase std::chrono::milliseconds server_timeout_; // The timeout value for BT loop execution - std::chrono::milliseconds bt_loop_duration_; + std::chrono::milliseconds max_timeout_; // The timeout value for waiting for a service to response std::chrono::milliseconds wait_for_service_timeout_; diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index a283dee33e7..86455a24a81 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -316,8 +316,10 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // the BT should have failed EXPECT_EQ(result, BT::NodeStatus::FAILURE); - // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2 - EXPECT_EQ(ticks, 2); + // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should + // be at most 2, but it can be 1 too, because the tickOnce may execute two ticks. + EXPECT_LE(ticks, 3); + EXPECT_GE(ticks, 1); } TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) @@ -362,7 +364,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) EXPECT_EQ(result, BT::NodeStatus::FAILURE); // since the server timeout is 90ms and bt loop duration is 10ms, number of ticks should be 9 - EXPECT_EQ(ticks, 9); + EXPECT_EQ(ticks, 10); // start a new execution cycle with the previous BT to ensure previous state doesn't leak into // the new cycle From e55c319d86d292fc79879028641f133709946b08 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Wed, 26 Jun 2024 01:16:35 +0800 Subject: [PATCH 14/19] fix bug mentioned in #3958 (#3972) (#4487) * bug fixed * add space * Update planner_server.cpp * add space for code style * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add ChildLifecycleNode mode in costmap_2d_ros * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower * fit to NodeOption: is_lifecycle_follower * fit reorder Werror * fix wrong use of is_lifecycle_follower * remove blank line * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * Add files via upload * NodeOption: is_lifecycle_follower_ * NodeOption:is_lifecycle_follower_ * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * change default * add NodeOption for costmap_2d_ros * add node options for costmap2dros as an independent node * code style reformat * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * changes * comment changes * change get_parameter into =false * comment modification * missing line * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * delete last line * change lifecycle_test fit to NodeOption --------- Co-authored-by: GoesM Co-authored-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 14 +++------- .../nav2_costmap_2d/costmap_2d_ros.hpp | 27 ++++++++++++++++++- nav2_costmap_2d/src/costmap_2d_ros.cpp | 5 ++-- nav2_costmap_2d/test/unit/lifecycle_test.cpp | 2 +- nav2_planner/src/planner_server.cpp | 16 ++--------- 5 files changed, 36 insertions(+), 28 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 447d8534819..01c529b923b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -304,11 +304,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); publishZeroVelocity(); vel_publisher_->on_deactivate(); @@ -334,11 +330,9 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) goal_checkers_.clear(); progress_checkers_.clear(); - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + + costmap_ros_->cleanup(); + // Release any allocated resources action_server_.reset(); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index ac3d1d57afc..2389abdc678 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -75,8 +75,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode public: /** * @brief Constructor for the wrapper + * @param options Additional options to control creation of the node. */ - Costmap2DROS(); + Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Constructor for the wrapper, the node will @@ -134,6 +135,28 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief as a child-LifecycleNode : + * Costmap2DROS may be launched by another Lifecycle Node as a composed module + * If composed, its parents will handle the shutdown, which includes this module + */ + void on_rcl_preshutdown() override + { + if (is_lifecycle_follower_) { + // Transitioning handled by parent node + return; + } + + // Else, if this is an independent node, this node needs to handle itself. + RCLCPP_INFO( + get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)", + this->get_name()); + + runCleanups(); + + destroyBond(); + } + /** * @brief Subscribes to sensor topics if necessary and starts costmap * updates, can be called to restart the costmap after calls to either @@ -382,6 +405,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode double transform_tolerance_{0}; ///< The timeout before transform errors double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors + bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node + // Derived parameters bool use_radius_{false}; std::vector unpadded_footprint_; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index a6f207885a4..1294ee0a148 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -61,8 +61,8 @@ namespace nav2_costmap_2d Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) : Costmap2DROS(name, "/", name, use_sim_time) {} -Costmap2DROS::Costmap2DROS() -: nav2_util::LifecycleNode("costmap", ""), +Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("costmap", "", options), name_("costmap"), default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, default_types_{ @@ -71,6 +71,7 @@ Costmap2DROS::Costmap2DROS() "nav2_costmap_2d::InflationLayer"} { declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + is_lifecycle_follower_ = false; init(); } diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp index 8a657d81ad1..a8ab817df3e 100644 --- a/nav2_costmap_2d/test/unit/lifecycle_test.cpp +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -22,7 +22,7 @@ TEST(LifecylceTest, CheckInitialTfTimeout) { rclcpp::init(0, nullptr); - auto costmap = std::make_shared("test_costmap"); + auto costmap = std::make_shared(rclcpp::NodeOptions()); costmap->set_parameter({"initial_transform_timeout", 0.0}); std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}}; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 1278656cf32..82a2e3adb7d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -223,11 +223,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -252,15 +248,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) plan_publisher_.reset(); tf_.reset(); - /* - * Double check whether something else transitioned it to INACTIVE - * already, e.g. the rcl preshutdown callback. - */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + costmap_ros_->cleanup(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { From b840484f0bdfd5329be8314d19aaa99a8592cca6 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sat, 6 Jul 2024 14:11:59 -0700 Subject: [PATCH 15/19] Behavior tree node for extracting pose from path (#4518) (#4524) * add get pose from path action Signed-off-by: MarcM0 * cleanup from PR suggestions Signed-off-by: MarcM0 * Updates for main compatibility Signed-off-by: MarcM0 * Lint and build fix Signed-off-by: MarcM0 * More Lint and warnings Signed-off-by: MarcM0 * More Lint and build Signed-off-by: MarcM0 * remove code left over from older file Signed-off-by: MarcM0 * fix test blackboard var name Signed-off-by: MarcM0 * only populate pose frame if empty Signed-off-by: MarcM0 * lint Signed-off-by: MarcM0 --------- Signed-off-by: MarcM0 (cherry picked from commit 12a9c1d805847709e3b82f8dcfbb43c67b5b2937) Co-authored-by: Marc Morcos <30278842+MarcM0@users.noreply.github.com> --- nav2_behavior_tree/CMakeLists.txt | 3 + .../action/get_pose_from_path_action.hpp | 55 ++++++ .../action/get_pose_from_path_action.cpp | 79 +++++++++ .../test/plugins/action/CMakeLists.txt | 4 + .../action/test_get_pose_from_path_action.cpp | 156 ++++++++++++++++++ 5 files changed, 297 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp create mode 100644 nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index fe539b0965f..ff88cce1ec9 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -183,6 +183,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node) add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp) list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node) +add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp) +list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node) + add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) list(APPEND plugin_libs nav2_pipeline_sequence_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp new file mode 100644 index 00000000000..091360b4795 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2024 Marc Morcos +// +// 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 NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "behaviortree_cpp/action_node.h" +#include "nav_msgs/msg/path.h" + +namespace nav2_behavior_tree +{ + +class GetPoseFromPath : public BT::ActionNodeBase +{ +public: + GetPoseFromPath( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("path", "Path to extract pose from"), + BT::OutputPort("pose", "Stamped Extracted Pose"), + BT::InputPort("index", 0, "Index of pose to extract from. -1 is end of list"), + }; + } + +private: + void halt() override {} + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp new file mode 100644 index 00000000000..cab0311a37a --- /dev/null +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -0,0 +1,79 @@ +// Copyright (c) 2024 Marc Morcos +// +// 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 +#include + +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp" + +namespace nav2_behavior_tree +{ + +GetPoseFromPath::GetPoseFromPath( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf) +{ +} + +inline BT::NodeStatus GetPoseFromPath::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + nav_msgs::msg::Path input_path; + getInput("path", input_path); + + int pose_index; + getInput("index", pose_index); + + if (input_path.poses.empty()) { + return BT::NodeStatus::FAILURE; + } + + // Account for negative indices + if(pose_index < 0) { + pose_index = input_path.poses.size() + pose_index; + } + + // out of bounds index + if(pose_index < 0 || static_cast(pose_index) >= input_path.poses.size()) { + return BT::NodeStatus::FAILURE; + } + + // extract pose + geometry_msgs::msg::PoseStamped output_pose; + output_pose = input_path.poses[pose_index]; + + // populate pose frame from path if necessary + if(output_pose.header.frame_id.empty()) { + output_pose.header.frame_id = input_path.header.frame_id; + } + + + setOutput("pose", output_pose); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GetPoseFromPath"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 4aaf185306b..513a19caec2 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -92,6 +92,10 @@ ament_add_gtest(test_remove_passed_goals_action test_remove_passed_goals_action. target_link_libraries(test_remove_passed_goals_action nav2_remove_passed_goals_action_bt_node) ament_target_dependencies(test_remove_passed_goals_action ${dependencies}) +ament_add_gtest(test_get_pose_from_path_action test_get_pose_from_path_action.cpp) +target_link_libraries(test_get_pose_from_path_action nav2_get_pose_from_path_action_bt_node) +ament_target_dependencies(test_get_pose_from_path_action ${dependencies}) + ament_add_gtest(test_planner_selector_node test_planner_selector_node.cpp) target_link_libraries(test_planner_selector_node nav2_planner_selector_bt_node) ament_target_dependencies(test_planner_selector_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp new file mode 100644 index 00000000000..39f6df2a3c9 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp @@ -0,0 +1,156 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2021 Samsung Research America +// Copyright (c) 2024 Marc Morcos +// +// 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 +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp" +#include "utils/test_behavior_tree_fixture.hpp" + +class GetPoseFromPathTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("get_pose_from_path_action_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "GetPoseFromPath", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr GetPoseFromPathTestFixture::node_ = nullptr; +BT::NodeConfiguration * GetPoseFromPathTestFixture::config_ = nullptr; +std::shared_ptr GetPoseFromPathTestFixture::factory_ = nullptr; +std::shared_ptr GetPoseFromPathTestFixture::tree_ = nullptr; + +TEST_F(GetPoseFromPathTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new path and set it on blackboard + nav_msgs::msg::Path path; + std::vector goals; + goals.resize(2); + goals[0].pose.position.x = 1.0; + goals[1].pose.position.x = 2.0; + path.poses = goals; + path.header.frame_id = "test_frame_1"; + config_->blackboard->set("path", path); + + config_->blackboard->set("index", 0); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // check if returned pose is correct + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(config_->blackboard->get("pose", pose)); + EXPECT_EQ(pose.header.frame_id, "test_frame_1"); + EXPECT_EQ(pose.pose.position.x, 1.0); + + // halt node so another goal can be sent + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // try last element + config_->blackboard->set("index", -1); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // check if returned pose is correct + EXPECT_TRUE(config_->blackboard->get("pose", pose)); + EXPECT_EQ(pose.header.frame_id, "test_frame_1"); + EXPECT_EQ(pose.pose.position.x, 2.0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} From 09f6572636a731e2bfa9589aadcca11ebeea38fd Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 9 Jul 2024 11:27:24 -0700 Subject: [PATCH 16/19] Fixing iron for backport of get pose from path action addition (#4529) --- .../plugins/action/get_pose_from_path_action.hpp | 2 +- nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp | 2 +- .../test/plugins/action/test_get_pose_from_path_action.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp index 091360b4795..4715b0eb226 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" -#include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp_v3/action_node.h" #include "nav_msgs/msg/path.h" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp index cab0311a37a..13b2da31405 100644 --- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -72,7 +72,7 @@ inline BT::NodeStatus GetPoseFromPath::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp_v3/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GetPoseFromPath"); diff --git a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp index 39f6df2a3c9..934dea80b5a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp @@ -23,7 +23,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp_v3/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp" From ddd1b4d49209722cbd4400ea3eadacae0a17e024 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 23 Aug 2024 17:16:48 -0700 Subject: [PATCH 17/19] Iron Sync 8: August 23, 2024 (#4645) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * earlier `executor_thread_.reset()` in nav2_costmap_ros (#4385) * reorder cleanup_queue Signed-off-by: goes * codestyle Signed-off-by: goes * codestyle Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes * [RotationShimController] Fix test for rotate to goal heading (#4289) (#4391) * Fix rotate to goal heading tests Signed-off-by: Antoine Gennart * Update bt_service_node.hpp debug msg (#4398) Provide service_name instead of service_node_name for debugging bt_service_node Signed-off-by: João Britto * reset laser_scan_filter before reinit (#4397) Signed-off-by: goes Co-authored-by: goes * Fix error messages (#4411) Signed-off-by: Christoph Froehlich * Warn if inflation_radius_ < inscribed_radius_ (#4423) * Warn if inflation_radius_ < inscribed_radius_ Signed-off-by: Tony Najjar * convert to error Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * chore: cleanup ros1 leftovers (#4446) Signed-off-by: Rein Appeldoorn * precomputeDistanceHeuristic is now computed once (#4451) Signed-off-by: Vincent Belpois Co-authored-by: SiddharthaUpase * shutdown services in destructor of `ClearCostmapService` (#4495) Signed-off-by: GoesM_server Co-authored-by: GoesM_server * Fix world to map coordinate conversion (#4506) Signed-off-by: HovorunBh * fix(nav2_costmap_2d): make obstacle layer not current on enabled toggle (#4507) Signed-off-by: Kemal Bektas Co-authored-by: Kemal Bektas * min_turning_r_ getting param fix (#4510) * min_turning_r_ getting param fix Signed-off-by: Ivan Radionov * Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp Signed-off-by: Steve Macenski Signed-off-by: Ivan Radionov --------- Signed-off-by: Ivan Radionov Signed-off-by: Steve Macenski Co-authored-by: Ivan Radionov Co-authored-by: Steve Macenski * Return out of map update if frames mismatch. Signed-off-by Joey Yang (#4517) Signed-off-by: Joey Yang * check nullptr in smoothPlan() (#4544) * check nullptr in smoothPlan() Signed-off-by: GoesM * code-style Signed-off-by: GoesM * code-style Signed-off-by: GoesM * simple change Signed-off-by: GoesM --------- Signed-off-by: GoesM Co-authored-by: GoesM * check nullPtr in `computeControl()` (#4548) Signed-off-by: goes Co-authored-by: goes * bumping to 1.2.10 for release * fixing merge conflict iron --------- Signed-off-by: goes Signed-off-by: Antoine Gennart Signed-off-by: João Britto Signed-off-by: Christoph Froehlich Signed-off-by: Tony Najjar Signed-off-by: Rein Appeldoorn Signed-off-by: Vincent Belpois Signed-off-by: GoesM_server Signed-off-by: HovorunBh Signed-off-by: Kemal Bektas Signed-off-by: Ivan Radionov Signed-off-by: Steve Macenski Signed-off-by: Joey Yang Signed-off-by: GoesM Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: goes Co-authored-by: Saitama Co-authored-by: João Britto Co-authored-by: Christoph Fröhlich Co-authored-by: Tony Najjar Co-authored-by: Rein Appeldoorn Co-authored-by: Vincent <46542431+VincidaB@users.noreply.github.com> Co-authored-by: SiddharthaUpase Co-authored-by: Bohdan <72872431+HovorunBh@users.noreply.github.com> Co-authored-by: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com> Co-authored-by: Kemal Bektas Co-authored-by: Ivan Radionov <45877502+JJRedmond@users.noreply.github.com> Co-authored-by: Ivan Radionov Co-authored-by: Joey Yang --- nav2_amcl/package.xml | 2 +- nav2_amcl/src/amcl_node.cpp | 1 + .../nav2_behavior_tree/bt_action_node.hpp | 2 +- .../bt_cancel_action_node.hpp | 4 +- .../nav2_behavior_tree/bt_service_node.hpp | 6 +-- nav2_behavior_tree/package.xml | 2 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_controller/src/controller_server.cpp | 13 ++++-- nav2_core/package.xml | 2 +- nav2_costmap_2d/cfg/Costmap2D.cfg | 23 ---------- nav2_costmap_2d/cfg/GenericPlugin.cfg | 7 --- nav2_costmap_2d/cfg/InflationPlugin.cfg | 12 ------ nav2_costmap_2d/cfg/ObstaclePlugin.cfg | 22 ---------- nav2_costmap_2d/cfg/VoxelPlugin.cfg | 22 ---------- .../nav2_costmap_2d/clear_costmap_service.hpp | 5 +++ nav2_costmap_2d/launch/example.launch | 21 --------- nav2_costmap_2d/launch/example_params.yaml | 43 ------------------- nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/plugins/inflation_layer.cpp | 10 +++++ nav2_costmap_2d/plugins/obstacle_layer.cpp | 5 ++- nav2_costmap_2d/plugins/static_layer.cpp | 1 + nav2_costmap_2d/src/clear_costmap_service.cpp | 8 ++++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 +- 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/cfg/KinematicParams.cfg | 33 -------------- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../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_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- .../nav2_mppi_controller/motion_models.hpp | 4 +- nav2_mppi_controller/package.xml | 2 +- nav2_mppi_controller/src/optimizer.cpp | 2 +- nav2_mppi_controller/test/critics_tests.cpp | 2 +- .../test/motion_model_tests.cpp | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- .../package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- .../test/test_shim_controller.cpp | 22 +++++++--- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- .../include/nav2_smac_planner/a_star.hpp | 1 + .../include/nav2_smac_planner/utils.hpp | 4 +- nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/a_star.cpp | 6 ++- nav2_smac_planner/test/test_utils.cpp | 9 ++++ nav2_smoother/package.xml | 2 +- nav2_smoother/src/nav2_smoother.cpp | 8 +++- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 67 files changed, 128 insertions(+), 249 deletions(-) delete mode 100755 nav2_costmap_2d/cfg/Costmap2D.cfg delete mode 100755 nav2_costmap_2d/cfg/GenericPlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/InflationPlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/ObstaclePlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/VoxelPlugin.cfg delete mode 100644 nav2_costmap_2d/launch/example.launch delete mode 100644 nav2_costmap_2d/launch/example_params.yaml delete mode 100644 nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 3effde7d189..0be0ee66ecc 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.9 + 1.2.10

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 6e92626fc93..11870318ce0 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1367,6 +1367,7 @@ AmclNode::dynamicParametersCallback( lasers_update_.clear(); frame_to_laser_.clear(); laser_scan_connection_.disconnect(); + laser_scan_filter_.reset(); laser_scan_sub_.reset(); initMessageFilters(); 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 b9246f16c0a..dbd6090e49e 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 @@ -100,7 +100,7 @@ class BtActionNode : public BT::ActionNodeBase RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for %f s", + node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs", action_name.c_str(), wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( 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 7831f533bc3..9b9ff390ecf 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 @@ -93,8 +93,8 @@ class BtCancelActionNode : public BT::ActionNodeBase RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", - action_name.c_str()); + node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs", + action_name.c_str(), wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string("Action server ") + action_name + std::string(" not available")); 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 0f7c52bd3bb..0fd2ac0d23c 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 @@ -84,12 +84,12 @@ class BtServiceNode : public BT::ActionNodeBase service_name_.c_str()); if (!service_client_->wait_for_service(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", - service_node_name.c_str()); + node_->get_logger(), "\"%s\" service server not available after waiting for %.2fs", + service_name_.c_str(), wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string( "Service server %s not available", - service_node_name.c_str())); + service_name_.c_str())); } RCLCPP_DEBUG( diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 48c8bd24176..63dbdd48394 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.9 + 1.2.10 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 9f2b9ad3ff4..ba0725850f7 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.9 + 1.2.10 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 5bf57af203f..cba47a2f882 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.9 + 1.2.10 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 4e708a9e396..38ac70767b6 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.9 + 1.2.10 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index f59f2f77381..2ed96f28e67 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.9 + 1.2.10 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 615d8ce78ac..f718c363cd5 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.9 + 1.2.10 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 172572b73ad..a8f210f94e6 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.9 + 1.2.10 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 4ebb396c7d4..677e56c8902 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.9 + 1.2.10 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 01c529b923b..9dd6c118eab 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -436,7 +436,12 @@ void ControllerServer::computeControl() RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { - std::string c_name = action_server_->get_current_goal()->controller_id; + auto goal = action_server_->get_current_goal(); + if (!goal) { + return; // goal would be nullptr if action_server_ is inactivate. + } + + std::string c_name = goal->controller_id; std::string current_controller; if (findControllerId(c_name, current_controller)) { current_controller_ = current_controller; @@ -444,7 +449,7 @@ void ControllerServer::computeControl() throw nav2_core::InvalidController("Failed to find controller name: " + c_name); } - std::string gc_name = action_server_->get_current_goal()->goal_checker_id; + std::string gc_name = goal->goal_checker_id; std::string current_goal_checker; if (findGoalCheckerId(gc_name, current_goal_checker)) { current_goal_checker_ = current_goal_checker; @@ -452,7 +457,7 @@ void ControllerServer::computeControl() throw nav2_core::ControllerException("Failed to find goal checker name: " + gc_name); } - std::string pc_name = action_server_->get_current_goal()->progress_checker_id; + std::string pc_name = goal->progress_checker_id; std::string current_progress_checker; if (findProgressCheckerId(pc_name, current_progress_checker)) { current_progress_checker_ = current_progress_checker; @@ -460,7 +465,7 @@ void ControllerServer::computeControl() throw nav2_core::ControllerException("Failed to find progress checker name: " + pc_name); } - setPlannerPath(action_server_->get_current_goal()->path); + setPlannerPath(goal->path); progress_checkers_[current_progress_checker_]->reset(); last_valid_cmd_time_ = now(); diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 2e6a02f6483..8ae83228a04 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.9 + 1.2.10 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/cfg/Costmap2D.cfg b/nav2_costmap_2d/cfg/Costmap2D.cfg deleted file mode 100755 index 07c4a1628bc..00000000000 --- a/nav2_costmap_2d/cfg/Costmap2D.cfg +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, str_t - -gen = ParameterGenerator() - -gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10) -gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100) -gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to be publish display information.", 0, 0, 100) - -#map params -gen.add("width", int_t, 0, "The width of the map in meters.", 10, 0) -gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0) -gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50) -gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0) -gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0) - -# robot footprint shape -gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]") -gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10) -gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "Costmap2D")) diff --git a/nav2_costmap_2d/cfg/GenericPlugin.cfg b/nav2_costmap_2d/cfg/GenericPlugin.cfg deleted file mode 100755 index 555e2b5415a..00000000000 --- a/nav2_costmap_2d/cfg/GenericPlugin.cfg +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t - -gen = ParameterGenerator() -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "GenericPlugin")) diff --git a/nav2_costmap_2d/cfg/InflationPlugin.cfg b/nav2_costmap_2d/cfg/InflationPlugin.cfg deleted file mode 100755 index 5c11eaf791f..00000000000 --- a/nav2_costmap_2d/cfg/InflationPlugin.cfg +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100) -gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50) -gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "InflationPlugin")) diff --git a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg deleted file mode 100755 index a94e98fbbef..00000000000 --- a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) - - -# gen.add("obstacle_max_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) -# gen.add("obstacle_min_range", double_t, 0, "The default minimum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 0.0, 0, 50) -# gen.add("raytrace_max_range", double_t, 0, "The default maximum range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) -# gen.add("raytrace_min_range", double_t, 0, "The default minimum range in meters from which to raytrace out obstacles from the map using sensor data.", 0.0, 0, 50) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin")) diff --git a/nav2_costmap_2d/cfg/VoxelPlugin.cfg b/nav2_costmap_2d/cfg/VoxelPlugin.cfg deleted file mode 100755 index 977fcb99d4c..00000000000 --- a/nav2_costmap_2d/cfg/VoxelPlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to use this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "Max Obstacle Height", 2.0, 0, 50) -gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0) -gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50) -gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16) -gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16) -gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "VoxelPlugin")) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 2d74530b436..844902dd100 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -48,6 +48,11 @@ class ClearCostmapService */ ClearCostmapService() = delete; + /** + * @brief A destructor + */ + ~ClearCostmapService(); + /** * @brief Clears the region outside of a user-specified area reverting to the static map */ diff --git a/nav2_costmap_2d/launch/example.launch b/nav2_costmap_2d/launch/example.launch deleted file mode 100644 index ac089abfba3..00000000000 --- a/nav2_costmap_2d/launch/example.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/nav2_costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml deleted file mode 100644 index 72bf695fad3..00000000000 --- a/nav2_costmap_2d/launch/example_params.yaml +++ /dev/null @@ -1,43 +0,0 @@ -global_frame: map -robot_base_frame: base_link -update_frequency: 5.0 -publish_frequency: 1.0 - -#set if you want the voxel map published -publish_voxel_map: true - -#set to true if you want to initialize the costmap from a static map -static_map: false - -#begin - COMMENT these lines if you set static_map to true -rolling_window: true -width: 6.0 -height: 6.0 -resolution: 0.025 -#end - COMMENT these lines if you set static_map to true - -#START VOXEL STUFF -map_type: voxel -origin_z: 0.0 -z_resolution: 0.2 -z_voxels: 10 -unknown_threshold: 10 -mark_threshold: 0 -#END VOXEL STUFF - -transform_tolerance: 0.3 -obstacle_max_range: 2.5 -obstacle_min_range: 0.0 -max_obstacle_height: 2.0 -raytrace_max_range: 3.0 -raytrace_min_range: 0.0 - -footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] -#robot_radius: 0.46 -footprint_padding: 0.01 -inflation_radius: 0.55 -cost_scaling_factor: 10.0 -lethal_cost_threshold: 100 -observation_sources: base_scan -base_scan: {data_type: LaserScan, expected_update_rate: 0.4, - observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index fd720fd24fd..3932748e7e4 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.2.9 + 1.2.10 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_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 067877e5484..90068f463f7 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -170,6 +170,16 @@ InflationLayer::onFootprintChanged() computeCaches(); need_reinflation_ = true; + if (inflation_radius_ < inscribed_radius_) { + RCLCPP_ERROR( + logger_, + "The configured inflation radius (%.3f) is smaller than " + "the computed inscribed radius (%.3f) of your footprint, " + "it is highly recommended to set inflation radius to be at " + "least as big as the inscribed radius to avoid collisions", + inflation_radius_, inscribed_radius_); + } + RCLCPP_DEBUG( logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 653c0885cae..4d5f3c717b4 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -304,8 +304,11 @@ ObstacleLayer::dynamicParametersCallback( max_obstacle_height_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { - if (param_name == name_ + "." + "enabled") { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { enabled_ = parameter.as_bool(); + if (enabled_) { + current_ = false; + } } else if (param_name == name_ + "." + "footprint_clearing_enabled") { footprint_clearing_enabled_ = parameter.as_bool(); } diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 729530d82b7..25a10e3bd8f 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -318,6 +318,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u "StaticLayer: Map update ignored. Current map is in frame %s " "but update was in frame %s", map_frame_.c_str(), update->header.frame_id.c_str()); + return; } unsigned int di = 0; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 857278b8416..96b3f1bc5f2 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -59,6 +59,14 @@ ClearCostmapService::ClearCostmapService( std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } +ClearCostmapService::~ClearCostmapService() +{ + // make sure services shutdown. + clear_except_service_.reset(); + clear_around_service_.reset(); + clear_entire_service_.reset(); +} + void ClearCostmapService::clearExceptRegionCallback( const shared_ptr/*request_header*/, const shared_ptr request, diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 1294ee0a148..8d882d6ea77 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -352,6 +352,7 @@ nav2_util::CallbackReturn Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + executor_thread_.reset(); costmap_publisher_.reset(); clear_costmap_service_.reset(); @@ -366,8 +367,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); - - executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 83c0619c522..14b310d0db4 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.2.9 + 1.2.10 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 61e4679cdfb..6fdc74af28a 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.2.9 + 1.2.10 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 491ee4ecb76..5cdae049f1c 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.2.9 + 1.2.10 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 a987b73763c..e5c32f78da2 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.2.9 + 1.2.10 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg b/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg deleted file mode 100644 index 51dab28aa48..00000000000 --- a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t - -gen = ParameterGenerator() - -# velocities -gen.add('min_vel_x', double_t, 0, 'The minimum x velocity for the robot in m/s', 0.0) -gen.add('max_vel_x', double_t, 0, 'The maximum x velocity for the robot in m/s', 0.55) -gen.add('min_vel_y', double_t, 0, 'The minimum y velocity for the robot in m/s', -0.1) -gen.add('max_vel_y', double_t, 0, 'The maximum y velocity for the robot in m/s', 0.1) -gen.add('max_vel_theta', double_t, 0, 'The absolute value of the maximum rotational velocity for the robot in rad/s. ' - 'The minimum rotational velocity is assumed to be -max_vel_theta', 1.0) - -# acceleration -gen.add('acc_lim_x', double_t, 0, 'The acceleration limit of the robot in the x direction in m/s^2', 2.5) -gen.add('acc_lim_y', double_t, 0, 'The acceleration limit of the robot in the y direction in m/s^2', 2.5) -gen.add('acc_lim_theta', double_t, 0, 'The acceleration limit of the robot in the theta direction in rad/s^2', 3.2) - -gen.add('decel_lim_x', double_t, 0, 'The deceleration limit of the robot in the x direction in m/s^2', -2.5) -gen.add('decel_lim_y', double_t, 0, 'The deceleration limit of the robot in the y direction in m/s^2', -2.5) -gen.add('decel_lim_theta', double_t, 0, 'The deceleration limit of the robot in the theta direction in rad/s^2', -3.2) - -gen.add('min_speed_xy', double_t, 0, 'The absolute value of the minimum translational/xy velocity in m/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0. ' - 'Previously called min_trans_vel', 0.1) -gen.add('max_speed_xy', double_t, 0, 'The absolute value of the maximum translational/xy velocity in m/s. ' - 'If the value is negative, then the max speed is hypot(max_vel_x, max_vel_y). ' - 'Previously called max_trans_vel', 0.55) -gen.add('min_speed_theta', double_t, 0, 'The absolute value of the minimum rotational velocity in rad/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0.' - ' Previously called min_rot_vel', 0.4) - -exit(gen.generate('dwb_plugins', 'dwb_plugins', 'KinematicParams')) diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 44497477362..7e4e473cf29 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.2.9 + 1.2.10 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index f0e5f2832bf..381df229bb3 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.2.9 + 1.2.10 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 5e0a9e990c0..824c9ce4e72 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.2.9 + 1.2.10 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 305ff7b75e1..db977f78a9f 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.2.9 + 1.2.10 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 85258fe2306..b41d116ef03 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.2.9 + 1.2.10 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index a7ba7c62815..8700c1d65f5 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.9 + 1.2.10 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 4fe85bfb05a..0c5671ea3e5 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -88,9 +88,9 @@ class AckermannMotionModel : public MotionModel /** * @brief Constructor for mppi::AckermannMotionModel */ - explicit AckermannMotionModel(ParametersHandler * param_handler) + explicit AckermannMotionModel(ParametersHandler * param_handler, const std::string & name) { - auto getParam = param_handler->getParamGetter("AckermannConstraints"); + auto getParam = param_handler->getParamGetter(name + ".AckermannConstraints"); getParam(min_turning_r_, "min_turning_r", 0.2); } diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 33c9af4085c..dd2be79bd22 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.2.9 + 1.2.10 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3c5d2032892..86359395ef9 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -411,7 +411,7 @@ void Optimizer::setMotionModel(const std::string & model) } else if (model == "Omni") { motion_model_ = std::make_shared(); } else if (model == "Ackermann") { - motion_model_ = std::make_shared(parameters_handler_); + motion_model_ = std::make_shared(parameters_handler_, name_); } else { throw nav2_core::ControllerException( std::string( diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index eb2a79fab7d..bfa912dbd76 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -117,7 +117,7 @@ TEST(CriticTests, ConstraintsCritic) // Now with ackermann, all in constraint so no costs to score state.vx = 0.40 * xt::ones({1000, 30}); state.wz = 1.5 * xt::ones({1000, 30}); - data.motion_model = std::make_shared(¶m_handler); + data.motion_model = std::make_shared(¶m_handler, node->get_name()); critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 6085896cfe3..4238fa03fb9 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -136,7 +136,7 @@ TEST(MotionModelTests, AckermannTest) auto node = std::make_shared("my_node"); ParametersHandler param_handler(node); std::unique_ptr model = - std::make_unique(¶m_handler); + std::make_unique(¶m_handler, node->get_name()); // Check that predict properly populates the trajectory velocities with the control velocities state.cvx = 10 * xt::ones({batches, timesteps}); diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 120a592e71a..9ec0c988fcb 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.2.9 + 1.2.10 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 6fcdf170dbf..f1f5085628e 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.2.9 + 1.2.10 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index ed091d614b7..92d596b2538 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.2.9 + 1.2.10 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 2b2a6ff96b1..36644e4da2d 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.2.9 + 1.2.10 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 59bc9b0fda9..47d4b0577ab 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.2.9 + 1.2.10 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 307f4385d90..ce17630e75c 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -343,13 +343,16 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { // Test state update and path setting nav_msgs::msg::Path path; - path.header.frame_id = "fake_frame"; + path.header.frame_id = "base_link"; path.poses.resize(4); geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "base_link"; geometry_msgs::msg::Twist velocity; nav2_controller::SimpleGoalChecker checker; + node->declare_parameter( + "checker.xy_goal_tolerance", + 1.0); checker.initialize(node, "checker", costmap); path.header.frame_id = "base_link"; @@ -359,15 +362,24 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[1].pose.position.y = 0.05; path.poses[2].pose.position.x = 0.10; path.poses[2].pose.position.y = 0.10; + // goal position within checker xy_goal_tolerance path.poses[3].pose.position.x = 0.20; path.poses[3].pose.position.y = 0.20; + // goal heading 45 degrees to the left + path.poses[3].pose.orientation.z = -0.3826834; + path.poses[3].pose.orientation.w = 0.9238795; path.poses[3].header.frame_id = "base_link"; - // this should make the goal checker to validated the fact that the robot is in range - // of the goal. The rotation shim controller should rotate toward the goal heading - // then it will throw an exception because the costmap is bogus controller->setPlan(path); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); + + // goal heading 45 degrees to the right + path.poses[3].pose.orientation.z = 0.3826834; + path.poses[3].pose.orientation.w = 0.9238795; + controller->setPlan(path); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } TEST(RotationShimControllerTest, testDynamicParameter) diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 7fb66af57ad..0300bad3a82 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.2.9 + 1.2.10 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index f6c056fdd4f..685b16a6726 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.2.9 + 1.2.10 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index eca1a65d813..ad39379d29b 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -240,6 +240,7 @@ class AStarAlgorithm int _timing_interval = 5000; bool _traverse_unknown; + bool _is_initialized; int _max_iterations; int _max_on_approach_iterations; double _max_planning_time; diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp index 277edb60834..9d715aeaa68 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -46,9 +46,9 @@ inline geometry_msgs::msg::Pose getWorldCoords( { geometry_msgs::msg::Pose msg; msg.position.x = - static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); + static_cast(costmap->getOriginX()) + (mx - 0.5) * costmap->getResolution(); msg.position.y = - static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); + static_cast(costmap->getOriginY()) + (my - 0.5) * costmap->getResolution(); return msg; } diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index c0b088c03ce..bb9473ad0c5 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.2.9 + 1.2.10 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 00517be8733..aa22c4c2c9e 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -36,6 +36,7 @@ AStarAlgorithm::AStarAlgorithm( const MotionModel & motion_model, const SearchInfo & search_info) : _traverse_unknown(true), + _is_initialized(false), _max_iterations(0), _max_planning_time(0), _x_size(0), @@ -67,7 +68,10 @@ void AStarAlgorithm::initialize( _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; _max_planning_time = max_planning_time; - NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + if(!_is_initialized) { + NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + } + _is_initialized = true; _dim3_size = dim_3_size; _expander = std::make_unique>( _motion_model, _search_info, _traverse_unknown, _dim3_size); diff --git a/nav2_smac_planner/test/test_utils.cpp b/nav2_smac_planner/test/test_utils.cpp index 8a6b2440ef5..7dd105d0b39 100644 --- a/nav2_smac_planner/test/test_utils.cpp +++ b/nav2_smac_planner/test/test_utils.cpp @@ -147,3 +147,12 @@ TEST(create_marker, test_createMarker) EXPECT_EQ(marker2.id, 8u); EXPECT_EQ(marker2.points.size(), 0u); } + +TEST(convert_map_to_world_to_map, test_convert_map_to_world_to_map) +{ + auto costmap = nav2_costmap_2d::Costmap2D(10.0, 10.0, 0.05, 0.0, 0.0, 0); + + float mx = 200.0; + float my = 100.0; + geometry_msgs::msg::Pose pose = getWorldCoords(mx, my, &costmap); +} diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 93842a507e8..057cbb22ed6 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.2.9 + 1.2.10 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 8119c614f9d..8522266250f 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -265,7 +265,12 @@ void SmootherServer::smoothPlan() auto result = std::make_shared(); try { - std::string c_name = action_server_->get_current_goal()->smoother_id; + auto goal = action_server_->get_current_goal(); + if (!goal) { + return; // if action_server_ is inactivate, goal would be a nullptr + } + + std::string c_name = goal->smoother_id; std::string current_smoother; if (findSmootherId(c_name, current_smoother)) { current_smoother_ = current_smoother; @@ -274,7 +279,6 @@ void SmootherServer::smoothPlan() } // Perform smoothing - auto goal = action_server_->get_current_goal(); result->path = goal->path; if (!validate(result->path)) { diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 87d6024c083..c14f0dae283 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.2.9 + 1.2.10 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 167441d3945..bf26f43f506 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.2.9 + 1.2.10 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 3e922c515ef..45be0f96b4c 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.9 + 1.2.10 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 17def7f4b6d..74b70ffe977 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.2.9 + 1.2.10 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 0b67699b733..2969ad347c8 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.2.9 + 1.2.10 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 e96d215fcf7..858c5915c9c 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.2.9 + 1.2.10 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index a4b681828fc..98845fcfb55 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.9 + 1.2.10 ROS2 Navigation Stack From d88d07755a7fedd9b69b110987e7e9c4fa0e1693 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 10 Sep 2024 13:06:58 -0700 Subject: [PATCH 18/19] Fixing #4661: MPPI ackermann reversing taking incorrect sign sometimes (#4664) (#4667) * Fixing #4661: MPPI ackermann reversing taking incorrect sign sometimes Signed-off-by: Steve Macenski * fixing unit test for type implicit cast Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski (cherry picked from commit 7eb47d84012dd9e25988ce5c9c269280e6ee3dd1) Co-authored-by: Steve Macenski --- .../nav2_mppi_controller/motion_models.hpp | 4 +- .../test/motion_model_tests.cpp | 78 +++++++++++++++++++ 2 files changed, 80 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 0c5671ea3e5..7d84ba29df5 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -112,8 +112,8 @@ class AckermannMotionModel : public MotionModel auto & vx = control_sequence.vx; auto & wz = control_sequence.wz; - auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) < min_turning_r_); - view = xt::sign(wz) * vx / min_turning_r_; + auto view = xt::masked_view(wz, (xt::fabs(vx) / xt::fabs(wz)) < min_turning_r_); + view = xt::sign(wz) * xt::fabs(vx) / min_turning_r_; } /** diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 4238fa03fb9..f22424d226a 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -164,6 +164,9 @@ TEST(MotionModelTests, AckermannTest) // VX equal since this doesn't change, the WZ is reduced if breaking the constraint EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } // Now, check the specifics of the minimum curvature constraint EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); @@ -177,3 +180,78 @@ TEST(MotionModelTests, AckermannTest) // Check it cleanly destructs model.reset(); } + +TEST(MotionModelTests, AckermannReversingTest) +{ + models::ControlSequence control_sequence; + models::ControlSequence control_sequence2; + models::State state; + int batches = 1000; + int timesteps = 50; + control_sequence.reset(timesteps); // populates with zeros + control_sequence2.reset(timesteps); // populates with zeros + state.reset(batches, timesteps); // populates with zeros + auto node = std::make_shared("my_node"); + ParametersHandler param_handler(node); + std::unique_ptr model = + std::make_unique(¶m_handler, node->get_name()); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({batches, timesteps}); + + // Manually set state index 0 from initial conditions which would be the speed of the robot + xt::view(state.vx, xt::all(), 0) = 10; + xt::view(state.wz, xt::all(), 0) = 1; + + model->predict(state); + + EXPECT_EQ(state.vx, state.cvx); + EXPECT_EQ(state.vy, xt::zeros({batches, timesteps})); // non-holonomic + EXPECT_EQ(state.wz, state.cwz); + + // Check that application of constraints are non-empty for Ackermann Drive + for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + float idx = static_cast(i); + control_sequence.vx(i) = -idx * idx * idx; // now reversing + control_sequence.wz(i) = idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence = control_sequence; + model->applyConstraints(control_sequence); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); + EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } + + // Repeat with negative rotation direction + for (unsigned int i = 0; i != control_sequence2.vx.shape(0); i++) { + float idx = static_cast(i); + control_sequence2.vx(i) = -idx * idx * idx; // now reversing + control_sequence2.wz(i) = -idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence2 = control_sequence2; + model->applyConstraints(control_sequence2); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence2.vx, control_sequence2.vx); + EXPECT_NE(initial_control_sequence2.wz, control_sequence2.wz); + for (unsigned int i = 1; i != control_sequence2.wz.shape(0); i++) { + EXPECT_LT(control_sequence2.wz(i), 0.0); + } + + // Now, check the specifics of the minimum curvature constraint + EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); + for (unsigned int i = 1; i != control_sequence2.vx.shape(0); i++) { + EXPECT_TRUE(fabs(control_sequence2.vx(i)) / fabs(control_sequence2.wz(i)) >= 0.2); + } + + // Check that Ackermann Drive is properly non-holonomic and parameterized + EXPECT_EQ(model->isHolonomic(), false); + + // Check it cleanly destructs + model.reset(); +} From 022e7e18570d75a969b96ccd1c3d5c5c443b3f12 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 2 Oct 2024 11:28:14 -0700 Subject: [PATCH 19/19] Adding disengagement threshold to rotation shim controller (#4699) (#4701) * adding disengagement threshold to rotation shim controller Signed-off-by: Steve Macenski * change default to 22.5 deg Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski (cherry picked from commit fc7e08611a453889feb112bc4c2160687d320a91) Co-authored-by: Steve Macenski --- .../nav2_rotation_shim_controller.hpp | 4 ++-- .../src/nav2_rotation_shim_controller.cpp | 14 ++++++++++++-- 2 files changed, 14 insertions(+), 4 deletions(-) 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 6d4e8b86b33..cb1b8ad7ed9 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 @@ -172,10 +172,10 @@ class RotationShimController : public nav2_core::Controller nav2_core::Controller::Ptr primary_controller_; bool path_updated_; nav_msgs::msg::Path current_path_; - double forward_sampling_distance_, angular_dist_threshold_; + 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_; + bool rotate_to_goal_heading_, in_rotation_; // Dynamic parameters handler std::mutex mutex_; 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 dde73151fda..f9724bdabe7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -29,7 +29,8 @@ namespace nav2_rotation_shim_controller RotationShimController::RotationShimController() : lp_loader_("nav2_core", "nav2_core::Controller"), primary_controller_(nullptr), - path_updated_(false) + path_updated_(false), + in_rotation_(false) { } @@ -51,6 +52,8 @@ void RotationShimController::configure( double control_frequency; nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".angular_dist_threshold", rclcpp::ParameterValue(0.785)); // 45 deg + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".angular_disengage_threshold", rclcpp::ParameterValue(0.785 / 2.0)); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( @@ -65,6 +68,7 @@ void RotationShimController::configure( node, plugin_name_ + ".rotate_to_goal_heading", 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_); node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_); node->get_parameter( plugin_name_ + ".rotate_to_heading_angular_vel", @@ -106,6 +110,7 @@ void RotationShimController::activate() plugin_name_.c_str()); primary_controller_->activate(); + in_rotation_ = false; auto node = node_.lock(); dyn_params_handler_ = node->add_on_set_parameters_callback( @@ -189,10 +194,14 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands double angular_distance_to_heading = std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x); - if (fabs(angular_distance_to_heading) > angular_dist_threshold_) { + + double angular_thresh = + in_rotation_ ? angular_disengage_threshold_ : angular_dist_threshold_; + if (abs(angular_distance_to_heading) > angular_thresh) { RCLCPP_DEBUG( logger_, "Robot is not within the new path's rough heading, rotating to heading..."); + in_rotation_ = true; return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); } else { RCLCPP_DEBUG( @@ -211,6 +220,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands } // If at this point, use the primary controller to path track + in_rotation_ = false; return primary_controller_->computeVelocityCommands(pose, velocity, goal_checker); }