From c370103a1b5bce8206f5b8d39e2582e13e7e0a5b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 20 Dec 2022 17:43:33 -0800 Subject: [PATCH] Humble sync 3: Dec 20 (#3334) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * remove exec_depend on behaviortree_cpp_v3 (#3279) * BT Service Node to throw if service was not available in time (#3256) * throw if service server wasn't available in time mimic the behavior of the bt action node constructor * throw if action unavailable in bt cancel action * use chrono literals namespace * fix linting errors * fix code style divergence * Remove duplicate of nav2_back_up_cancel_bt_node (#3332) * Remove unused velocity scaling config from example xml (#3330) Signed-off-by: Borong Yuan Signed-off-by: Borong Yuan * Make mapUpdateLoop() indicator variables to be thread-safe (#3308) * Ensure that plugin initialization to be called before updating routines (#3307) * Solve bug when CostmapInfoServer is reactivated (#3292) * Solve bug when CostmapInfoServer is reactivated * Implemented smoother selector bt node (#3283) * Implemented smoother selector bt node Signed-off-by: Owen Hooper <17ofh@queensu.ca> * updated copyright in modified file Signed-off-by: Owen Hooper <17ofh@queensu.ca> Signed-off-by: Owen Hooper <17ofh@queensu.ca> * Add allow_unknown parameter to theta star planner (#3286) * Add allow unknown parameter to theta star planner * Add allow unknown parameter to tests * missing comma * Change cost of unknown tiles * Uncrustify * bump to 1.1.3 for humble sync 3 Signed-off-by: Borong Yuan Signed-off-by: Owen Hooper <17ofh@queensu.ca> Co-authored-by: Adam Aposhian Co-authored-by: Erwin Lejeune Co-authored-by: Sven Langner Co-authored-by: Borong Yuan Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com> Co-authored-by: Owen Hooper <17ofh@queensu.ca> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/CMakeLists.txt | 3 + .../bt_cancel_action_node.hpp | 9 +- .../nav2_behavior_tree/bt_service_node.hpp | 13 +- .../plugins/action/smoother_selector_node.hpp | 99 ++++++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 6 + nav2_behavior_tree/package.xml | 3 +- .../plugins/action/smoother_selector_node.cpp | 92 +++++++++ .../test/plugins/action/CMakeLists.txt | 4 + .../action/test_smoother_selector_node.cpp | 187 ++++++++++++++++++ nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 3 +- nav2_bt_navigator/src/bt_navigator.cpp | 1 - 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/CMakeLists.txt | 1 + .../nav2_costmap_2d/costmap_2d_ros.hpp | 7 +- nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 39 ++-- .../test/regression/CMakeLists.txt | 20 ++ .../test/regression/order_layer.cpp | 60 ++++++ .../test/regression/order_layer.hpp | 46 +++++ .../test/regression/order_layer.xml | 7 + .../test/regression/plugin_api_order.cpp | 53 +++++ 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 +- nav2_lifecycle_manager/package.xml | 2 +- .../costmap_filter_info_server.hpp | 2 +- nav2_map_server/package.xml | 2 +- .../costmap_filter_info_server.cpp | 17 +- .../unit/test_costmap_filter_info_server.cpp | 31 +++ nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- .../README.md | 1 - .../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/theta_star.hpp | 13 +- nav2_theta_star_planner/package.xml | 2 +- nav2_theta_star_planner/src/theta_star.cpp | 1 + .../src/theta_star_planner.cpp | 6 + .../test/test_theta_star.cpp | 46 +++++ 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 +- 62 files changed, 765 insertions(+), 75 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/smoother_selector_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp create mode 100644 nav2_costmap_2d/test/regression/order_layer.cpp create mode 100644 nav2_costmap_2d/test/regression/order_layer.hpp create mode 100644 nav2_costmap_2d/test/regression/order_layer.xml create mode 100644 nav2_costmap_2d/test/regression/plugin_api_order.cpp diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index a2182a941da..90b31fd75b1 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.3 + 1.1.4

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 71ba3326180..2f02b38509e 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -186,6 +186,9 @@ list(APPEND plugin_libs nav2_planner_selector_bt_node) add_library(nav2_controller_selector_bt_node SHARED plugins/action/controller_selector_node.cpp) list(APPEND plugin_libs nav2_controller_selector_bt_node) +add_library(nav2_smoother_selector_bt_node SHARED plugins/action/smoother_selector_node.cpp) +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) 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 b48478ecc48..504df9157a1 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 @@ -27,6 +27,8 @@ namespace nav2_behavior_tree { +using namespace std::chrono_literals; // NOLINT + /** * @brief Abstract class representing an action for cancelling BT node * @tparam ActionT Type of action @@ -87,7 +89,12 @@ 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()); - action_client_->wait_for_action_server(); + if (!action_client_->wait_for_action_server(1s)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", + action_name.c_str()); + throw std::runtime_error(std::string("Action server %s not available", action_name.c_str())); + } } /** 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 c1970e38b18..bd81ee0ad48 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 @@ -17,6 +17,7 @@ #include #include +#include #include "behaviortree_cpp_v3/action_node.h" #include "nav2_util/node_utils.hpp" @@ -26,6 +27,8 @@ namespace nav2_behavior_tree { +using namespace std::chrono_literals; // NOLINT + /** * @brief Abstract class representing a service based BT node * @tparam ServiceT Type of service @@ -71,7 +74,15 @@ class BtServiceNode : public BT::ActionNodeBase RCLCPP_DEBUG( node_->get_logger(), "Waiting for \"%s\" service", service_name_.c_str()); - service_client_->wait_for_service(); + if (!service_client_->wait_for_service(1s)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", + service_node_name.c_str()); + throw std::runtime_error( + std::string( + "Service server %s not available", + service_node_name.c_str())); + } RCLCPP_DEBUG( node_->get_logger(), "\"%s\" BtServiceNode initialized", diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp new file mode 100644 index 00000000000..f9bb293d264 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// 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__SMOOTHER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_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 SmootherSelector behavior is used to switch the smoother + * that will be used by the smoother server. It subscribes to a topic "smoother_selector" + * to get the decision about what smoother must be used. It is usually used before of + * the FollowPath. The selected_smoother output port is passed to smoother_id + * input port of the FollowPath + */ +class SmootherSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::SmootherSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + SmootherSelector( + 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_smoother", + "the default smoother to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "smoother_selector", + "the input topic name to select the smoother"), + + BT::OutputPort( + "selected_smoother", + "Selected smoother by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the smoother_selector topic + * + * @param msg the message with the id of the smoother_selector + */ + void callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr smoother_selector_sub_; + + std::string last_selected_smoother_; + + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index f44848aec9d..1c452596d91 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -164,6 +164,12 @@ Name of the selected controller received from the topic subcription + + Name of the topic to receive smoother selection commands + Default smoother of the smoother selector + Name of the selected smoother received from the topic subcription + + Name of the topic to receive goal checker selection commands Default goal checker of the controller selector diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 9db3bba4a03..007d8965248 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.3 + 1.1.4 TODO Michael Jeronimo Carlos Orduno @@ -36,7 +36,6 @@ rclcpp_action rclcpp_lifecycle std_msgs - behaviortree_cpp_v3 builtin_interfaces geometry_msgs sensor_msgs diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp new file mode 100644 index 00000000000..0a84062e088 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp @@ -0,0 +1,92 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// Copyright (c) 2022 Owen Hooper +// +// 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/smoother_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +SmootherSelector::SmootherSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + smoother_selector_sub_ = node_->create_subscription( + topic_name_, + qos, + std::bind(&SmootherSelector::callbackSmootherSelect, this, _1), + sub_option); +} + +BT::NodeStatus SmootherSelector::tick() +{ + callback_group_executor_.spin_some(); + + // This behavior always use the last selected smoother received from the topic input. + // When no input is specified it uses the default smoother. + // If the default smoother is not specified then we work in "required smoother mode": + // In this mode, the behavior returns failure if the smoother selection is not received from + // the topic input. + if (last_selected_smoother_.empty()) { + std::string default_smoother; + getInput("default_smoother", default_smoother); + if (default_smoother.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_smoother_ = default_smoother; + } + } + + setOutput("selected_smoother", last_selected_smoother_); + + return BT::NodeStatus::SUCCESS; +} + +void +SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_smoother_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("SmootherSelector"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 0b7cae1a6cc..c640e824244 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -100,6 +100,10 @@ ament_add_gtest(test_controller_selector_node test_controller_selector_node.cpp) target_link_libraries(test_controller_selector_node nav2_controller_selector_bt_node) ament_target_dependencies(test_controller_selector_node ${dependencies}) +ament_add_gtest(test_smoother_selector_node test_smoother_selector_node.cpp) +target_link_libraries(test_smoother_selector_node nav2_smoother_selector_bt_node) +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}) diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp new file mode 100644 index 00000000000..c59cac1b2cf --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// 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 "../../test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class SmootherSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("smoother_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( + "SmootherSelector", + 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 SmootherSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * SmootherSelectorTestFixture::config_ = nullptr; +std::shared_ptr SmootherSelectorTestFixture::factory_ = nullptr; +std::shared_ptr SmootherSelectorTestFixture::tree_ = nullptr; + +TEST_F(SmootherSelectorTestFixture, 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_smoother_result; + config_->blackboard->get("selected_smoother", selected_smoother_result); + + EXPECT_EQ(selected_smoother_result, "DWB"); + + std_msgs::msg::String selected_smoother_cmd; + + selected_smoother_cmd.data = "DWC"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto smoother_selector_pub = + node_->create_publisher("smoother_selector_custom_topic_name", qos); + + // publish a few updates of the selected_smoother + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + smoother_selector_pub->publish(selected_smoother_cmd); + + rclcpp::spin_some(node_); + } + + // check smoother updated + config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_EQ("DWC", selected_smoother_result); +} + +TEST_F(SmootherSelectorTestFixture, 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_smoother_result; + config_->blackboard->get("selected_smoother", selected_smoother_result); + + EXPECT_EQ(selected_smoother_result, "GridBased"); + + std_msgs::msg::String selected_smoother_cmd; + + selected_smoother_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto smoother_selector_pub = + node_->create_publisher("smoother_selector", qos); + + // publish a few updates of the selected_smoother + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + smoother_selector_pub->publish(selected_smoother_cmd); + + rclcpp::spin_some(node_); + } + + // check smoother updated + config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_EQ("RRT", selected_smoother_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 e3720b02dc2..b6ebb1046af 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.3 + 1.1.4 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 1a810c2a231..ded87dfe85f 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.3 + 1.1.4 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 a636f3590be..c1ae283b462 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.3 + 1.1.4 TODO Michael Jeronimo Apache-2.0 @@ -24,7 +24,6 @@ nav2_util nav2_core - behaviortree_cpp_v3 rclcpp rclcpp_action rclcpp_lifecycle diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 1168006704e..d155d54f521 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -83,7 +83,6 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_spin_cancel_bt_node", "nav2_assisted_teleop_cancel_bt_node", "nav2_back_up_cancel_bt_node", - "nav2_back_up_cancel_bt_node", "nav2_drive_on_heading_cancel_bt_node" }; diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 53174b7aab0..8ed16858a01 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.3 + 1.1.4 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 2af8bace078..c32890318b7 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.3 + 1.1.4 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 9b41192a0d7..5c60f13d078 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.3 + 1.1.4 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 50faaaf4cbc..4d4b341d566 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.3 + 1.1.4 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 3e796d0ef19..d2d17287187 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.3 + 1.1.4 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 96b4f493393..683cb04fdd2 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -180,6 +180,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) add_subdirectory(test) + pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml) endif() ament_export_include_directories(include) 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 a5dfc6182ac..af93f4c7e0c 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 @@ -38,6 +38,7 @@ #ifndef NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_ #define NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_ +#include #include #include #include @@ -329,9 +330,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ void mapUpdateLoop(double frequency); bool map_update_thread_shutdown_{false}; - bool stop_updates_{false}; - bool initialized_{false}; - bool stopped_{true}; + std::atomic stop_updates_{false}; + std::atomic initialized_{false}; + std::atomic stopped_{true}; 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/package.xml b/nav2_costmap_2d/package.xml index 73dcd3d87d2..d461e287d01 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.3 + 1.1.4 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/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 58f346ba4fd..7f1d582ffb4 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -415,24 +415,27 @@ Costmap2DROS::mapUpdateLoop(double frequency) while (rclcpp::ok() && !map_update_thread_shutdown_) { nav2_util::ExecutionTimer timer; - // Measure the execution time of the updateMap method - timer.start(); - updateMap(); - timer.end(); - - RCLCPP_DEBUG(get_logger(), "Map update time: %.9f", timer.elapsed_time_in_seconds()); - if (publish_cycle_ > rclcpp::Duration(0s) && layered_costmap_->isInitialized()) { - unsigned int x0, y0, xn, yn; - layered_costmap_->getBounds(&x0, &xn, &y0, &yn); - costmap_publisher_->updateBounds(x0, xn, y0, yn); - - auto current_time = now(); - if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT - { - RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); - costmap_publisher_->publishCostmap(); - last_publish_ = current_time; + // Execute after start() will complete plugins activation + if (!stopped_) { + // Measure the execution time of the updateMap method + timer.start(); + updateMap(); + timer.end(); + + RCLCPP_DEBUG(get_logger(), "Map update time: %.9f", timer.elapsed_time_in_seconds()); + if (publish_cycle_ > rclcpp::Duration(0s) && layered_costmap_->isInitialized()) { + unsigned int x0, y0, xn, yn; + layered_costmap_->getBounds(&x0, &xn, &y0, &yn); + costmap_publisher_->updateBounds(x0, xn, y0, yn); + + auto current_time = now(); + if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due + (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + { + RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); + costmap_publisher_->publishCostmap(); + last_publish_ = current_time; + } } } diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index c492459d2ff..c9a52aa8af7 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,4 +1,24 @@ +# Bresenham2D corner cases test ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) target_link_libraries(costmap_bresenham_2d nav2_costmap_2d_core ) + +# OrderLayer for checking Costmap2D plugins API calling order +add_library(order_layer SHARED + order_layer.cpp) +ament_target_dependencies(order_layer + ${dependencies} +) +install(TARGETS + order_layer + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# 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 +) diff --git a/nav2_costmap_2d/test/regression/order_layer.cpp b/nav2_costmap_2d/test/regression/order_layer.cpp new file mode 100644 index 00000000000..6e69e210a72 --- /dev/null +++ b/nav2_costmap_2d/test/regression/order_layer.cpp @@ -0,0 +1,60 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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. Reserved. + +#include "order_layer.hpp" + +#include +#include + +using namespace std::chrono_literals; + +namespace nav2_costmap_2d +{ + +OrderLayer::OrderLayer() +: activated_(false) +{ +} + +void OrderLayer::activate() +{ + std::this_thread::sleep_for(100ms); + activated_ = true; +} + +void OrderLayer::deactivate() +{ + activated_ = false; +} + +void OrderLayer::updateBounds( + double, double, double, double *, double *, double *, double *) +{ + if (!activated_) { + throw std::runtime_error("update before activated"); + } +} + +void OrderLayer::updateCosts( + nav2_costmap_2d::Costmap2D &, int, int, int, int) +{ + if (!activated_) { + throw std::runtime_error("update before activated"); + } +} + +} // namespace nav2_costmap_2d + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::OrderLayer, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/test/regression/order_layer.hpp b/nav2_costmap_2d/test/regression/order_layer.hpp new file mode 100644 index 00000000000..35bb2aeabb7 --- /dev/null +++ b/nav2_costmap_2d/test/regression/order_layer.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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. Reserved. + +#ifndef NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ +#define NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ + +#include "nav2_costmap_2d/layer.hpp" + +namespace nav2_costmap_2d +{ + +class OrderLayer : public nav2_costmap_2d::Layer +{ +public: + OrderLayer(); + + virtual void activate(); + virtual void deactivate(); + + virtual void reset() {} + virtual bool isClearable() {return false;} + + virtual void updateBounds( + double, double, double, double *, double *, double *, double *); + + virtual void updateCosts( + nav2_costmap_2d::Costmap2D &, int, int, int, int); + +private: + bool activated_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ diff --git a/nav2_costmap_2d/test/regression/order_layer.xml b/nav2_costmap_2d/test/regression/order_layer.xml new file mode 100644 index 00000000000..ffc553a1522 --- /dev/null +++ b/nav2_costmap_2d/test/regression/order_layer.xml @@ -0,0 +1,7 @@ + + + + Plugin checking order of activate() and updateBounds()/updateCosts() calls + + + diff --git a/nav2_costmap_2d/test/regression/plugin_api_order.cpp b/nav2_costmap_2d/test/regression/plugin_api_order.cpp new file mode 100644 index 00000000000..c7baf34453a --- /dev/null +++ b/nav2_costmap_2d/test/regression/plugin_api_order.cpp @@ -0,0 +1,53 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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. Reserved. + +#include +#include +#include + +#include +#include + +TEST(CostmapPluginsTester, checkPluginAPIOrder) +{ + std::shared_ptr costmap_ros = + std::make_shared("costmap_ros"); + + // Workaround to avoid setting base_link->map transform + costmap_ros->set_parameter(rclcpp::Parameter("robot_base_frame", "map")); + // Specifying order verification plugin in the parameters + std::vector plugins_str; + plugins_str.push_back("order_layer"); + costmap_ros->set_parameter(rclcpp::Parameter("plugins", plugins_str)); + costmap_ros->declare_parameter( + "order_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::OrderLayer"))); + + // Do actual test: ensure that plugin->updateBounds()/updateCosts() + // will be called after plugin->activate() + costmap_ros->on_configure(costmap_ros->get_current_state()); + costmap_ros->on_activate(costmap_ros->get_current_state()); + + // Do cleanup + costmap_ros->on_deactivate(costmap_ros->get_current_state()); + costmap_ros->on_cleanup(costmap_ros->get_current_state()); + costmap_ros->on_shutdown(costmap_ros->get_current_state()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 4216e878d63..509153095d8 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.3 + 1.1.4 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 61c9ad2c4aa..70bb7915879 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.3 + 1.1.4 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 01870d51d2d..23f5f84886e 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.3 + 1.1.4 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 b96cd4b6257..ef00367c5b0 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.3 + 1.1.4 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 aab31ad8878..b4598fee026 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.3 + 1.1.4 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 e90c4c4a9cc..443bdf44d30 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.3 + 1.1.4 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 cabf4c19b33..2738d143ae1 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.3 + 1.1.4 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 76d7814f13a..9429c2f8e70 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.3 + 1.1.4 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 20234d5f7ae..945943e8e5f 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.3 + 1.1.4 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/costmap_filter_info_server.hpp b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp index 99311deff75..14a69ad229d 100644 --- a/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp +++ b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp @@ -71,7 +71,7 @@ class CostmapFilterInfoServer : public nav2_util::LifecycleNode private: rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; - std::unique_ptr msg_; + nav2_msgs::msg::CostmapFilterInfo msg_; }; // CostmapFilterInfoServer } // namespace nav2_map_server diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 864e627452a..f8fbea5b642 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.3 + 1.1.4 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp index 7b1c618315d..f6671b02912 100644 --- a/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp +++ b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp @@ -48,13 +48,13 @@ CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/) publisher_ = this->create_publisher( filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - msg_ = std::make_unique(); - msg_->header.frame_id = ""; - msg_->header.stamp = now(); - msg_->type = get_parameter("type").as_int(); - msg_->filter_mask_topic = get_parameter("mask_topic").as_string(); - msg_->base = static_cast(get_parameter("base").as_double()); - msg_->multiplier = static_cast(get_parameter("multiplier").as_double()); + msg_ = nav2_msgs::msg::CostmapFilterInfo(); + msg_.header.frame_id = ""; + msg_.header.stamp = now(); + msg_.type = get_parameter("type").as_int(); + msg_.filter_mask_topic = get_parameter("mask_topic").as_string(); + msg_.base = static_cast(get_parameter("base").as_double()); + msg_.multiplier = static_cast(get_parameter("multiplier").as_double()); return nav2_util::CallbackReturn::SUCCESS; } @@ -65,7 +65,8 @@ CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Activating"); publisher_->on_activate(); - publisher_->publish(std::move(msg_)); + auto costmap_filter_info = std::make_unique(msg_); + publisher_->publish(std::move(costmap_filter_info)); // create bond connection createBond(); diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp index 168fbc9f7b9..072aaf8d1be 100644 --- a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -59,6 +59,16 @@ class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer on_cleanup(get_current_state()); on_shutdown(get_current_state()); } + + void deactivate() + { + on_deactivate(get_current_state()); + } + + void activate() + { + on_activate(get_current_state()); + } }; class InfoServerTester : public ::testing::Test @@ -144,3 +154,24 @@ TEST_F(InfoServerTester, testCostmapFilterInfoPublish) EXPECT_NEAR(info_->base, BASE, EPSILON); EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); } + +TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate) +{ + info_server_->deactivate(); + info_ = nullptr; + info_server_->activate(); + + rclcpp::Time start_time = info_server_->now(); + while (!isReceived()) { + rclcpp::spin_some(info_server_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + // Waiting no more than 5 seconds + ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); + } + + // Checking received CostmapFilterInfo for consistency + EXPECT_EQ(info_->type, TYPE); + EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC); + EXPECT_NEAR(info_->base, BASE, EPSILON); + EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); +} diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 07ead113892..1867c28fd86 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.3 + 1.1.4 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 7cea1555cc7..a4f76371e95 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.3 + 1.1.4 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 529a9e37cc1..2282cd3ee7c 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.3 + 1.1.4 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index dcdedcdc0e6..ac5faec3adf 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -111,7 +111,6 @@ controller_server: transform_tolerance: 0.1 use_velocity_scaled_lookahead_dist: false min_approach_linear_velocity: 0.05 - use_approach_linear_velocity_scaling: true approach_velocity_scaling_dist: 1.0 use_collision_detection: true max_allowed_time_to_collision_up_to_carrot: 1.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 80a036d89bf..daa218f2da6 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.3 + 1.1.4 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 2bb4f15ef76..3225f1ed37d 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.3 + 1.1.4 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 3f79200a22f..e4169f1348d 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.3 + 1.1.4 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 4452af008a9..c983032bd6f 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.3 + 1.1.4 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 772d19e86ed..222ad19b6ff 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.3 + 1.1.4 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 413a9b012af..a13d49de0a1 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.3 + 1.1.4 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 6d3044d3805..ca233b7f9d6 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.3 + 1.1.4 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index dacd8a57bc3..d1ddf7354ce 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -24,6 +24,8 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" const double INF_COST = DBL_MAX; +const int UNKNOWN_COST = 255; +const int OBS_COST = 254; const int LETHAL_COST = 252; struct coordsM @@ -70,6 +72,8 @@ class ThetaStar double w_heuristic_cost_; /// parameter to set the number of adjacent nodes to be searched on int how_many_corners_; + /// parameter to set weather the planner can plan through unknown space + bool allow_unknown_; /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; @@ -91,7 +95,9 @@ class ThetaStar */ inline bool isSafe(const int & cx, const int & cy) const { - return costmap_->getCost(cx, cy) < LETHAL_COST; + return (costmap_->getCost( + cx, + cy) == UNKNOWN_COST && allow_unknown_) || costmap_->getCost(cx, cy) < LETHAL_COST; } /** @@ -185,7 +191,10 @@ class ThetaStar bool isSafe(const int & cx, const int & cy, double & cost) const { double curr_cost = getCost(cx, cy); - if (curr_cost < LETHAL_COST) { + if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || curr_cost < LETHAL_COST) { + if (costmap_->getCost(cx, cy) == UNKNOWN_COST) { + curr_cost = OBS_COST - 1; + } cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; return true; } else { diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 4e30bbae542..bbec02ae7bd 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.3 + 1.1.4 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index e0267b34c0e..baddc754d7a 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -23,6 +23,7 @@ ThetaStar::ThetaStar() w_euc_cost_(2.0), w_heuristic_cost_(1.0), how_many_corners_(8), + allow_unknown_(true), size_x_(0), size_y_(0), index_generated_(0) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index cc98649ccc2..2913b5ef7e5 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -45,6 +45,10 @@ void ThetaStarPlanner::configure( RCLCPP_WARN(logger_, "Your value for - .how_many_corners was overridden, and is now set to 8"); } + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + ".allow_unknown", planner_->allow_unknown_); + nav2_util::declare_parameter_if_not_declared( node, name_ + ".w_euc_cost", rclcpp::ParameterValue(1.0)); node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); @@ -215,6 +219,8 @@ ThetaStarPlanner::dynamicParametersCallback(std::vector param } else if (type == ParameterType::PARAMETER_BOOL) { if (name == name_ + ".use_final_approach_orientation") { use_final_approach_orientation_ = parameter.as_bool(); + } else if (name == name_ + ".allow_unknown") { + planner_->allow_unknown_ = parameter.as_bool(); } } } diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 7776179963f..54706c92a64 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -185,3 +185,49 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { life_node.reset(); costmap_ros.reset(); } + +TEST(ThetaStarPlanner, test_theta_star_reconfigure) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr life_node = + std::make_shared("ThetaStarPlannerTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + auto planner = std::make_unique(); + try { + // Expect to throw due to invalid prims file in param + planner->configure(life_node, "test", nullptr, costmap_ros); + } catch (...) { + } + planner->activate(); + + auto rec_param = std::make_shared( + life_node->get_node_base_interface(), life_node->get_node_topics_interface(), + life_node->get_node_graph_interface(), + life_node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.how_many_corners", 8), + rclcpp::Parameter("test.w_euc_cost", 1.0), + rclcpp::Parameter("test.w_traversal_cost", 2.0), + rclcpp::Parameter("test.use_final_approach_orientation", false), + rclcpp::Parameter("test.allow_unknown", false)}); + + rclcpp::spin_until_future_complete( + life_node->get_node_base_interface(), + results); + + EXPECT_EQ(life_node->get_parameter("test.how_many_corners").as_int(), 8); + EXPECT_EQ( + life_node->get_parameter("test.w_euc_cost").as_double(), + 1.0); + EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0); + EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false); + EXPECT_EQ(life_node->get_parameter("test.allow_unknown").as_bool(), false); + + rclcpp::spin_until_future_complete( + life_node->get_node_base_interface(), + results); +} diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 63dd80a0ca8..0f92cf9ab7b 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.3 + 1.1.4 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index ba5b283840f..387c0ea3fea 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.3 + 1.1.4 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 f7769dc1ef0..4b8b6379c10 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.3 + 1.1.4 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 a25b16b109b..8532ccb1c57 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.3 + 1.1.4 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 378b2202be4..cdabc7d4b5d 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.3 + 1.1.4 ROS2 Navigation Stack