From 1dea56b2b0ad2abfafffbb1b729d942c73825222 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Fri, 26 Jan 2024 05:51:32 +0800 Subject: [PATCH 01/17] change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM Co-authored-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 65df56dac15..1550e64c8e1 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -328,6 +328,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) nomotion_update_srv_.reset(); 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(); @@ -341,7 +342,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // Transforms tf_broadcaster_.reset(); - tf_listener_.reset(); tf_buffer_.reset(); // PubSub From f7f0fdfaaf0ae7c0315ed48aa43fb352e57c861f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 Jan 2024 16:29:41 -0500 Subject: [PATCH 02/17] Allow path end pose deviation revive (#4065) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski --- nav2_planner/src/planner_server.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 01b2384845f..0e393f8304c 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -408,7 +408,10 @@ 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]; From 3c61381136ca102a6d2c6b98b7dbd6223cda663c Mon Sep 17 00:00:00 2001 From: Kino Date: Sat, 27 Jan 2024 08:50:27 +0800 Subject: [PATCH 03/17] Change costmap_queue to shared library (#4072) Signed-off-by: cybaol --- nav2_dwb_controller/costmap_queue/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ) From 03922d260f8eb18b51258b32ea0218e558a3b185 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Tue, 30 Jan 2024 00:23:15 +0800 Subject: [PATCH 04/17] free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in #4078 (#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM * reformat Signed-off-by: GoesM --------- Signed-off-by: GoesM Co-authored-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 1550e64c8e1..ef5635a4015 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -333,6 +333,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) 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; From 630d5c2a615d0c63b6bb99e805157f1bf3d135ff Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 7 Feb 2024 17:35:37 -0800 Subject: [PATCH 05/17] adding progress checker selector BT node (#4109) --- nav2_behavior_tree/CMakeLists.txt | 3 + .../action/progress_checker_selector_node.hpp | 96 +++++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 6 + .../action/progress_checker_selector_node.cpp | 81 ++++++++ .../test/plugins/action/CMakeLists.txt | 4 + .../test_progress_checker_selector_node.cpp | 187 ++++++++++++++++++ 6 files changed, 377 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 6522072368e..72b5c1b6f23 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -192,6 +192,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/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 ce5979929cb..1a2e3d7be66 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -174,6 +174,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/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; +} From 9aec27e2a1416302456e4c0dfa619db861885700 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 12 Feb 2024 13:27:00 -0800 Subject: [PATCH 06/17] New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Share code Signed-off-by: Brice * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice * fix lint cpp - extra space Signed-off-by: Brice * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski * Fix tests Signed-off-by: Steve Macenski * Update test_a_star.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM Co-authored-by: GoesM Signed-off-by: Brice * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * Adding new Smac paper to readme Signed-off-by: Steve Macenski Signed-off-by: Brice * Update README.md Signed-off-by: Steve Macenski Signed-off-by: Brice * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti * misse change Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti Signed-off-by: Brice * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski Signed-off-by: Brice * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman * Remove stored node and assert Signed-off-by: Ryan Friedman * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman * Add full test coverage with subscriber Signed-off-by: Ryan Friedman * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman * Fix syntax errors Signed-off-by: Ryan Friedman * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman * Remove unused code Signed-off-by: Ryan Friedman * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman * Use pass-by-reference * Instead of std::move(std::unique_ptr) Signed-off-by: Ryan Friedman * Finish twist subscriber tests Signed-off-by: Ryan Friedman * Add other constructor and docs Signed-off-by: Ryan Friedman * Fix linter issues Signed-off-by: Ryan Friedman * Manually fix paren alignment Signed-off-by: Ryan Friedman * Remove GSoC reference Signed-off-by: Ryan Friedman * Document twist bool param in README Signed-off-by: Ryan Friedman * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman * More linter fixes Signed-off-by: Ryan Friedman * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring --------- Signed-off-by: Ryan Friedman Signed-off-by: Steve Macenski Signed-off-by: Rhys Mainwaring Co-authored-by: pedro-fuoco Co-authored-by: Steve Macenski Co-authored-by: Rhys Mainwaring Signed-off-by: Brice * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol Signed-off-by: Brice * fix include of hpp Signed-off-by: Brice Renaudeau * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski * Update cost_critic.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Brice Signed-off-by: Steve Macenski Signed-off-by: GoesM Signed-off-by: Davide Faconti Signed-off-by: gg Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman Signed-off-by: Rhys Mainwaring Signed-off-by: cybaol Signed-off-by: Brice Renaudeau Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM Co-authored-by: Davide Faconti Co-authored-by: Joshua Wallace Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan Co-authored-by: pedro-fuoco Co-authored-by: Rhys Mainwaring Co-authored-by: Kino --- nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/README.md | 26 +++ nav2_mppi_controller/critics.xml | 4 + .../critics/cost_critic.hpp | 98 +++++++++ .../src/critics/cost_critic.cpp | 200 ++++++++++++++++++ nav2_mppi_controller/test/critics_tests.cpp | 1 + .../test/optimizer_smoke_test.cpp | 2 +- 7 files changed, 331 insertions(+), 1 deletion(-) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/cost_critic.cpp 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 164f20fe4cc..7d5c9fb836f 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -99,6 +99,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. | @@ -111,6 +114,20 @@ This process is then repeated a number of times and returns a converged solution | cost_scaling_factor | double | Default 10.0. Exponential decay factor across inflation radius. This should be the same as for your inflation layer (Humble only) | inflation_radius | double | Default 0.55. Radius to inflate costmap around lethal obstacles. This should be the same as for your inflation layer (Humble only) +#### 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 | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -215,6 +232,15 @@ controller_server: collision_cost: 10000.0 collision_margin_distance: 0.1 near_goal_distance: 0.5 + # Option to replace Obstacles and use Cost instead + # CostCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 3.81 + # critical_cost: 300.0 + # consider_footprint: true + # collision_cost: 1000000.0 + # near_goal_distance: 1.0 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/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp new file mode 100644 index 00000000000..137e9433760 --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -0,0 +1,200 @@ +// 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) +{ + 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 + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap, + inflation_layer_name_); + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + } else { + 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 fd05ab4bee6..8b26aad774f 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -23,6 +23,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( From d8df9fb39df30bc7df124b76ac8f95b992861f9b Mon Sep 17 00:00:00 2001 From: Ryan Date: Tue, 13 Feb 2024 15:31:47 -0700 Subject: [PATCH 07/17] Use ament_export_targets for all targets (#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman --- nav2_costmap_2d/CMakeLists.txt | 53 ++++++++++--------- .../test/integration/CMakeLists.txt | 44 +++++---------- .../test/regression/CMakeLists.txt | 6 +-- nav2_costmap_2d/test/unit/CMakeLists.txt | 27 +++++----- 4 files changed, 58 insertions(+), 72 deletions(-) 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/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index ef0b148440c..5d33caa63f6 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -1,67 +1,49 @@ 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_test(test_collision_checker @@ -123,6 +105,6 @@ ament_add_test(range_tests # ${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 78fbfa6cf79..aafc4d561be 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 @@ -11,7 +11,7 @@ ament_target_dependencies(order_layer ${dependencies} ) target_link_libraries(order_layer - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) install(TARGETS order_layer @@ -23,5 +23,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 19c2edb56fe..3f2f100d7a2 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -5,53 +5,54 @@ 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 ) From e8f16586da73476ea55575496c5c74954768907c Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 7 Mar 2024 14:12:33 -0800 Subject: [PATCH 08/17] Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski --- nav2_mppi_controller/README.md | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 7d5c9fb836f..58de7b79830 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -203,7 +203,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 @@ -223,24 +223,24 @@ controller_server: cost_power: 1 cost_weight: 5.0 threshold_to_consider: 0.5 - 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 - # Option to replace Obstacles and use Cost instead - # CostCritic: + # ObstaclesCritic: # enabled: true # cost_power: 1 - # cost_weight: 3.81 - # critical_cost: 300.0 - # consider_footprint: true - # collision_cost: 1000000.0 - # near_goal_distance: 1.0 + # 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 + 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 From 71dc02e02f5cf67228279d6b137473b3af292650 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Tue, 12 Mar 2024 07:08:28 +0800 Subject: [PATCH 09/17] completely shutdown inital_pose_sub_ (#4176) Signed-off-by: GoesM Co-authored-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index ef5635a4015..c465aa41490 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -326,6 +326,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // don't continue to process incoming messages global_loc_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 From 7f544f92063d21ac872ea11ca577eba9983a8f53 Mon Sep 17 00:00:00 2001 From: Antonio Park Date: Mon, 18 Mar 2024 20:44:40 +0900 Subject: [PATCH 10/17] fix typos in description messages (#4188) Signed-off-by: Antonio Park --- nav2_amcl/src/amcl_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index c465aa41490..34f4abce5cb 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1095,20 +1095,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; } @@ -1122,7 +1122,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; } From 0ae30f400551950e0abc6ee104d98db3ff2898bf Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 18 Mar 2024 13:41:28 +0100 Subject: [PATCH 11/17] AMCL: Set an initial guess by service call (#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock * added whitespace Signed-off-by: Alexander Mock * renamed initial pose service in callback bind Signed-off-by: Alexander Mock --------- Signed-off-by: Alexander Mock --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 11 +++++++++++ nav2_amcl/src/amcl_node.cpp | 14 ++++++++++++++ nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/srv/SetInitialPose.srv | 3 +++ 4 files changed, 29 insertions(+) create mode 100644 nav2_msgs/srv/SetInitialPose.srv diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 2360d09c253..030a85f38cd 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -35,6 +35,7 @@ #include "nav2_amcl/sensors/laser/laser.hpp" #include "nav2_msgs/msg/particle.hpp" #include "nav2_msgs/msg/particle_cloud.hpp" +#include "nav2_msgs/srv/set_initial_pose.hpp" #include "nav_msgs/srv/set_map.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "std_srvs/srv/empty.hpp" @@ -210,6 +211,16 @@ class AmclNode : public nav2_util::LifecycleNode const std::shared_ptr request, std::shared_ptr response); + // service server for providing an initial pose guess + rclcpp::Service::SharedPtr initial_guess_srv_; + /* + * @brief Service callback for an initial pose guess request + */ + void initialPoseReceivedSrv( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + // Let amcl update samples without requiring motion rclcpp::Service::SharedPtr nomotion_update_srv_; /* diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 34f4abce5cb..ba2010d507e 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -325,6 +325,7 @@ 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(); @@ -495,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( @@ -1535,6 +1545,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_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 2be81db0a74..b2cbccec481 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,6 +30,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/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 +--- + From 1d57084488d5bc975b0b0de75fc0aa374e6c7661 Mon Sep 17 00:00:00 2001 From: Antonio Park Date: Tue, 19 Mar 2024 22:11:29 +0900 Subject: [PATCH 12/17] Move lines for pre-computation to outside a loop (#4191) Signed-off-by: Kyungsik Park --- .../sensors/laser/likelihood_field_model.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) 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]; From 14d73de2afa112aa4a6f6e17a21f4d53e5844131 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 19 Mar 2024 17:00:11 +0100 Subject: [PATCH 13/17] Fix typo (#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar * Update README.md --------- Signed-off-by: Tony Najjar --- nav2_smac_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index c96df8da178..f9261cca91a 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 ``` From c18a19db44f59d1b197b3389d5503ceea968a71c Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Wed, 20 Mar 2024 22:14:15 +0100 Subject: [PATCH 14/17] Update footprint iif changed (#4193) Signed-off-by: Brice --- nav2_smac_planner/src/collision_checker.cpp | 1 + nav2_smac_planner/src/smac_planner_hybrid.cpp | 4 ++++ nav2_smac_planner/src/smac_planner_lattice.cpp | 4 ++++ 3 files changed, 9 insertions(+) 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/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 08ae3b5e8d7..482e0d27a73 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -283,6 +283,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 248ef1f218a..31090e10dd0 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 From a31a9a55620194d12d32a54398a7fd9646e4b809 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 4 Apr 2024 14:09:21 -0700 Subject: [PATCH 15/17] bump humble to 1.1.14 for sync --- 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/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/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_regulated_pure_pursuit_controller/package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- 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 +- 38 files changed, 38 insertions(+), 38 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index cac7910c5ba..1f5199979a0 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.13 + 1.1.14

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 f8a2170ddb9..b0fe9257fd2 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.13 + 1.1.14 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 5efaa152e24..0d6ca68ffc7 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.13 + 1.1.14 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 1e41f7a1ffa..0340705620e 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.13 + 1.1.14 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 02a5b4cad28..572ebe3c641 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.13 + 1.1.14 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index db17606e374..9ffc92422e4 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.13 + 1.1.14 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 456c8167c87..631a50e658e 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.13 + 1.1.14 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 cde44f16960..1e456a608c4 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.13 + 1.1.14 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 307180c8bfa..efd36e841f6 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.13 + 1.1.14 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index c4f2d2e77e3..5898d781897 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.13 + 1.1.14 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 41c31f39ac9..39e29574ec6 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.13 + 1.1.14 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 e784e648231..eb9ecc1aeb2 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.13 + 1.1.14 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 56607270d26..e4d7b922567 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.13 + 1.1.14 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 1dbf1dbd4d1..3193de89816 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.13 + 1.1.14 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 7419bfd2394..9dbea374a36 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.13 + 1.1.14 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 a21aef7e064..92efb2a7cca 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.13 + 1.1.14 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 5675e9455fb..fd08166dd95 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.1.13 + 1.1.14 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 195ee76e463..486b5879494 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.1.13 + 1.1.14 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 31adaf72788..d1b16c2212c 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.1.13 + 1.1.14 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 2fc9ef060c3..e04a65d0a7c 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.13 + 1.1.14 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 0852f14e228..13c58a57266 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.13 + 1.1.14 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index fceccc0ac14..f5a3211036c 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.13 + 1.1.14 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 593337b0858..ad356d640d1 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.13 + 1.1.14 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 8139ee98381..9f235683d01 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.13 + 1.1.14 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 36b13c0e5fa..0934f6e6e7a 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.13 + 1.1.14 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 fde5ca99392..df20eda09be 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.1.13 + 1.1.14 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 19834eb5f88..6f3c8214312 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.13 + 1.1.14 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 192a0a4d65e..25b640ca9a3 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.13 + 1.1.14 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 4f4b03332b9..9dae3590e19 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.13 + 1.1.14 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 c03b7de7fc3..617a3d570b5 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.13 + 1.1.14 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 3d25dd76988..38a352d0bea 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.13 + 1.1.14 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 8a14df53ebc..6dd18d384a9 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.13 + 1.1.14 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 7366ee3fe44..7fa8d96b54a 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.13 + 1.1.14 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 86b4fab06c7..19e690c777b 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.13 + 1.1.14 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 3cf71d4e712..9fa7e99540e 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.13 + 1.1.14 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 d9ad807fa4f..7a2d8dd89f1 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.13 + 1.1.14 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 507251b514b..2669d720b88 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.13 + 1.1.14 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 8e47a24f0ea..d546039a2c8 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.13 + 1.1.14 ROS2 Navigation Stack From d79b67f487c730d3929f8dc83f24ea4503730913 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 4 Apr 2024 14:27:48 -0700 Subject: [PATCH 16/17] fix merge conflict --- .../test/plugins/action/test_progress_checker_selector_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 574a514886b..08d589ac031 100644 --- 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 @@ -18,7 +18,7 @@ #include #include -#include "utils/test_action_server.hpp" +#include "../../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" From bbd4f34b22e32ae56974536514264ac01fcccad7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 4 Apr 2024 14:51:50 -0700 Subject: [PATCH 17/17] custom version of cost critic's cost method --- .../src/critics/cost_critic.cpp | 22 ++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 137e9433760..b91ab075b6a 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -57,6 +57,7 @@ void CostCritic::initialize() 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_) { @@ -65,13 +66,24 @@ float CostCritic::findCircumscribedCost( } // check if the costmap has an inflation layer - const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( - costmap, - inflation_layer_name_); - if (inflation_layer != nullptr) { + 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); - } else { + } + + if (!inflation_layer_found) { RCLCPP_WARN( logger_, "No inflation layer found in costmap configuration. "