From 600633ae3cb1b090c9c01bab77e13410b12a0f10 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 16 Nov 2023 10:22:04 -0800 Subject: [PATCH 01/38] adding support for rotate in place cusps (#3934) --- .../src/regulated_pure_pursuit_controller.cpp | 17 ++++++++++++++++- nav2_util/include/nav2_util/geometry_utils.hpp | 2 +- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 8c332d4969e..9434b105f4c 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -419,12 +419,27 @@ double RegulatedPurePursuitController::findVelocitySignChange( /* Checking for the existance of cusp, in the path, using the dot product and determine it's distance from the robot. If there is no cusp in the path, then just determine the distance to the goal location. */ - if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) { + const double dot_prod = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_prod < 0.0) { // returning the distance if there is a cusp // The transformed path is in the robots frame, so robot is at the origin return hypot( transformed_plan.poses[pose_id].pose.position.x, transformed_plan.poses[pose_id].pose.position.y); + } else if ( + (hypot(oa_x, oa_y) == 0.0 && + transformed_plan.poses[pose_id - 1].pose.orientation != + transformed_plan.poses[pose_id].pose.orientation) + || + (hypot(ab_x, ab_y) == 0.0 && + transformed_plan.poses[pose_id].pose.orientation != + transformed_plan.poses[pose_id + 1].pose.orientation)) + { + // returning the distance since the points overlap + // but are not simply duplicate points (e.g. in place rotation) + return hypot( + transformed_plan.poses[pose_id].pose.position.x, + transformed_plan.poses[pose_id].pose.position.y); } } diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 884b7b366ed..67cc87243d6 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -132,7 +132,7 @@ inline Iter min_by(Iter begin, Iter end, Getter getCompareVal) Iter lowest_it = begin; for (Iter it = ++begin; it != end; ++it) { auto comp = getCompareVal(*it); - if (comp < lowest) { + if (comp <= lowest) { lowest = comp; lowest_it = it; } From cefce2cd0461aa999d075e7427b1bb8e4a0c6b19 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 16 Nov 2023 12:38:32 -0800 Subject: [PATCH 02/38] adding base footprint publisher to nav2_util (#3860) * adding base footprint publisher to nav2_util * adding unit test --- .../nav2_util/base_footprint_publisher.hpp | 129 ++++++++++++++++++ nav2_util/src/CMakeLists.txt | 5 + nav2_util/src/base_footprint_publisher.cpp | 27 ++++ nav2_util/test/CMakeLists.txt | 4 + .../test/test_base_footprint_publisher.cpp | 72 ++++++++++ 5 files changed, 237 insertions(+) create mode 100644 nav2_util/include/nav2_util/base_footprint_publisher.hpp create mode 100644 nav2_util/src/base_footprint_publisher.cpp create mode 100644 nav2_util/test/test_base_footprint_publisher.cpp diff --git a/nav2_util/include/nav2_util/base_footprint_publisher.hpp b/nav2_util/include/nav2_util/base_footprint_publisher.hpp new file mode 100644 index 00000000000..52bcdb53eb0 --- /dev/null +++ b/nav2_util/include/nav2_util/base_footprint_publisher.hpp @@ -0,0 +1,129 @@ +// Copyright (c) 2023 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ +#define NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +namespace nav2_util +{ + +/** + * @brief A TF2 listener that overrides the subscription callback + * to inject base footprint publisher removing Z, Pitch, and Roll for + * 3D state estimation but desiring a 2D frame for navigation, visualization, or other reasons + */ +class BaseFootprintPublisherListener : public tf2_ros::TransformListener +{ +public: + BaseFootprintPublisherListener(tf2::BufferCore & buffer, bool spin_thread, rclcpp::Node & node) + : tf2_ros::TransformListener(buffer, spin_thread) + { + node.declare_parameter( + "base_link_frame", rclcpp::ParameterValue(std::string("base_link"))); + node.declare_parameter( + "base_footprint_frame", rclcpp::ParameterValue(std::string("base_footprint"))); + base_link_frame_ = node.get_parameter("base_link_frame").as_string(); + base_footprint_frame_ = node.get_parameter("base_footprint_frame").as_string(); + tf_broadcaster_ = std::make_shared(node); + } + + /** + * @brief Overrides TF2 subscription callback to inject base footprint publisher + */ + void subscription_callback(tf2_msgs::msg::TFMessage::ConstSharedPtr msg, bool is_static) override + { + TransformListener::subscription_callback(msg, is_static); + + if (is_static) { + return; + } + + for (unsigned int i = 0; i != msg->transforms.size(); i++) { + auto & t = msg->transforms[i]; + if (t.child_frame_id == base_link_frame_) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = t.header.stamp; + transform.header.frame_id = base_link_frame_; + transform.child_frame_id = base_footprint_frame_; + + // Project to Z-zero + transform.transform.translation = t.transform.translation; + transform.transform.translation.z = 0.0; + + // Remove Roll and Pitch + tf2::Quaternion q; + q.setRPY(0, 0, tf2::getYaw(t.transform.rotation)); + q.normalize(); + transform.transform.rotation.x = q.x(); + transform.transform.rotation.y = q.y(); + transform.transform.rotation.z = q.z(); + transform.transform.rotation.w = q.w(); + + tf_broadcaster_->sendTransform(transform); + return; + } + } + } + +protected: + std::shared_ptr tf_broadcaster_; + std::string base_link_frame_, base_footprint_frame_; +}; + +/** + * @class nav2_util::BaseFootprintPublisher + * @brief Republishes the ``base_link`` frame as ``base_footprint`` + * stripping away the Z, Roll, and Pitch of the full 3D state to provide + * a 2D projection for navigation when state estimation is full 3D + */ +class BaseFootprintPublisher : public rclcpp::Node +{ +public: + /** + * @brief A constructor + */ + explicit BaseFootprintPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("base_footprint_publisher", options) + { + RCLCPP_INFO(get_logger(), "Creating base footprint publisher"); + tf_buffer_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), + get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + listener_publisher_ = std::make_shared( + *tf_buffer_, true, *this); + } + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr listener_publisher_; +}; + +} // end namespace nav2_util + +#endif // NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index a639a0e59e9..e378eac514d 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -28,6 +28,11 @@ add_executable(lifecycle_bringup ) target_link_libraries(lifecycle_bringup ${library_name}) +add_executable(base_footprint_publisher + base_footprint_publisher.cpp +) +target_link_libraries(base_footprint_publisher ${library_name}) + find_package(Boost REQUIRED COMPONENTS program_options) install(TARGETS diff --git a/nav2_util/src/base_footprint_publisher.cpp b/nav2_util/src/base_footprint_publisher.cpp new file mode 100644 index 00000000000..f3b6791db44 --- /dev/null +++ b/nav2_util/src/base_footprint_publisher.cpp @@ -0,0 +1,27 @@ +// 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_util/base_footprint_publisher.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 9f1ae99bbcb..d68291f6195 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,3 +41,7 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) + +ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp) +ament_target_dependencies(test_base_footprint_publisher geometry_msgs) +target_link_libraries(test_base_footprint_publisher ${library_name}) diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp new file mode 100644 index 00000000000..47dc83c7f31 --- /dev/null +++ b/nav2_util/test/test_base_footprint_publisher.cpp @@ -0,0 +1,72 @@ +// 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 + +#include "nav2_util/base_footprint_publisher.hpp" +#include "gtest/gtest.h" +#include "tf2/exceptions.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) +{ + auto node = std::make_shared(); + rclcpp::spin_some(node->get_node_base_interface()); + + auto tf_broadcaster = std::make_shared(node); + auto buffer = std::make_shared(node->get_clock()); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); + buffer->setCreateTimerInterface(timer_interface); + auto listener = std::make_shared(*buffer, true); + + std::string base_link = "base_link"; + std::string base_footprint = "base_footprint"; + + // Publish something to TF, should fail, doesn't exist + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node->now(); + transform.header.frame_id = "test1_1"; + transform.child_frame_id = "test1"; + tf_broadcaster->sendTransform(transform); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_THROW( + buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero), + tf2::TransformException); + + // This is valid, should work now and communicate the Z-removed info + transform.header.stamp = node->now(); + transform.header.frame_id = "odom"; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = 1.0; + transform.transform.translation.y = 1.0; + transform.transform.translation.z = 1.0; + tf_broadcaster->sendTransform(transform); + rclcpp::Rate r(1.0); + r.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + auto t = buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero); + EXPECT_EQ(t.transform.translation.x, 1.0); + EXPECT_EQ(t.transform.translation.y, 1.0); + EXPECT_EQ(t.transform.translation.z, 0.0); +} From d9bea52838d67b7ecfefb60788a3a4ff61f587ca Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 17 Nov 2023 10:59:49 -0800 Subject: [PATCH 03/38] Fix linting error (#3969) * Fix linting error * Update regulated_pure_pursuit_controller.cpp --- .../src/regulated_pure_pursuit_controller.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 9434b105f4c..76837754e86 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -426,14 +426,16 @@ double RegulatedPurePursuitController::findVelocitySignChange( return hypot( transformed_plan.poses[pose_id].pose.position.x, transformed_plan.poses[pose_id].pose.position.y); - } else if ( + } + + if ( (hypot(oa_x, oa_y) == 0.0 && - transformed_plan.poses[pose_id - 1].pose.orientation != - transformed_plan.poses[pose_id].pose.orientation) + transformed_plan.poses[pose_id - 1].pose.orientation != + transformed_plan.poses[pose_id].pose.orientation) || (hypot(ab_x, ab_y) == 0.0 && - transformed_plan.poses[pose_id].pose.orientation != - transformed_plan.poses[pose_id + 1].pose.orientation)) + transformed_plan.poses[pose_id].pose.orientation != + transformed_plan.poses[pose_id + 1].pose.orientation)) { // returning the distance since the points overlap // but are not simply duplicate points (e.g. in place rotation) From 6f27d014959fec7b3ed47c988ce9c363f1c88e75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20Ga=C5=82ecki?= <73710544+embeddedadam@users.noreply.github.com> Date: Mon, 20 Nov 2023 20:02:03 +0100 Subject: [PATCH 04/38] WIP: Make BT nodes have configurable wait times. (#3960) * Make BT nodes have configurable wait times. Previous solution provided hardcoded 1s value. Right now the value can be configured for BT Action, Cancel and Service nodes. [#3920] Signed-off-by: Adam Galecki * Make BT nodes have configurable wait times. Previous solution provided hardcoded 1s value. Right now the value can be configured for BT Action, Cancel and Service nodes. [#3920] Signed-off-by: Adam Galecki * Fix typos, linting errors and value type from float to int * Fix extra underscores * Fix extra underscore * Update unit tests with blackboard parameter Signed-off-by: Adam Galecki * Fix formatting errors Signed-off-by: Adam Galecki * Update system tests to match new parameter Signed-off-by: Adam Galecki --------- Signed-off-by: Adam Galecki --- .../include/nav2_behavior_tree/bt_action_node.hpp | 7 ++++++- .../include/nav2_behavior_tree/bt_action_server.hpp | 3 +++ .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 9 +++++++++ .../include/nav2_behavior_tree/bt_cancel_action_node.hpp | 6 +++++- .../include/nav2_behavior_tree/bt_service_node.hpp | 7 ++++++- .../test/plugins/action/test_assisted_teleop_action.cpp | 3 +++ .../plugins/action/test_assisted_teleop_cancel_node.cpp | 3 +++ .../test/plugins/action/test_back_up_action.cpp | 3 +++ .../test/plugins/action/test_back_up_cancel_node.cpp | 3 +++ .../test/plugins/action/test_bt_action_node.cpp | 1 + .../test/plugins/action/test_clear_costmap_service.cpp | 9 +++++++++ .../action/test_compute_path_through_poses_action.cpp | 3 +++ .../plugins/action/test_compute_path_to_pose_action.cpp | 3 +++ .../test/plugins/action/test_controller_cancel_node.cpp | 3 +++ .../test/plugins/action/test_drive_on_heading_action.cpp | 3 +++ .../plugins/action/test_drive_on_heading_cancel_node.cpp | 3 +++ .../test/plugins/action/test_follow_path_action.cpp | 3 +++ .../action/test_navigate_through_poses_action.cpp | 3 +++ .../test/plugins/action/test_navigate_to_pose_action.cpp | 3 +++ .../test_reinitialize_global_localization_service.cpp | 3 +++ .../test/plugins/action/test_smooth_path_action.cpp | 3 +++ .../test/plugins/action/test_spin_action.cpp | 3 +++ .../test/plugins/action/test_spin_cancel_node.cpp | 3 +++ .../test/plugins/action/test_wait_action.cpp | 3 +++ .../test/plugins/action/test_wait_cancel_node.cpp | 3 +++ nav2_bringup/params/nav2_multirobot_params_1.yaml | 1 + nav2_bringup/params/nav2_multirobot_params_2.yaml | 1 + nav2_bringup/params/nav2_multirobot_params_all.yaml | 1 + nav2_bringup/params/nav2_params.yaml | 1 + .../src/behavior_tree/test_behavior_tree_node.cpp | 2 ++ .../src/gps_navigation/nav2_no_map_params.yaml | 1 + 31 files changed, 100 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index e6eef2992f4..39c5c5f7d95 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -61,6 +61,8 @@ class BtActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); // Initialize the input and output messages goal_ = typename ActionT::Goal(); @@ -93,7 +95,7 @@ class BtActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(1s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", action_name.c_str()); @@ -462,6 +464,9 @@ class BtActionNode : public BT::ActionNodeBase // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_duration_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // To track the action server acknowledgement when a new goal is sent std::shared_ptr::SharedPtr>> future_goal_handle_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 467e4c80d0b..331ca237c8f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -248,6 +248,9 @@ class BtActionServer // Default timeout value while waiting for response from a server std::chrono::milliseconds default_server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 8d06ee7075f..cd089692b87 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -158,6 +158,9 @@ bool BtActionServer::on_configure() int default_server_timeout; node->get_parameter("default_server_timeout", default_server_timeout); default_server_timeout_ = std::chrono::milliseconds(default_server_timeout); + int wait_for_service_timeout; + node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); + wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -172,6 +175,9 @@ bool BtActionServer::on_configure() blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT + blackboard_->set( + "wait_for_service_timeout", + wait_for_service_timeout_); return true; } @@ -235,6 +241,9 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); + blackboard->set( + "wait_for_service_timeout", + wait_for_service_timeout_); } } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index 179c93e4d23..ea3e41d859d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -59,6 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); std::string remapped_action_name; if (getInput("server_name", remapped_action_name)) { @@ -89,7 +91,7 @@ class BtCancelActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(1s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", action_name.c_str()); @@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase // The timeout value while waiting for response from a server when a // new action goal is canceled std::chrono::milliseconds server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index a027ac7760c..1afdb5adfd1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -62,6 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); @@ -77,7 +79,7 @@ class BtServiceNode : public BT::ActionNodeBase RCLCPP_DEBUG( node_->get_logger(), "Waiting for \"%s\" service", service_name_.c_str()); - if (!service_client_->wait_for_service(1s)) { + if (!service_client_->wait_for_service(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", service_node_name.c_str()); @@ -249,6 +251,9 @@ class BtServiceNode : public BT::ActionNodeBase // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_duration_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // To track the server response when a new request is sent std::shared_future future_result_; bool request_sent_{false}; diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp index 38033598d01..4bb99057591 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -68,6 +68,9 @@ class AssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index a7af3efef6a..40f6f65c88a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "assisted_teleop"); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 6f0d54e7ca9..85e3d3764dc 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -68,6 +68,9 @@ class BackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp index 35ab9d2f88d..99e6eb1b45d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelBackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "back_up"); diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index 12c25d30d6f..a283dee33e7 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -173,6 +173,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("node", node_); config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); + config_->blackboard->set("wait_for_service_timeout", 1000ms); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("on_cancelled_triggered", false); diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index 4351152acfb..88173e84205 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -53,6 +53,9 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -139,6 +142,9 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -231,6 +237,9 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 40bbeb1a375..ec619b01205 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -76,6 +76,9 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index ca5bcea666c..d726186cd31 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -74,6 +74,9 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index 5128391d03b..290c9133a52 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelControllerActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "follow_path"); diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index 043f1aedb62..06478df588f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -69,6 +69,9 @@ class DriveOnHeadingActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp index cc2e7f0693f..5378123a461 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelDriveOnHeadingTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "drive_on_heading_cancel"); diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 4fa291c4b78..c4fcc575193 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -67,6 +67,9 @@ class FollowPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index bd028827af8..5fa2c9aa036 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -70,6 +70,9 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); std::vector poses; config_->blackboard->set>( diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index 13e3a2a2ca9..264b775a680 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -68,6 +68,9 @@ class NavigateToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 122e6d0e0b1..2f7a36e7fa7 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -53,6 +53,9 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index d1c930b5e40..c2e43890212 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -67,6 +67,9 @@ class SmoothPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index 56f5cdae746..a6d9f36d57a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -68,6 +68,9 @@ class SpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index d773e88bcad..ea3b01b983f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelSpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "spin"); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 2df39599a98..f54250a2b67 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -59,6 +59,9 @@ class WaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp index e8d4d053c7d..0773c72c6da 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelWaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "wait"); diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 692ca6ec803..d0bdce21db2 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 2d57aef3830..cd51b1332e4 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 6f7e66cd617..b6e5712092f 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 112a1de98b1..e70fa9216cd 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 48ba35038cd..48b97848f8f 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -138,6 +138,8 @@ class BehaviorTreeHandler "server_timeout", std::chrono::milliseconds(20)); // NOLINT blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT + blackboard->set( + "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 2539cef7b59..7b3125afbae 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -5,6 +5,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" From fbe8f56042a07513630f50b563f5690a30b1346a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 20 Nov 2023 12:41:01 -0800 Subject: [PATCH 05/38] Enabling soft realtime prioritization to the Controller Server (#3914) * Enabling soft realtime prioritization to the controller server * abstracting to another function * changing default priorities * linting --- nav2_bringup/params/nav2_params.yaml | 1 + .../nav2_controller/controller_server.hpp | 1 + nav2_controller/src/controller_server.cpp | 22 ++++++---- nav2_util/include/nav2_util/node_thread.hpp | 6 ++- .../nav2_util/simple_action_server.hpp | 41 +++++++++++++++++-- nav2_util/src/node_thread.cpp | 9 ++-- 6 files changed, 64 insertions(+), 16 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index e70fa9216cd..356118f19db 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -122,6 +122,7 @@ controller_server: progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + use_realtime_priority: false # Progress checker parameters progress_checker: diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 544155ebb3f..992c8fab041 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -265,6 +265,7 @@ class ControllerServer : public nav2_util::LifecycleNode double min_theta_velocity_threshold_; double failure_tolerance_; + bool use_realtime_priority_; // Whether we've published the single controller warning yet geometry_msgs::msg::PoseStamped end_pose_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 0386c06fc7d..ea513d1eb2b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -62,6 +62,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit")); declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0)); + declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false)); // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -126,6 +127,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) std::string speed_limit_topic; get_parameter("speed_limit_topic", speed_limit_topic); get_parameter("failure_tolerance", failure_tolerance_); + get_parameter("use_realtime_priority", use_realtime_priority_); costmap_ros_->configure(); // Launch a thread to run the costmap node @@ -221,13 +223,19 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); // Create the action server that we implement with our followPath method - action_server_ = std::make_unique( - shared_from_this(), - "follow_path", - std::bind(&ControllerServer::computeControl, this), - nullptr, - std::chrono::milliseconds(500), - true, server_options); + // This may throw due to real-time prioritzation if user doesn't have real-time permissions + try { + action_server_ = std::make_unique( + shared_from_this(), + "follow_path", + std::bind(&ControllerServer::computeControl, this), + nullptr, + std::chrono::milliseconds(500), + true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } // Set subscribtion to the speed limiting topic speed_limit_sub_ = create_subscription( diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp index 78d84fb8dbf..e0c16927927 100644 --- a/nav2_util/include/nav2_util/node_thread.hpp +++ b/nav2_util/include/nav2_util/node_thread.hpp @@ -32,13 +32,15 @@ class NodeThread * @brief A background thread to process node callbacks constructor * @param node_base Interface to Node to spin in thread */ - explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); + explicit NodeThread( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); /** * @brief A background thread to process executor's callbacks constructor * @param executor Interface to executor to spin in thread */ - explicit NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); + explicit NodeThread( + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); /** * @brief A background thread to process node callbacks constructor diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index f05a4602acf..fac67989bfa 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -57,6 +57,8 @@ class SimpleActionServer * @param server_timeout Timeout to to react to stop or preemption requests * @param spin_thread Whether to spin with a dedicated thread internally * @param options Options to pass to the underlying rcl_action_server_t + * @param realtime Whether the action server's worker thread should have elevated + * prioritization (soft realtime) */ template explicit SimpleActionServer( @@ -66,13 +68,15 @@ class SimpleActionServer CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), bool spin_thread = false, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + const bool realtime = false) : SimpleActionServer( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), - action_name, execute_callback, completion_callback, server_timeout, spin_thread, options) + action_name, execute_callback, completion_callback, + server_timeout, spin_thread, options, realtime) {} /** @@ -83,6 +87,8 @@ class SimpleActionServer * @param server_timeout Timeout to to react to stop or preemption requests * @param spin_thread Whether to spin with a dedicated thread internally * @param options Options to pass to the underlying rcl_action_server_t + * @param realtime Whether the action server's worker thread should have elevated + * prioritization (soft realtime) */ explicit SimpleActionServer( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, @@ -94,7 +100,8 @@ class SimpleActionServer CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), bool spin_thread = false, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + const bool realtime = false) : node_base_interface_(node_base_interface), node_clock_interface_(node_clock_interface), node_logging_interface_(node_logging_interface), @@ -106,6 +113,7 @@ class SimpleActionServer spin_thread_(spin_thread) { using namespace std::placeholders; // NOLINT + use_realtime_prioritization_ = realtime; if (spin_thread_) { callback_group_ = node_base_interface->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -170,6 +178,26 @@ class SimpleActionServer return rclcpp_action::CancelResponse::ACCEPT; } + /** + * @brief Sets thread priority level + */ + void setSoftRealTimePriority() + { + if (use_realtime_prioritization_) { + sched_param sch; + sch.sched_priority = 49; + if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { + std::string errmsg( + "Cannot set as real-time thread. Users must set: hard rtprio 99 and " + " soft rtprio 99 in /etc/security/limits.conf to enable " + "realtime prioritization! Error: "); + throw std::runtime_error(errmsg + std::strerror(errno)); + } else { + debug_msg("Soft realtime prioritization successfully set!"); + } + } + } + /** * @brief Handles accepted goals and adds to preempted queue to switch to * @param Goal A server goal handle to cancel @@ -202,7 +230,11 @@ class SimpleActionServer // Return quickly to avoid blocking the executor, so spin up a new thread debug_msg("Executing goal asynchronously."); - execution_future_ = std::async(std::launch::async, [this]() {work();}); + execution_future_ = std::async( + std::launch::async, [this]() { + setSoftRealTimePriority(); + work(); + }); } } @@ -509,6 +541,7 @@ class SimpleActionServer CompletionCallback completion_callback_; std::future execution_future_; bool stop_execution_{false}; + bool use_realtime_prioritization_{false}; mutable std::recursive_mutex update_mutex_; bool server_active_{false}; diff --git a/nav2_util/src/node_thread.cpp b/nav2_util/src/node_thread.cpp index c19fbd98477..adcbe50b780 100644 --- a/nav2_util/src/node_thread.cpp +++ b/nav2_util/src/node_thread.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include - #include "nav2_util/node_thread.hpp" namespace nav2_util { -NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) +NodeThread::NodeThread( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) : node_(node_base) { executor_ = std::make_shared(); @@ -35,7 +35,10 @@ NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nod NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor) : executor_(executor) { - thread_ = std::make_unique([&]() {executor_->spin();}); + thread_ = std::make_unique( + [&]() { + executor_->spin(); + }); } NodeThread::~NodeThread() From 4a36697520679a63536506fcc8380b1d59086bf3 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 20 Nov 2023 15:22:48 -0800 Subject: [PATCH 06/38] Update PULL_REQUEST_TEMPLATE.md (#3977) --- .github/PULL_REQUEST_TEMPLATE.md | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 630c2a02265..c1cfa2787f7 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -9,6 +9,7 @@ | Ticket(s) this addresses | (add tickets here #1) | | Primary OS tested on | (Ubuntu, MacOS, Windows) | | Robotic platform tested on | (Steve's Robot, gazebo simulation of Tally, hardware turtlebot) | +| Does this PR contain AI generated software? | (No; Yes and it is marked inline in the code) | --- From 4026c8fe00c96f0fe5d4e2b1dde91885009fa918 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Tue, 21 Nov 2023 11:20:47 -0500 Subject: [PATCH 07/38] fix a few outdated comments in smac planners (#3978) --- .../include/nav2_smac_planner/a_star.hpp | 13 +++++-------- .../include/nav2_smac_planner/node_basic.hpp | 1 - .../include/nav2_smac_planner/node_lattice.hpp | 2 -- 3 files changed, 5 insertions(+), 11 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 31dae78968f..650d8d4adff 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -72,8 +72,7 @@ class AStarAlgorithm typedef std::priority_queue, NodeComparator> NodeQueue; /** - * @brief A constructor for nav2_smac_planner::PlannerServer - * @param neighborhood The type of neighborhood to use for search (4 or 8 connected) + * @brief A constructor for nav2_smac_planner::AStarAlgorithm */ explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); @@ -196,7 +195,7 @@ class AStarAlgorithm inline NodePtr getNextNode(); /** - * @brief Get pointer to next goal in open set + * @brief Add a node to the open set * @param cost The cost to sort into the open set of the node * @param node Node pointer reference to add to open set */ @@ -204,8 +203,7 @@ class AStarAlgorithm /** * @brief Adds node to graph - * @param cost The cost to sort into the open set of the node - * @param node Node pointer reference to add to open set + * @param index Node index to add */ inline NodePtr addToGraph(const unsigned int & index); @@ -218,9 +216,8 @@ class AStarAlgorithm /** * @brief Get cost of heuristic of node - * @param node Node index current - * @param node Node index of new - * @return Heuristic cost between the nodes + * @param node Node pointer to get heuristic for + * @return Heuristic cost for node */ inline float getHeuristicCost(const NodePtr & node); diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index eb3680ec1c2..0fbace43604 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -47,7 +47,6 @@ class NodeBasic public: /** * @brief A constructor for nav2_smac_planner::NodeBasic - * @param cost_in The costmap cost at this node * @param index The index of this node for self-reference */ explicit NodeBasic(const unsigned int index) diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 7f6a82d31c7..4fe6a284cf4 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -58,8 +58,6 @@ struct LatticeMotionTable /** * @brief Initializing state lattice planner's motion model * @param size_x_in Size of costmap in X - * @param size_y_in Size of costmap in Y - * @param angle_quantization_in Size of costmap in bin sizes * @param search_info Parameters for searching */ void initMotionModel( From f94f69ecf713df96938e245ae8a4a1fd95e2030b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 21 Nov 2023 14:09:03 -0800 Subject: [PATCH 08/38] adding soft realtime prioritization for collision monitor and velocity smoother (#3979) * adding soft realtime prioritization for collision monitor and velocity smoother * refactor simple action server to use new utils API --- .../src/collision_monitor_node.cpp | 14 ++++++++++++++ nav2_util/include/nav2_util/node_utils.hpp | 8 ++++++++ .../include/nav2_util/simple_action_server.hpp | 14 +++----------- nav2_util/src/node_utils.cpp | 14 ++++++++++++++ nav2_velocity_smoother/src/velocity_smoother.cpp | 12 ++++++++++++ 5 files changed, 51 insertions(+), 11 deletions(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 481cafbb654..29e3f3031de 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -77,6 +77,20 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) collision_points_marker_pub_ = this->create_publisher( "~/collision_points_marker", 1); + auto node = shared_from_this(); + nav2_util::declare_parameter_if_not_declared( + node, "use_realtime_priority", rclcpp::ParameterValue(false)); + bool use_realtime_priority = false; + node->get_parameter("use_realtime_priority", use_realtime_priority); + if (use_realtime_priority) { + try { + nav2_util::setSoftRealTimePriority(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } + } + return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index d0bb65cf41e..a6a511a89c5 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// 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. @@ -171,6 +172,13 @@ void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child) } } +/** + * @brief Sets the caller thread to have a soft-realtime prioritization by + * increasing the priority level of the host thread. + * May throw exception if unable to set prioritization successfully + */ +void setSoftRealTimePriority(); + } // namespace nav2_util #endif // NAV2_UTIL__NODE_UTILS_HPP_ diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index fac67989bfa..c74dcf55f29 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -25,6 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_util/node_thread.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_util { @@ -184,17 +185,8 @@ class SimpleActionServer void setSoftRealTimePriority() { if (use_realtime_prioritization_) { - sched_param sch; - sch.sched_priority = 49; - if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { - std::string errmsg( - "Cannot set as real-time thread. Users must set: hard rtprio 99 and " - " soft rtprio 99 in /etc/security/limits.conf to enable " - "realtime prioritization! Error: "); - throw std::runtime_error(errmsg + std::strerror(errno)); - } else { - debug_msg("Soft realtime prioritization successfully set!"); - } + nav2_util::setSoftRealTimePriority(); + debug_msg("Soft realtime prioritization successfully set!"); } } diff --git a/nav2_util/src/node_utils.cpp b/nav2_util/src/node_utils.cpp index e5415b81adc..993eaf53b6e 100644 --- a/nav2_util/src/node_utils.cpp +++ b/nav2_util/src/node_utils.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// 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. @@ -89,4 +90,17 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix) return rclcpp::Node::make_shared("_", options); } +void setSoftRealTimePriority() +{ + sched_param sch; + sch.sched_priority = 49; + if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { + std::string errmsg( + "Cannot set as real-time thread. Users must set: hard rtprio 99 and " + " soft rtprio 99 in /etc/security/limits.conf to enable " + "realtime prioritization! Error: "); + throw std::runtime_error(errmsg + std::strerror(errno)); + } +} + } // namespace nav2_util diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 1c86659aae4..d3dd5c2a997 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -133,6 +133,18 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) "cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1)); + declare_parameter_if_not_declared(node, "use_realtime_priority", rclcpp::ParameterValue(false)); + bool use_realtime_priority = false; + node->get_parameter("use_realtime_priority", use_realtime_priority); + if (use_realtime_priority) { + try { + nav2_util::setSoftRealTimePriority(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } + } + return nav2_util::CallbackReturn::SUCCESS; } From 6622118fc07a3e4804f44cc176bdccc8cbc1aed8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 21 Nov 2023 15:19:48 -0800 Subject: [PATCH 09/38] removing API with rclcpp new version (#3981) --- .../bt_action_server_impl.hpp | 2 +- nav2_util/include/nav2_util/node_utils.hpp | 19 ----------- nav2_util/test/test_node_utils.cpp | 32 ------------------- 3 files changed, 1 insertion(+), 52 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index cd089692b87..49574999c40 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -135,7 +135,7 @@ bool BtActionServer::on_configure() node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); nav2_util::declare_parameter_if_not_declared( node, "transform_tolerance", rclcpp::ParameterValue(0.1)); - nav2_util::copy_all_parameters(node, client_node_); + rclcpp::copy_all_parameter_values(node, client_node_); // set the timeout in seconds for the action server to discard goal handles if not finished double action_server_result_timeout; diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index a6a511a89c5..99570224add 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -153,25 +153,6 @@ std::string get_plugin_type_param( return plugin_type; } -/** - * @brief A method to copy all parameters from one node (parent) to another (child). - * May throw parameter exceptions in error conditions - * @param parent Node to copy parameters from - * @param child Node to copy parameters to - */ -template -void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child) -{ - using Parameters = std::vector; - std::vector param_names = parent->list_parameters({}, 0).names; - Parameters params = parent->get_parameters(param_names); - for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) { - if (!child->has_parameter(iter->get_name())) { - child->declare_parameter(iter->get_name(), iter->get_parameter_value()); - } - } -} - /** * @brief Sets the caller thread to have a soft-realtime prioritization by * increasing the priority level of the host thread. diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index ff85d2bd415..bf945276358 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -94,35 +94,3 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error); } - -TEST(TestParamCopying, TestParamCopying) -{ - auto node1 = std::make_shared("test_node1"); - auto node2 = std::make_shared("test_node2"); - - // Tests for (1) multiple types, (2) recursion, (3) overriding values - node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); - node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); - node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); - node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); - node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); - - // Show Node2 is empty of Node1's parameters, but contains its own - EXPECT_FALSE(node2->has_parameter("Foo1")); - EXPECT_FALSE(node2->has_parameter("Foo2")); - EXPECT_FALSE(node2->has_parameter("Foo.bar")); - EXPECT_TRUE(node2->has_parameter("Foo")); - EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); - - nav2_util::copy_all_parameters(node1, node2); - - // Test new parameters exist, of expected value, and original param is not overridden - EXPECT_TRUE(node2->has_parameter("Foo1")); - EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); - EXPECT_TRUE(node2->has_parameter("Foo2")); - EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); - EXPECT_TRUE(node2->has_parameter("Foo.bar")); - EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); - EXPECT_TRUE(node2->has_parameter("Foo")); - EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); -} From 4bc3e8a534005e983ee7a49a09ec00aeb9820661 Mon Sep 17 00:00:00 2001 From: Max Kirchhoff <151524065+IPA-MKI@users.noreply.github.com> Date: Wed, 22 Nov 2023 17:42:09 +0100 Subject: [PATCH 10/38] Move isValidVelocity back to kinematic_parameters (#3973) * fix(xy_theta_iterator): isValidSpeed should check function params only * feat(kinematic_params): move isValidSpeed() back to kinematic_params as it was in ROS1. --- .../dwb_plugins/kinematic_parameters.hpp | 37 +++++++++++++++++-- .../include/dwb_plugins/xy_theta_iterator.hpp | 12 ------ .../dwb_plugins/src/xy_theta_iterator.cpp | 25 +------------ 3 files changed, 36 insertions(+), 38 deletions(-) diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index ed3cabc3d00..bfed0c2ec54 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -35,12 +35,13 @@ #ifndef DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_ #define DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_ +#include #include #include #include -#include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" namespace dwb_plugins { @@ -75,6 +76,36 @@ struct KinematicParameters inline double getMinSpeedXY_SQ() {return min_speed_xy_sq_;} inline double getMaxSpeedXY_SQ() {return max_speed_xy_sq_;} + /** + * @brief Check to see whether the combined x/y/theta velocities are valid + * @return True if the magnitude hypot(x,y) and theta are within the robot's + * absolute limits + * + * This is based on three parameters: min_speed_xy, max_speed_xy and + * min_speed_theta. The speed is valid if 1) The combined magnitude hypot(x,y) + * is less than max_speed_xy (or max_speed_xy is negative) AND 2) min_speed_xy + * is negative or min_speed_theta is negative or hypot(x,y) is greater than + * min_speed_xy or fabs(theta) is greater than min_speed_theta. + */ + inline bool isValidSpeed(double x, double y, double theta) + { + double vmag_sq = x * x + y * y; + if (max_speed_xy_ >= 0.0 && + vmag_sq > max_speed_xy_sq_ + std::numeric_limits::epsilon()) + { + return false; + } + if ( + min_speed_xy_ >= 0.0 && vmag_sq + std::numeric_limits::epsilon() < min_speed_xy_sq_ && + min_speed_theta_ >= 0.0 && + fabs(theta) + std::numeric_limits::epsilon() < min_speed_theta_) + { + return false; + } + if (vmag_sq == 0.0 && theta == 0.0) {return false;} + return true; + } + protected: // For parameter descriptions, see cfg/KinematicParams.cfg double min_vel_x_{0}; @@ -127,8 +158,8 @@ class KinematicsHandler * @brief Callback executed when a paramter change is detected * @param parameters list of changed parameters */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); void update_kinematics(KinematicParameters kinematics); std::string plugin_name_; }; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp index 88c18046ed5..68ac9405c2f 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp @@ -58,18 +58,6 @@ class XYThetaIterator : public VelocityIterator nav_2d_msgs::msg::Twist2D nextTwist() override; protected: - /** - * @brief Check to see whether the combined x/y/theta velocities are valid - * @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits - * - * This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta. - * The speed is valid if - * 1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative) - * AND - * 2) min_speed_xy is negative or min_speed_theta is negative or - * hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta. - */ - bool isValidSpeed(double x, double y, double theta); virtual bool isValidVelocity(); void iterateToValidVelocity(); int vx_samples_, vy_samples_, vtheta_samples_; diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp index 50b19aa337c..5ca1992c6bc 100644 --- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp @@ -41,8 +41,6 @@ #include "nav_2d_utils/parameters.hpp" #include "nav2_util/node_utils.hpp" -#define EPSILON 1E-5 - namespace dwb_plugins { void XYThetaIterator::initialize( @@ -92,29 +90,10 @@ void XYThetaIterator::startNewIteration( } } -bool XYThetaIterator::isValidSpeed(double x, double y, double theta) -{ - KinematicParameters kinematics = kinematics_handler_->getKinematics(); - double vmag_sq = x * x + y * y; - if (kinematics.getMaxSpeedXY() >= 0.0 && vmag_sq > kinematics.getMaxSpeedXY_SQ() + EPSILON) { - return false; - } - if (kinematics.getMinSpeedXY() >= 0.0 && vmag_sq + EPSILON < kinematics.getMinSpeedXY_SQ() && - kinematics.getMinSpeedTheta() >= 0.0 && fabs(theta) + EPSILON < kinematics.getMinSpeedTheta()) - { - return false; - } - if (vmag_sq == 0.0 && th_it_->getVelocity() == 0.0) { - return false; - } - return true; -} - bool XYThetaIterator::isValidVelocity() { - return isValidSpeed( - x_it_->getVelocity(), y_it_->getVelocity(), - th_it_->getVelocity()); + return kinematics_handler_->getKinematics().isValidSpeed( + x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity()); } bool XYThetaIterator::hasMoreTwists() From 6d2278e42dafb7c421f9c6255a3856882a9dea82 Mon Sep 17 00:00:00 2001 From: magda_sk Date: Mon, 27 Nov 2023 23:40:38 +0100 Subject: [PATCH 11/38] 3732 costmap raw update msgs rebased (fixes github issues with PR #3948) (#3965) * Adding CostmapUpdate msg * CostmapRawUpdate Publisher * remove test logs and typos * change data type to uint8 * pass unique ptr to raw costmap update publisher * adding subsriber for updates * -Werror=type-limits for update lower bounds checking * copy update msg * change code formatting * adding lock guards on costmap in footprint_collision_checker.cpp * adding lock guards for costmap subscriber clients * review suggestions implementation * fixing gtests errors * changes after second round of review * Update nav2_msgs/msg/CostmapUpdate.msg Co-authored-by: Steve Macenski * Integration test for costmap subscriber draft * Change the current grid parameters in separate method * WIP int test * adding method in PublisherCostmap2D for OccupancyGridUpdate population * test full costmap and updates while generating results * Integration tests for subscribers * Expected number of msgs related to number of mapchanges in tests * next round of reviews * refactor names of Costmap2DPublisher * remove unnecessary costmap_received_ flag form CostmapSubscriber --------- Co-authored-by: Steve Macenski --- .../src/constrained_smoother.cpp | 1 + .../test/test_constrained_smoother.cpp | 7 +- .../nav2_costmap_2d/costmap_2d_publisher.hpp | 16 +- .../nav2_costmap_2d/costmap_subscriber.hpp | 28 ++- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 107 ++++++---- nav2_costmap_2d/src/costmap_subscriber.cpp | 129 +++++++++--- .../test/integration/CMakeLists.txt | 21 ++ .../integration/test_costmap_subscriber.cpp | 184 ++++++++++++++++++ .../test_costmap_topic_collision_checker.cpp | 7 +- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/CostmapUpdate.msg | 11 ++ nav2_smoother/src/simple_smoother.cpp | 2 + nav2_smoother/test/test_smoother_server.cpp | 7 +- 13 files changed, 442 insertions(+), 79 deletions(-) create mode 100644 nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp create mode 100644 nav2_msgs/msg/CostmapUpdate.msg diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 305f4141f3c..406ca377c3f 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -132,6 +132,7 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat // Smooth plan auto costmap = costmap_sub_->getCostmap(); + std::lock_guard lock(*(costmap->getMutex())); if (!smoother_->smooth(path_world, start_dir, end_dir, costmap.get(), smoother_params_)) { RCLCPP_WARN( logger_, diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 1ccae77f774..6687b3452da 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -82,7 +82,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 2074ad5ea60..08b09504e52 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -48,6 +48,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "map_msgs/msg/occupancy_grid_update.hpp" #include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/msg/costmap_update.hpp" #include "nav2_msgs/srv/get_costmap.hpp" #include "tf2/transform_datatypes.h" #include "nav2_util/lifecycle_node.hpp" @@ -91,6 +92,7 @@ class Costmap2DPublisher costmap_pub_->on_activate(); costmap_update_pub_->on_activate(); costmap_raw_pub_->on_activate(); + costmap_raw_update_pub_->on_activate(); } /** @@ -101,6 +103,7 @@ class Costmap2DPublisher costmap_pub_->on_deactivate(); costmap_update_pub_->on_deactivate(); costmap_raw_pub_->on_deactivate(); + costmap_raw_update_pub_->on_deactivate(); } /** @@ -136,9 +139,16 @@ class Costmap2DPublisher void prepareGrid(); void prepareCostmap(); + /** @brief Prepare OccupancyGridUpdate msg for publication. */ + std::unique_ptr createGridUpdateMsg(); + /** @brief Prepare CostmapUpdate msg for publication. */ + std::unique_ptr createCostmapUpdateMsg(); + /** @brief Publish the latest full costmap to the new subscriber. */ // void onNewSubscription(const ros::SingleSubscriberPublisher& pub); + void updateGridParams(); + /** @brief GetCostmap callback service */ void costmap_service_callback( const std::shared_ptr request_header, @@ -164,12 +174,14 @@ class Costmap2DPublisher // Publisher for raw costmap values as msg::Costmap from layered costmap rclcpp_lifecycle::LifecyclePublisher::SharedPtr costmap_raw_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + costmap_raw_update_pub_; // Service for getting the costmaps rclcpp::Service::SharedPtr costmap_service_; - float grid_resolution; - unsigned int grid_width, grid_height; + float grid_resolution_; + unsigned int grid_width_, grid_height_; std::unique_ptr grid_; std::unique_ptr costmap_raw_; // Translate from 0-255 values in costmap to -1 to 100 values in message. diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index e4ea745015d..4f50614876c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/msg/costmap_update.hpp" #include "nav2_util/lifecycle_node.hpp" namespace nav2_costmap_2d @@ -52,25 +53,36 @@ class CostmapSubscriber ~CostmapSubscriber() {} /** - * @brief A Get the costmap from topic + * @brief Get current costmap */ std::shared_ptr getCostmap(); - - /** - * @brief Convert an occ grid message into a costmap object - */ - void toCostmap2D(); /** * @brief Callback for the costmap topic */ void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg); + /** + * @brief Callback for the costmap's update topic + */ + void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg); protected: + bool isCostmapReceived() {return costmap_ != nullptr;} + void processCurrentCostmapMsg(); + + bool haveCostmapParametersChanged(); + bool hasCostmapSizeChanged(); + bool hasCostmapResolutionChanged(); + bool hasCostmapOriginPositionChanged(); + + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr costmap_update_sub_; + std::shared_ptr costmap_; nav2_msgs::msg::Costmap::SharedPtr costmap_msg_; + std::string topic_name_; - bool costmap_received_{false}; - rclcpp::Subscription::SharedPtr costmap_sub_; + std::mutex costmap_msg_mutex_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 87881bc7caa..58c7d43d567 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -76,6 +76,8 @@ Costmap2DPublisher::Costmap2DPublisher( custom_qos); costmap_update_pub_ = node->create_publisher( topic_name + "_updates", custom_qos); + costmap_raw_update_pub_ = node->create_publisher( + topic_name + "_raw_updates", custom_qos); // Create a service that will use the callback function to handle requests. costmap_service_ = node->create_service( @@ -115,32 +117,37 @@ void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub.publish(grid_); } */ + +void Costmap2DPublisher::updateGridParams() +{ + saved_origin_x_ = costmap_->getOriginX(); + saved_origin_y_ = costmap_->getOriginY(); + grid_resolution_ = costmap_->getResolution(); + grid_width_ = costmap_->getSizeInCellsX(); + grid_height_ = costmap_->getSizeInCellsY(); +} + // prepare grid_ message for publication. void Costmap2DPublisher::prepareGrid() { std::unique_lock lock(*(costmap_->getMutex())); - grid_resolution = costmap_->getResolution(); - grid_width = costmap_->getSizeInCellsX(); - grid_height = costmap_->getSizeInCellsY(); grid_ = std::make_unique(); grid_->header.frame_id = global_frame_; grid_->header.stamp = clock_->now(); - grid_->info.resolution = grid_resolution; + grid_->info.resolution = grid_resolution_; - grid_->info.width = grid_width; - grid_->info.height = grid_height; + grid_->info.width = grid_width_; + grid_->info.height = grid_height_; double wx, wy; costmap_->mapToWorld(0, 0, wx, wy); - grid_->info.origin.position.x = wx - grid_resolution / 2; - grid_->info.origin.position.y = wy - grid_resolution / 2; + grid_->info.origin.position.x = wx - grid_resolution_ / 2; + grid_->info.origin.position.y = wy - grid_resolution_ / 2; grid_->info.origin.position.z = 0.0; grid_->info.origin.orientation.w = 1.0; - saved_origin_x_ = costmap_->getOriginX(); - saved_origin_y_ = costmap_->getOriginY(); grid_->data.resize(grid_->info.width * grid_->info.height); @@ -181,44 +188,74 @@ void Costmap2DPublisher::prepareCostmap() } } -void Costmap2DPublisher::publishCostmap() +std::unique_ptr Costmap2DPublisher::createGridUpdateMsg() +{ + auto update = std::make_unique(); + + update->header.stamp = clock_->now(); + update->header.frame_id = global_frame_; + update->x = x0_; + update->y = y0_; + update->width = xn_ - x0_; + update->height = yn_ - y0_; + update->data.resize(update->width * update->height); + + std::uint32_t i = 0; + for (std::uint32_t y = y0_; y < yn_; y++) { + for (std::uint32_t x = x0_; x < xn_; x++) { + update->data[i++] = cost_translation_table_[costmap_->getCost(x, y)]; + } + } + return update; +} + +std::unique_ptr Costmap2DPublisher::createCostmapUpdateMsg() { - if (costmap_raw_pub_->get_subscription_count() > 0) { - prepareCostmap(); - costmap_raw_pub_->publish(std::move(costmap_raw_)); + auto msg = std::make_unique(); + + msg->header.stamp = clock_->now(); + msg->header.frame_id = global_frame_; + msg->x = x0_; + msg->y = y0_; + msg->size_x = xn_ - x0_; + msg->size_y = yn_ - y0_; + msg->data.resize(msg->size_x * msg->size_y); + + std::uint32_t i = 0; + for (std::uint32_t y = y0_; y < yn_; y++) { + for (std::uint32_t x = x0_; x < xn_; x++) { + msg->data[i++] = costmap_->getCost(x, y); + } } + return msg; +} +void Costmap2DPublisher::publishCostmap() +{ float resolution = costmap_->getResolution(); - if (always_send_full_costmap_ || grid_resolution != resolution || - grid_width != costmap_->getSizeInCellsX() || - grid_height != costmap_->getSizeInCellsY() || + if (always_send_full_costmap_ || grid_resolution_ != resolution || + grid_width_ != costmap_->getSizeInCellsX() || + grid_height_ != costmap_->getSizeInCellsY() || saved_origin_x_ != costmap_->getOriginX() || saved_origin_y_ != costmap_->getOriginY()) { + updateGridParams(); if (costmap_pub_->get_subscription_count() > 0) { prepareGrid(); costmap_pub_->publish(std::move(grid_)); } + if (costmap_raw_pub_->get_subscription_count() > 0) { + prepareCostmap(); + costmap_raw_pub_->publish(std::move(costmap_raw_)); + } } else if (x0_ < xn_) { + // Publish just update msgs + std::unique_lock lock(*(costmap_->getMutex())); if (costmap_update_pub_->get_subscription_count() > 0) { - std::unique_lock lock(*(costmap_->getMutex())); - // Publish Just an Update - auto update = std::make_unique(); - update->header.stamp = rclcpp::Time(); - update->header.frame_id = global_frame_; - update->x = x0_; - update->y = y0_; - update->width = xn_ - x0_; - update->height = yn_ - y0_; - update->data.resize(update->width * update->height); - unsigned int i = 0; - for (unsigned int y = y0_; y < yn_; y++) { - for (unsigned int x = x0_; x < xn_; x++) { - unsigned char cost = costmap_->getCost(x, y); - update->data[i++] = cost_translation_table_[cost]; - } - } - costmap_update_pub_->publish(std::move(update)); + costmap_update_pub_->publish(createGridUpdateMsg()); + } + if (costmap_raw_update_pub_->get_subscription_count() > 0) { + costmap_raw_update_pub_->publish(createCostmapUpdateMsg()); } } diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index d56ac942e98..0426b87be30 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -14,22 +14,30 @@ #include #include +#include #include "nav2_costmap_2d/costmap_subscriber.hpp" namespace nav2_costmap_2d { +constexpr int costmapUpdateQueueDepth = 10; + CostmapSubscriber::CostmapSubscriber( const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name) : topic_name_(topic_name) { auto node = parent.lock(); + logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); + costmap_update_sub_ = node->create_subscription( + topic_name_ + "_updates", + rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1)); } CostmapSubscriber::CostmapSubscriber( @@ -38,60 +46,119 @@ CostmapSubscriber::CostmapSubscriber( : topic_name_(topic_name) { auto node = parent.lock(); + logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); + costmap_update_sub_ = node->create_subscription( + topic_name_ + "_updates", + rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1)); } std::shared_ptr CostmapSubscriber::getCostmap() { - if (!costmap_received_) { + if (!isCostmapReceived()) { throw std::runtime_error("Costmap is not available"); } - toCostmap2D(); + if (costmap_msg_) { + processCurrentCostmapMsg(); + } return costmap_; } -void CostmapSubscriber::toCostmap2D() +void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) { - auto current_costmap_msg = std::atomic_load(&costmap_msg_); - - if (costmap_ == nullptr) { - costmap_ = std::make_shared( - current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, - current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x, - current_costmap_msg->metadata.origin.position.y); - } else if (costmap_->getSizeInCellsX() != current_costmap_msg->metadata.size_x || // NOLINT - costmap_->getSizeInCellsY() != current_costmap_msg->metadata.size_y || - costmap_->getResolution() != current_costmap_msg->metadata.resolution || - costmap_->getOriginX() != current_costmap_msg->metadata.origin.position.x || - costmap_->getOriginY() != current_costmap_msg->metadata.origin.position.y) { - // Update the size of the costmap - costmap_->resizeMap( - current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, - current_costmap_msg->metadata.resolution, - current_costmap_msg->metadata.origin.position.x, - current_costmap_msg->metadata.origin.position.y); + std::lock_guard lock(costmap_msg_mutex_); + costmap_msg_ = msg; } + if (!isCostmapReceived()) { + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); - unsigned char * master_array = costmap_->getCharMap(); - unsigned int index = 0; - for (unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i) { - for (unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j) { - master_array[index] = current_costmap_msg->data[index]; - ++index; + processCurrentCostmapMsg(); + } +} + +void CostmapSubscriber::costmapUpdateCallback( + const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg) +{ + if (isCostmapReceived()) { + if (costmap_msg_) { + processCurrentCostmapMsg(); + } + + std::lock_guard lock(*(costmap_->getMutex())); + + auto map_cell_size_x = costmap_->getSizeInCellsX(); + auto map_call_size_y = costmap_->getSizeInCellsY(); + + if (map_cell_size_x < update_msg->x + update_msg->size_x || + map_call_size_y < update_msg->y + update_msg->size_y) + { + RCLCPP_WARN( + logger_, "Update area outside of original map area. Costmap bounds: %d X %d, " + "Update origin: %d, %d bounds: %d X %d", map_cell_size_x, map_call_size_y, + update_msg->x, update_msg->y, update_msg->size_x, update_msg->size_y); + return; + } + unsigned char * master_array = costmap_->getCharMap(); + // copy update msg row-wise + for (size_t y = 0; y < update_msg->size_y; ++y) { + auto starting_index_of_row_update_in_costmap = (y + update_msg->y) * map_cell_size_x + + update_msg->x; + + std::copy_n( + update_msg->data.begin() + (y * update_msg->size_x), + update_msg->size_x, &master_array[starting_index_of_row_update_in_costmap]); } + } else { + RCLCPP_WARN(logger_, "No costmap received."); } } -void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) +void CostmapSubscriber::processCurrentCostmapMsg() { - std::atomic_store(&costmap_msg_, msg); - if (!costmap_received_) { - costmap_received_ = true; + std::scoped_lock lock(*(costmap_->getMutex()), costmap_msg_mutex_); + if (haveCostmapParametersChanged()) { + costmap_->resizeMap( + costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y, + costmap_msg_->metadata.resolution, + costmap_msg_->metadata.origin.position.x, + costmap_msg_->metadata.origin.position.y); } + + unsigned char * master_array = costmap_->getCharMap(); + std::copy(costmap_msg_->data.begin(), costmap_msg_->data.end(), master_array); + costmap_msg_.reset(); +} + +bool CostmapSubscriber::haveCostmapParametersChanged() +{ + return hasCostmapSizeChanged() || + hasCostmapResolutionChanged() || + hasCostmapOriginPositionChanged(); +} + +bool CostmapSubscriber::hasCostmapSizeChanged() +{ + return costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x || + costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y; +} + +bool CostmapSubscriber::hasCostmapResolutionChanged() +{ + return costmap_->getResolution() != costmap_msg_->metadata.resolution; +} + +bool CostmapSubscriber::hasCostmapOriginPositionChanged() +{ + return costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x || + costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y; } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 0768972a0bb..868b5081a1b 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -76,6 +76,17 @@ target_link_libraries(test_costmap_publisher_exec layers ) +ament_add_gtest_executable(test_costmap_subscriber_exec +test_costmap_subscriber.cpp +) +ament_target_dependencies(test_costmap_subscriber_exec + ${dependencies} +) +target_link_libraries(test_costmap_subscriber_exec + nav2_costmap_2d_core + nav2_costmap_2d_client +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -136,6 +147,16 @@ ament_add_test(test_costmap_publisher_exec TEST_EXECUTABLE=$ ) +ament_add_test(test_costmap_subscriber_exec + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback # ament_add_gtest_executable(costmap_tester diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp new file mode 100644 index 00000000000..65fa120cd60 --- /dev/null +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -0,0 +1,184 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; +class TestCostmapSubscriberShould : public ::testing::Test +{ +public: + TestCostmapSubscriberShould() + : topicName("/costmap"), node(nav2_util::LifecycleNode::make_shared("test_subscriber")) + { + dummyCostmapMsgSubscriber = node->create_subscription( + topicName + "_raw", 10, + std::bind(&TestCostmapSubscriberShould::costmapCallback, this, std::placeholders::_1)); + + dummyCostmapUpdateMsgSubscriber = + node->create_subscription( + topicName + "_raw_updates", 10, std::bind( + &TestCostmapSubscriberShould::costmapUpdateCallback, this, + std::placeholders::_1)); + } + + void SetUp() override + { + fullCostmapMsgCount = 0; + updateCostmapMsgCount = 0; + + costmapSubscriber = + std::make_unique(node, topicName + "_raw"); + + costmapToSend = std::make_shared(10, 10, 1.0, 0.0, 0.0); + } + + void TearDown() + { + costmapSubscriber.reset(); + costmapToSend.reset(); + } + + void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr) + { + this->fullCostmapMsgCount++; + } + void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr) + { + this->updateCostmapMsgCount++; + } + + struct CostmapObservation + { + std::uint32_t x; + std::uint32_t y; + std::uint8_t cost; + }; + + struct MapChange + { + std::vector observations; + std::uint32_t x0; + std::uint32_t xn; + std::uint32_t y0; + std::uint32_t yn; + }; + +/* *INDENT-OFF* */ + const std::vector mapChanges {{{{2, 2, 255}, {2, 3, 255}, {3, 2, 255}}, 2, 4, 2, 4}, + {{{7, 7, 255}, {7, 8, 255}}, 7, 8, 7, 9}, + {{{2, 2, 0}, {2, 3, 0}, {3, 2, 0}}, 2, 4, 2, 4}}; +/* *INDENT-ON* */ + +protected: + std::vector getCurrentCharMapFromSubscriber() + { + auto currentSubscriberCostmap = costmapSubscriber->getCostmap(); + return + std::vector( + currentSubscriberCostmap->getCharMap(), + currentSubscriberCostmap->getCharMap() + currentSubscriberCostmap->getSizeInCellsX() * + currentSubscriberCostmap->getSizeInCellsX()); + } + + std::vector getCurrentCharMapToSend() + { + return std::vector( + costmapToSend->getCharMap(), + costmapToSend->getCharMap() + costmapToSend->getSizeInCellsX() * + costmapToSend->getSizeInCellsX()); + } + + int fullCostmapMsgCount; + int updateCostmapMsgCount; + std::string topicName; + + nav2_util::LifecycleNode::SharedPtr node; + rclcpp::Logger logger {rclcpp::get_logger("test_costmap_subscriber_should")}; + + std::unique_ptr costmapSubscriber; + std::shared_ptr costmapToSend; + rclcpp::Subscription::SharedPtr dummyCostmapMsgSubscriber; + rclcpp::Subscription::SharedPtr dummyCostmapUpdateMsgSubscriber; +}; + +TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) +{ + bool always_send_full_costmap = true; + + std::vector> expectedCostmaps; + std::vector> recievedCostmaps; + + auto costmapPublisher = std::make_shared( + node, costmapToSend.get(), "", topicName, always_send_full_costmap); + costmapPublisher->on_activate(); + + for (const auto & mapChange :mapChanges) { + for (const auto & observation : mapChange.observations) { + costmapToSend->setCost(observation.x, observation.y, observation.cost); + } + + expectedCostmaps.emplace_back(getCurrentCharMapToSend()); + + costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); + costmapPublisher->publishCostmap(); + + rclcpp::spin_some(node->get_node_base_interface()); + + recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + } + + ASSERT_EQ(fullCostmapMsgCount, mapChanges.size()); + ASSERT_EQ(updateCostmapMsgCount, 0); + + ASSERT_EQ(expectedCostmaps, recievedCostmaps); + + costmapPublisher->on_deactivate(); +} + +TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) +{ + bool always_send_full_costmap = false; + + std::vector> expectedCostmaps; + std::vector> recievedCostmaps; + + auto costmapPublisher = std::make_shared( + node, costmapToSend.get(), "", topicName, always_send_full_costmap); + costmapPublisher->on_activate(); + + for (const auto & mapChange :mapChanges) { + for (const auto & observation : mapChange.observations) { + costmapToSend->setCost(observation.x, observation.y, observation.cost); + } + + expectedCostmaps.emplace_back(getCurrentCharMapToSend()); + + costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); + costmapPublisher->publishCostmap(); + + rclcpp::spin_some(node->get_node_base_interface()); + + recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + } + + ASSERT_EQ(fullCostmapMsgCount, 1); + ASSERT_EQ(updateCostmapMsgCount, mapChanges.size() - 1); + + ASSERT_EQ(expectedCostmaps, recievedCostmaps); + + costmapPublisher->on_deactivate(); +} + +TEST_F( + TestCostmapSubscriberShould, + throwExceptionIfGetCostmapMethodIsCalledBeforeAnyCostmapMsgReceived) +{ + ASSERT_ANY_THROW(costmapSubscriber->getCostmap()); +} diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 466fd015c87..ec6748f980e 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -63,7 +63,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index e588ff0fa5f..206ae86322b 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/CollisionDetectorState.msg" "msg/Costmap.msg" "msg/CostmapMetaData.msg" + "msg/CostmapUpdate.msg" "msg/CostmapFilterInfo.msg" "msg/SpeedLimit.msg" "msg/VoxelGrid.msg" diff --git a/nav2_msgs/msg/CostmapUpdate.msg b/nav2_msgs/msg/CostmapUpdate.msg new file mode 100644 index 00000000000..3c865d326ba --- /dev/null +++ b/nav2_msgs/msg/CostmapUpdate.msg @@ -0,0 +1,11 @@ +# Update msg for Costmap containing the modified part of Costmap +std_msgs/Header header + +uint32 x +uint32 y + +uint32 size_x +uint32 size_y + +# The cost data, in row-major order, starting with (x,y) from 0-255 in Costmap format rather than OccupancyGrid 0-100. +uint8[] data diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 29f3b89a828..0b494d96c6e 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -72,6 +72,8 @@ bool SimpleSmoother::smooth( std::vector path_segments = findDirectionalPathSegments(path); + std::lock_guard lock(*(costmap->getMutex())); + for (unsigned int i = 0; i != path_segments.size(); i++) { if (path_segments[i].end - path_segments[i].start > 9) { // Populate path segment diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 67472eb46bf..8fb1b6ad0df 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -146,7 +146,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; From c1e4a427b120da9aea40b78d34e71d68a53c5379 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Nov 2023 14:14:05 -0800 Subject: [PATCH 12/38] adding default collision monitor to bringup (#3989) * adding default collision monitor to bringup * Update nav2_multirobot_params_1.yaml * adding to system test files * adjust for test --- nav2_bringup/launch/navigation_launch.py | 25 +++++++++++++-- .../params/nav2_multirobot_params_1.yaml | 31 +++++++++++++++++++ .../params/nav2_multirobot_params_2.yaml | 31 +++++++++++++++++++ .../params/nav2_multirobot_params_all.yaml | 31 +++++++++++++++++++ nav2_bringup/params/nav2_params.yaml | 31 +++++++++++++++++++ .../src/costmap_filters/keepout_params.yaml | 31 +++++++++++++++++++ .../costmap_filters/speed_global_params.yaml | 31 +++++++++++++++++++ .../costmap_filters/speed_local_params.yaml | 31 +++++++++++++++++++ .../gps_navigation/nav2_no_map_params.yaml | 31 +++++++++++++++++++ 9 files changed, 270 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 2f438c66e03..36cf6384cd6 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -45,9 +45,10 @@ def generate_launch_description(): 'smoother_server', 'planner_server', 'behavior_server', + 'velocity_smoother', + 'collision_monitor', 'bt_navigator', 'waypoint_follower', - 'velocity_smoother', ] # Map fully qualified names to relative ones so the node's namespace can be prepended. @@ -198,7 +199,18 @@ def generate_launch_description(): parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], remappings=remappings - + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + + [('cmd_vel', 'cmd_vel_nav')], + ), + Node( + package='nav2_collision_monitor', + executable='collision_monitor', + name='collision_monitor', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, ), Node( package='nav2_lifecycle_manager', @@ -266,7 +278,14 @@ def generate_launch_description(): name='velocity_smoother', parameters=[configured_params], remappings=remappings - + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + + [('cmd_vel', 'cmd_vel_nav')], + ), + ComposableNode( + package='nav2_collision_monitor', + plugin='nav2_collision_monitor::CollisionMonitor', + name='collision_monitor', + parameters=[configured_params], + remappings=remappings, ), ComposableNode( package='nav2_lifecycle_manager', diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index d0bdce21db2..baceb0e531b 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -285,3 +285,34 @@ waypoint_follower: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/robot1/scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index cd51b1332e4..583bb64b562 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -284,3 +284,34 @@ waypoint_follower: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/robot2/scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index b6e5712092f..5fb4b9fe631 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -346,3 +346,34 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 356118f19db..39d72a5297c 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -343,3 +343,34 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 7667d3081f2..f5d79153ec0 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -307,3 +307,34 @@ filter_mask_server: frame_id: "map" topic_name: "/filter_mask" yaml_filename: "keepout_mask.yaml" + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 58b92e608ff..bf65fdf9ba5 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -298,3 +298,34 @@ filter_mask_server: frame_id: "map" topic_name: "/filter_mask" yaml_filename: "speed_mask.yaml" + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index a3be1396bcc..3799062cf87 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -298,3 +298,34 @@ filter_mask_server: frame_id: "map" topic_name: "/filter_mask" yaml_filename: "speed_mask.yaml" + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 7b3125afbae..b8bc078103b 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -306,3 +306,34 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_link" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True From ef85df2f0419db5f2aba68c685c16d034a2aa6b6 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 30 Nov 2023 13:45:42 -0800 Subject: [PATCH 13/38] Revert "adding base footprint publisher to nav2_util (#3860)" (#3994) This reverts commit cefce2cd0461aa999d075e7427b1bb8e4a0c6b19. --- .../nav2_util/base_footprint_publisher.hpp | 129 ------------------ nav2_util/src/CMakeLists.txt | 5 - nav2_util/src/base_footprint_publisher.cpp | 27 ---- nav2_util/test/CMakeLists.txt | 4 - .../test/test_base_footprint_publisher.cpp | 72 ---------- 5 files changed, 237 deletions(-) delete mode 100644 nav2_util/include/nav2_util/base_footprint_publisher.hpp delete mode 100644 nav2_util/src/base_footprint_publisher.cpp delete mode 100644 nav2_util/test/test_base_footprint_publisher.cpp diff --git a/nav2_util/include/nav2_util/base_footprint_publisher.hpp b/nav2_util/include/nav2_util/base_footprint_publisher.hpp deleted file mode 100644 index 52bcdb53eb0..00000000000 --- a/nav2_util/include/nav2_util/base_footprint_publisher.hpp +++ /dev/null @@ -1,129 +0,0 @@ -// Copyright (c) 2023 Open Navigation LLC -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ -#define NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "tf2_msgs/msg/tf_message.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/buffer.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" - -namespace nav2_util -{ - -/** - * @brief A TF2 listener that overrides the subscription callback - * to inject base footprint publisher removing Z, Pitch, and Roll for - * 3D state estimation but desiring a 2D frame for navigation, visualization, or other reasons - */ -class BaseFootprintPublisherListener : public tf2_ros::TransformListener -{ -public: - BaseFootprintPublisherListener(tf2::BufferCore & buffer, bool spin_thread, rclcpp::Node & node) - : tf2_ros::TransformListener(buffer, spin_thread) - { - node.declare_parameter( - "base_link_frame", rclcpp::ParameterValue(std::string("base_link"))); - node.declare_parameter( - "base_footprint_frame", rclcpp::ParameterValue(std::string("base_footprint"))); - base_link_frame_ = node.get_parameter("base_link_frame").as_string(); - base_footprint_frame_ = node.get_parameter("base_footprint_frame").as_string(); - tf_broadcaster_ = std::make_shared(node); - } - - /** - * @brief Overrides TF2 subscription callback to inject base footprint publisher - */ - void subscription_callback(tf2_msgs::msg::TFMessage::ConstSharedPtr msg, bool is_static) override - { - TransformListener::subscription_callback(msg, is_static); - - if (is_static) { - return; - } - - for (unsigned int i = 0; i != msg->transforms.size(); i++) { - auto & t = msg->transforms[i]; - if (t.child_frame_id == base_link_frame_) { - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = t.header.stamp; - transform.header.frame_id = base_link_frame_; - transform.child_frame_id = base_footprint_frame_; - - // Project to Z-zero - transform.transform.translation = t.transform.translation; - transform.transform.translation.z = 0.0; - - // Remove Roll and Pitch - tf2::Quaternion q; - q.setRPY(0, 0, tf2::getYaw(t.transform.rotation)); - q.normalize(); - transform.transform.rotation.x = q.x(); - transform.transform.rotation.y = q.y(); - transform.transform.rotation.z = q.z(); - transform.transform.rotation.w = q.w(); - - tf_broadcaster_->sendTransform(transform); - return; - } - } - } - -protected: - std::shared_ptr tf_broadcaster_; - std::string base_link_frame_, base_footprint_frame_; -}; - -/** - * @class nav2_util::BaseFootprintPublisher - * @brief Republishes the ``base_link`` frame as ``base_footprint`` - * stripping away the Z, Roll, and Pitch of the full 3D state to provide - * a 2D projection for navigation when state estimation is full 3D - */ -class BaseFootprintPublisher : public rclcpp::Node -{ -public: - /** - * @brief A constructor - */ - explicit BaseFootprintPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("base_footprint_publisher", options) - { - RCLCPP_INFO(get_logger(), "Creating base footprint publisher"); - tf_buffer_ = std::make_shared(get_clock()); - auto timer_interface = std::make_shared( - get_node_base_interface(), - get_node_timers_interface()); - tf_buffer_->setCreateTimerInterface(timer_interface); - listener_publisher_ = std::make_shared( - *tf_buffer_, true, *this); - } - -protected: - std::shared_ptr tf_buffer_; - std::shared_ptr listener_publisher_; -}; - -} // end namespace nav2_util - -#endif // NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index e378eac514d..a639a0e59e9 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -28,11 +28,6 @@ add_executable(lifecycle_bringup ) target_link_libraries(lifecycle_bringup ${library_name}) -add_executable(base_footprint_publisher - base_footprint_publisher.cpp -) -target_link_libraries(base_footprint_publisher ${library_name}) - find_package(Boost REQUIRED COMPONENTS program_options) install(TARGETS diff --git a/nav2_util/src/base_footprint_publisher.cpp b/nav2_util/src/base_footprint_publisher.cpp deleted file mode 100644 index f3b6791db44..00000000000 --- a/nav2_util/src/base_footprint_publisher.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// 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_util/base_footprint_publisher.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node->get_node_base_interface()); - rclcpp::shutdown(); - - return 0; -} diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index d68291f6195..9f1ae99bbcb 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,7 +41,3 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) - -ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp) -ament_target_dependencies(test_base_footprint_publisher geometry_msgs) -target_link_libraries(test_base_footprint_publisher ${library_name}) diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp deleted file mode 100644 index 47dc83c7f31..00000000000 --- a/nav2_util/test/test_base_footprint_publisher.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// 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 - -#include "nav2_util/base_footprint_publisher.hpp" -#include "gtest/gtest.h" -#include "tf2/exceptions.h" - -class RclCppFixture -{ -public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} -}; -RclCppFixture g_rclcppfixture; - -TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) -{ - auto node = std::make_shared(); - rclcpp::spin_some(node->get_node_base_interface()); - - auto tf_broadcaster = std::make_shared(node); - auto buffer = std::make_shared(node->get_clock()); - auto timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); - buffer->setCreateTimerInterface(timer_interface); - auto listener = std::make_shared(*buffer, true); - - std::string base_link = "base_link"; - std::string base_footprint = "base_footprint"; - - // Publish something to TF, should fail, doesn't exist - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = node->now(); - transform.header.frame_id = "test1_1"; - transform.child_frame_id = "test1"; - tf_broadcaster->sendTransform(transform); - rclcpp::spin_some(node->get_node_base_interface()); - EXPECT_THROW( - buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero), - tf2::TransformException); - - // This is valid, should work now and communicate the Z-removed info - transform.header.stamp = node->now(); - transform.header.frame_id = "odom"; - transform.child_frame_id = "base_link"; - transform.transform.translation.x = 1.0; - transform.transform.translation.y = 1.0; - transform.transform.translation.z = 1.0; - tf_broadcaster->sendTransform(transform); - rclcpp::Rate r(1.0); - r.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); - auto t = buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero); - EXPECT_EQ(t.transform.translation.x, 1.0); - EXPECT_EQ(t.transform.translation.y, 1.0); - EXPECT_EQ(t.transform.translation.z, 0.0); -} From aa9396e6c805bc787d8a7f3a46bfc16b1315939b Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Fri, 1 Dec 2023 13:49:21 +0800 Subject: [PATCH 14/38] fix bug mentioned in #3958 (#3972) * bug fixed * add space * Update planner_server.cpp * add space for code style * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add ChildLifecycleNode mode in costmap_2d_ros * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower * fit to NodeOption: is_lifecycle_follower * fit reorder Werror * fix wrong use of is_lifecycle_follower * remove blank line * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * Add files via upload * NodeOption: is_lifecycle_follower_ * NodeOption:is_lifecycle_follower_ * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * change default * add NodeOption for costmap_2d_ros * add node options for costmap2dros as an independent node * code style reformat * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp Co-authored-by: Steve Macenski * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp Co-authored-by: Steve Macenski * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp Co-authored-by: Steve Macenski * changes * comment changes * change get_parameter into =false * comment modification * missing line * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * delete last line * change lifecycle_test fit to NodeOption --------- Co-authored-by: GoesM Co-authored-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 14 +++------- .../nav2_costmap_2d/costmap_2d_ros.hpp | 27 ++++++++++++++++++- nav2_costmap_2d/src/costmap_2d_ros.cpp | 5 ++-- nav2_costmap_2d/test/unit/lifecycle_test.cpp | 2 +- nav2_planner/src/planner_server.cpp | 16 ++--------- 5 files changed, 36 insertions(+), 28 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ea513d1eb2b..76754c58151 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -290,11 +290,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); publishZeroVelocity(); vel_publisher_->on_deactivate(); @@ -320,11 +316,9 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) goal_checkers_.clear(); progress_checkers_.clear(); - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + + costmap_ros_->cleanup(); + // Release any allocated resources action_server_.reset(); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index f319e52f330..b1ddedceeb2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -75,8 +75,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode public: /** * @brief Constructor for the wrapper + * @param options Additional options to control creation of the node. */ - Costmap2DROS(); + Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Constructor for the wrapper, the node will @@ -134,6 +135,28 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief as a child-LifecycleNode : + * Costmap2DROS may be launched by another Lifecycle Node as a composed module + * If composed, its parents will handle the shutdown, which includes this module + */ + void on_rcl_preshutdown() override + { + if (is_lifecycle_follower_) { + // Transitioning handled by parent node + return; + } + + // Else, if this is an independent node, this node needs to handle itself. + RCLCPP_INFO( + get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)", + this->get_name()); + + runCleanups(); + + destroyBond(); + } + /** * @brief Subscribes to sensor topics if necessary and starts costmap * updates, can be called to restart the costmap after calls to either @@ -381,6 +404,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode double transform_tolerance_{0}; ///< The timeout before transform errors double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors + bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node + // Derived parameters bool use_radius_{false}; std::vector unpadded_footprint_; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index c35933c9083..bc887eeeb3b 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -61,8 +61,8 @@ namespace nav2_costmap_2d Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) : Costmap2DROS(name, "/", name, use_sim_time) {} -Costmap2DROS::Costmap2DROS() -: nav2_util::LifecycleNode("costmap", ""), +Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("costmap", "", options), name_("costmap"), default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, default_types_{ @@ -71,6 +71,7 @@ Costmap2DROS::Costmap2DROS() "nav2_costmap_2d::InflationLayer"} { declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + is_lifecycle_follower_ = false; init(); } diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp index 8a657d81ad1..a8ab817df3e 100644 --- a/nav2_costmap_2d/test/unit/lifecycle_test.cpp +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -22,7 +22,7 @@ TEST(LifecylceTest, CheckInitialTfTimeout) { rclcpp::init(0, nullptr); - auto costmap = std::make_shared("test_costmap"); + auto costmap = std::make_shared(rclcpp::NodeOptions()); costmap->set_parameter({"initial_transform_timeout", 0.0}); std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}}; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 84edc6e4df3..fea40ea4053 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -218,11 +218,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -247,15 +243,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) plan_publisher_.reset(); tf_.reset(); - /* - * Double check whether something else transitioned it to INACTIVE - * already, e.g. the rcl preshutdown callback. - */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + costmap_ros_->cleanup(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { From 15c9be0aa5d09dd3be40bb46a66a5175464dd1a4 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 5 Dec 2023 16:11:32 -0800 Subject: [PATCH 15/38] Convert all wall timers and wall rates to ROS clock respecting rates and timers (#4000) * Convert all wall timers and wall rates to ROS clock respecting rates and timers * linty mclint face * WPF wait plugin respect time * move duration metrics to use local clocks * bumping version for cache to break it * complete timing refactor * remove old variable --- .circleci/config.yml | 6 +++--- .../nav2_behavior_tree/behavior_tree_engine.hpp | 4 +++- nav2_behavior_tree/src/behavior_tree_engine.cpp | 3 ++- .../nav2_behaviors/plugins/drive_on_heading.hpp | 4 ++-- .../include/nav2_behaviors/timed_behavior.hpp | 14 +++++++------- nav2_behaviors/plugins/assisted_teleop.cpp | 8 ++++---- nav2_behaviors/plugins/back_up.cpp | 2 +- nav2_behaviors/plugins/spin.cpp | 4 ++-- .../include/nav2_planner/planner_server.hpp | 3 --- nav2_planner/src/planner_server.cpp | 8 ++++---- .../include/nav2_smoother/nav2_smoother.hpp | 2 -- nav2_smoother/src/nav2_smoother.cpp | 4 ++-- .../behaviors/backup/backup_behavior_tester.cpp | 4 +++- .../backup/test_backup_behavior_launch.py | 2 +- .../drive_on_heading_behavior_tester.cpp | 4 +++- .../behaviors/spin/test_spin_behavior_launch.py | 2 +- .../behaviors/wait/test_wait_behavior_launch.py | 2 +- .../src/behaviors/wait/wait_behavior_tester.cpp | 1 - nav2_velocity_smoother/src/velocity_smoother.cpp | 4 ++-- .../plugins/wait_at_waypoint.hpp | 1 + .../plugins/wait_at_waypoint.cpp | 3 ++- 21 files changed, 44 insertions(+), 41 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 9d3e0f490d8..ab6312a3681 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v16\ + - "<< parameters.key >>-v17\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v16\ + - "<< parameters.key >>-v17\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v16\ + key: "<< parameters.key >>-v17\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 596fda97911..daf609d067c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -25,6 +25,7 @@ #include "behaviortree_cpp_v3/xml_parsing.h" #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "rclcpp/rclcpp.hpp" namespace nav2_behavior_tree { @@ -46,7 +47,8 @@ class BehaviorTreeEngine * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine * @param plugin_libraries vector of BT plugin library names to load */ - explicit BehaviorTreeEngine(const std::vector & plugin_libraries); + explicit BehaviorTreeEngine( + const std::vector & plugin_libraries); virtual ~BehaviorTreeEngine() {} /** diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 947778da5d6..429368ea96a 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -25,7 +25,8 @@ namespace nav2_behavior_tree { -BehaviorTreeEngine::BehaviorTreeEngine(const std::vector & plugin_libraries) +BehaviorTreeEngine::BehaviorTreeEngine( + const std::vector & plugin_libraries) { BT::SharedLibrary loader; for (const auto & p : plugin_libraries) { diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index de4aec7532e..cd0a1674285 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -76,7 +76,7 @@ class DriveOnHeading : public TimedBehavior command_speed_ = command->speed; command_time_allowance_ = command->time_allowance; - end_time_ = this->steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; if (!nav2_util::getCurrentPose( initial_pose_, *this->tf_, this->local_frame_, this->robot_base_frame_, @@ -95,7 +95,7 @@ class DriveOnHeading : public TimedBehavior */ ResultStatus onCycleUpdate() override { - rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { this->stopRobot(); RCLCPP_WARN( diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 6af6490b618..d01e14179ff 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -119,8 +119,8 @@ class TimedBehavior : public nav2_core::Behavior { node_ = parent; auto node = node_.lock(); - logger_ = node->get_logger(); + clock_ = node->get_clock(); RCLCPP_INFO(logger_, "Configuring %s", name.c_str()); @@ -200,7 +200,7 @@ class TimedBehavior : public nav2_core::Behavior rclcpp::Duration elasped_time_{0, 0}; // Clock - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + rclcpp::Clock::SharedPtr clock_; // Logger rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")}; @@ -231,11 +231,11 @@ class TimedBehavior : public nav2_core::Behavior return; } - auto start_time = steady_clock_.now(); + auto start_time = clock_->now(); rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { - elasped_time_ = steady_clock_.now() - start_time; + elasped_time_ = clock_->now() - start_time; if (action_server_->is_cancel_requested()) { RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); stopRobot(); @@ -252,7 +252,7 @@ class TimedBehavior : public nav2_core::Behavior " however feature is currently not implemented. Aborting and stopping.", behavior_name_.c_str()); stopRobot(); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; onActionCompletion(result); action_server_->terminate_current(result); return; @@ -264,14 +264,14 @@ class TimedBehavior : public nav2_core::Behavior RCLCPP_INFO( logger_, "%s completed successfully", behavior_name_.c_str()); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; onActionCompletion(result); action_server_->succeeded_current(result); return; case Status::FAILED: RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str()); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; result->error_code = on_cycle_update_result.error_code; onActionCompletion(result); action_server_->terminate_current(result); diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 4628010a3de..a0fc9729991 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -67,7 +67,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptrtime_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE}; } @@ -82,7 +82,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() feedback_->current_teleop_duration = elasped_time_; action_server_->publish_feedback(feedback_); - rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { stopRobot(); RCLCPP_WARN_STREAM( @@ -125,7 +125,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() if (time == simulation_time_step_) { RCLCPP_DEBUG_STREAM_THROTTLE( logger_, - steady_clock_, + *clock_, 1000, behavior_name_.c_str() << " collided on first time step, setting velocity to zero"); scaled_twist.linear.x = 0.0f; @@ -135,7 +135,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() } else { RCLCPP_DEBUG_STREAM_THROTTLE( logger_, - steady_clock_, + *clock_, 1000, behavior_name_.c_str() << " collision approaching in " << time << " seconds"); double scale_factor = time / projection_time_; diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index 1ec9e83264d..7a194874c7e 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -31,7 +31,7 @@ ResultStatus BackUp::onRun(const std::shared_ptr comma command_speed_ = -std::fabs(command->speed); command_time_allowance_ = command->time_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; if (!nav2_util::getCurrentPose( initial_pose_, *tf_, local_frame_, robot_base_frame_, diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 1e3b07fbf1c..55a7872247a 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -91,14 +91,14 @@ ResultStatus Spin::onRun(const std::shared_ptr command) cmd_yaw_); command_time_allowance_ = command->time_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE}; } ResultStatus Spin::onCycleUpdate() { - rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { stopRobot(); RCLCPP_WARN( diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index c54b9ae829b..2413e2eb3f0 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -241,9 +241,6 @@ class PlannerServer : public nav2_util::LifecycleNode double max_planner_duration_; std::string planner_ids_concat_; - // Clock - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; - // TF buffer std::shared_ptr tf_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index fea40ea4053..d087cbef758 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -361,7 +361,7 @@ void PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = steady_clock_.now(); + auto start_time = this->now(); // Initialize the ComputePathThroughPoses goal and result auto goal = action_server_poses_->get_current_goal(); @@ -421,7 +421,7 @@ void PlannerServer::computePlanThroughPoses() result->path = concat_path; publishPlan(result->path); - auto cycle_duration = steady_clock_.now() - start_time; + auto cycle_duration = this->now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { @@ -480,7 +480,7 @@ PlannerServer::computePlan() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = steady_clock_.now(); + auto start_time = this->now(); // Initialize the ComputePathToPose goal and result auto goal = action_server_pose_->get_current_goal(); @@ -517,7 +517,7 @@ PlannerServer::computePlan() // Publish the plan for visualization purposes publishPlan(result->path); - auto cycle_duration = steady_clock_.now() - start_time; + auto cycle_duration = this->now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index 35eaadb8614..fac9ffe3af6 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -166,8 +166,6 @@ class SmootherServer : public nav2_util::LifecycleNode std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; std::shared_ptr collision_checker_; - - rclcpp::Clock steady_clock_; }; } // namespace nav2_smoother diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 92d567a0573..4e7f7242deb 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -259,7 +259,7 @@ bool SmootherServer::findSmootherId( void SmootherServer::smoothPlan() { - auto start_time = steady_clock_.now(); + auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a path to smooth."); @@ -283,7 +283,7 @@ void SmootherServer::smoothPlan() result->was_completed = smoothers_[current_smoother_]->smooth( result->path, goal->max_smoothing_duration); - result->smoothing_duration = steady_clock_.now() - start_time; + result->smoothing_duration = this->now() - start_time; if (!result->was_completed) { RCLCPP_INFO( diff --git a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp index a270c8ab082..974c2d60b2a 100644 --- a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp @@ -35,7 +35,9 @@ BackupBehaviorTester::BackupBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("backup_behavior_test"); + rclcpp::NodeOptions options; + options.parameter_overrides({{"use_sim_time", true}}); + node_ = rclcpp::Node::make_shared("backup_behavior_test", options); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index 6f4a6c3c16e..bd4c5432fe8 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -102,7 +102,7 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_backup_behavior_node', output='screen' + cmd=[testExecutable], name='test_backup_behavior_node', output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp index 8db6ef6406e..967c6beb046 100644 --- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp @@ -35,7 +35,9 @@ DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test"); + rclcpp::NodeOptions options; + options.parameter_overrides({{"use_sim_time", true}}); + node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test", options); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index 4552f51768d..c9df3d32432 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -102,7 +102,7 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_spin_behavior_node', output='screen' + cmd=[testExecutable], name='test_spin_behavior_node', output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index f4c4e5bc168..80f15298209 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -102,7 +102,7 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_wait_behavior_node', output='screen' + cmd=[testExecutable], name='test_wait_behavior_node', output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp index ea6e58afb7d..dffd52f9115 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp @@ -164,7 +164,6 @@ bool WaitBehaviorTester::behaviorTest( RCLCPP_INFO(node_->get_logger(), "result received"); - if ((node_->now() - start_time).seconds() < static_cast(wait_time)) { return false; } diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index d3dd5c2a997..d2dad659130 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -154,7 +154,7 @@ VelocitySmoother::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Activating"); smoothed_cmd_pub_->on_activate(); double timer_duration_ms = 1000.0 / smoothing_frequency_; - timer_ = create_wall_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); @@ -362,7 +362,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } double timer_duration_ms = 1000.0 / smoothing_frequency_; - timer_ = create_wall_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); } else if (name == "velocity_timeout") { diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp index 911ae441a23..cbd27002464 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp @@ -73,6 +73,7 @@ class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor int waypoint_pause_duration_; bool is_enabled_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; + rclcpp::Clock::SharedPtr clock_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp index 917655e21a0..77e20bcf768 100644 --- a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp @@ -42,6 +42,7 @@ void WaitAtWaypoint::initialize( throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"}; } logger_ = node->get_logger(); + clock_ = node->get_clock(); nav2_util::declare_parameter_if_not_declared( node, plugin_name + ".waypoint_pause_duration", @@ -77,7 +78,7 @@ bool WaitAtWaypoint::processAtWaypoint( logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds", curr_waypoint_index, waypoint_pause_duration_); - rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); + clock_->sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); return true; } } // namespace nav2_waypoint_follower From 0df59c7ad97c3fd1d6f021654c15f605efbe04a4 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 6 Dec 2023 17:24:01 -0500 Subject: [PATCH 16/38] clear start cell for planning (#3980) * clear start cell for planning * clean up * add headers * added comment * start blocked * code review * code review --- nav2_navfn_planner/src/navfn_planner.cpp | 6 ------ .../include/nav2_smac_planner/a_star.hpp | 5 +++++ nav2_smac_planner/src/a_star.cpp | 20 +++++++++++++++---- nav2_smac_planner/src/smac_planner_2d.cpp | 8 +++++--- nav2_smac_planner/src/smac_planner_hybrid.cpp | 6 ++++++ .../src/smac_planner_lattice.cpp | 5 +++++ nav2_smac_planner/test/test_a_star.cpp | 4 ---- .../nav2_theta_star_planner/theta_star.hpp | 5 +++++ nav2_theta_star_planner/src/theta_star.cpp | 8 ++++++++ .../src/theta_star_planner.cpp | 7 +------ 10 files changed, 51 insertions(+), 23 deletions(-) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index c65119698a2..2ee8d69f53e 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -147,12 +147,6 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( std::to_string(goal.pose.position.y) + ") was outside bounds"); } - if (costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was in lethal cost"); - } - if (tolerance_ == 0 && costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav2_core::GoalOccupied( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 650d8d4adff..e4ecf38f035 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -245,6 +245,11 @@ class AStarAlgorithm inline void populateExpansionsLog( const NodePtr & node, std::vector> * expansions_log); + /** + * @brief Clear Start + */ + void clearStart(); + int _timing_interval = 5000; bool _traverse_unknown; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index d6c9db69bb3..55fe35e5311 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -234,10 +234,8 @@ bool AStarAlgorithm::areInputsValid() throw nav2_core::GoalOccupied("Goal was in lethal cost"); } - // Check if starting point is valid - if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::StartOccupied("Start was in lethal cost"); - } + // Note: We do not check the if the start is valid because it is cleared + clearStart(); return true; } @@ -465,6 +463,20 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } +template<> +void AStarAlgorithm::clearStart() +{ + auto coords = Node2D::getCoords(_start->getIndex()); + _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); +} + +template +void AStarAlgorithm::clearStart() +{ + auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); + _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 94b8ee1306e..77650a61f4f 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -240,9 +240,6 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Corner case of start and goal beeing on the same cell if (mx_start == mx_goal && my_start == my_goal) { - if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied("Start was in lethal cost"); - } pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal // orientation, unless use_final_approach_orientation=true where we need it to be the start @@ -262,6 +259,11 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) { + // Note: If the start is blocked only one iteration will occur before failure + if (num_iterations == 1) { + throw nav2_core::StartOccupied("Start occupied"); + } + if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index d623ab61107..fc016ac5457 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -399,6 +399,12 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } _expansions_publisher->publish(msg); } + + // Note: If the start is blocked only one iteration will occur before failure + if (num_iterations == 1) { + throw nav2_core::StartOccupied("Start occupied"); + } + if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 8b46fc3ae74..d3ddf5bdef2 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -287,6 +287,11 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( if (!_a_star->createPath( path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) { + // Note: If the start is blocked only one iteration will occur before failure + if (num_iterations == 1) { + throw nav2_core::StartOccupied("Start occupied"); + } + if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index b6107b141db..f048cc55503 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -92,10 +92,6 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setCollisionChecker(checker.get()); num_it = 0; EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); - a_star_2.setStart(50, 50, 0); // invalid - a_star_2.setGoal(0, 0, 0); // valid - num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); a_star_2.setStart(0, 0, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid num_it = 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 d1ddf7354ce..a58a4f87eda 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 @@ -116,6 +116,11 @@ class ThetaStar return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); } + /** + * @brief Clear Start + */ + void clearStart(); + int nodes_opened = 0; protected: diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 9432f867a45..f83b745b22e 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -258,4 +258,12 @@ void ThetaStar::initializePosn(int size_inc) node_position_.push_back(nullptr); } } + +void ThetaStar::clearStart() +{ + unsigned int mx_start = static_cast(src_.x); + unsigned int my_start = static_cast(src_.y); + costmap_->setCost(mx_start, my_start, nav2_costmap_2d::FREE_SPACE); +} + } // namespace theta_star diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index f93eaa44d73..ca9391111b3 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -111,12 +111,6 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( std::to_string(goal.pose.position.y) + ") was outside bounds"); } - if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was in lethal cost"); - } - if (planner_->costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav2_core::GoalOccupied( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + @@ -141,6 +135,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( return global_path; } + planner_->clearStart(); planner_->setStartAndGoal(start, goal); RCLCPP_DEBUG( logger_, "Got the src and dst... (%i, %i) && (%i, %i)", From 113564965f54009686d521902ff3fcc9d101c5b5 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 7 Dec 2023 13:53:15 -0800 Subject: [PATCH 17/38] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 1d5b83ab57b..e910e77b013 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,8 @@ For detailed instructions on how to: Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). +If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org. + ## Our Sponsors Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. From 04c6ca5eed4529757fcb9d4ca48a50e2a7ce023a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 13 Dec 2023 10:45:11 -0800 Subject: [PATCH 18/38] improving collision behavior in MPPI (#4009) * improving collision behavior in MPPI * update readme * fix test updates * adjust nav through poses for contextual error codes * contextual error codes to Nav Through Poses --- ...ould_a_planner_recovery_help_condition.hpp | 3 ++ ...ould_a_planner_recovery_help_condition.cpp | 5 ++- nav2_bringup/params/nav2_params.yaml | 6 +-- ...hrough_poses_w_replanning_and_recovery.xml | 40 ++++++++++++------- nav2_mppi_controller/README.md | 6 +-- .../src/critics/obstacles_critic.cpp | 17 ++++++-- .../src/critics/path_angle_critic.cpp | 4 +- nav2_mppi_controller/test/critics_tests.cpp | 6 +-- 8 files changed, 57 insertions(+), 30 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp index e53d6e4c351..729271a8c5a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp @@ -18,6 +18,7 @@ #include #include "nav2_msgs/action/compute_path_to_pose.hpp" +#include "nav2_msgs/action/compute_path_through_poses.hpp" #include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp" namespace nav2_behavior_tree @@ -27,6 +28,8 @@ class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent { using Action = nav2_msgs::action::ComputePathToPose; using ActionResult = Action::Result; + using ThroughAction = nav2_msgs::action::ComputePathThroughPoses; + using ThroughActionResult = Action::Result; public: WouldAPlannerRecoveryHelp( diff --git a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp index 6a8d3f33cad..91de9c2e22a 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp @@ -26,7 +26,10 @@ WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp( error_codes_to_check_ = { ActionResult::UNKNOWN, ActionResult::NO_VALID_PATH, - ActionResult::TIMEOUT + ActionResult::TIMEOUT, + ThroughActionResult::UNKNOWN, + ThroughActionResult::TIMEOUT, + ThroughActionResult::NO_VALID_PATH }; } diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 39d72a5297c..5f0e6e775d1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -199,7 +199,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.70 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -256,7 +256,7 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.7 always_send_full_costmap: True # The yaml_filename does not need to be specified since it going to be set by defaults in launch. @@ -362,7 +362,7 @@ collision_monitor: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 2.0 + time_before_collision: 1.2 simulation_time_step: 0.1 min_points: 6 visualize: False diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 9257836be88..ebbe3cbbe56 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -13,26 +13,38 @@ - + + + + - + + + + - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 13c314205bd..4f5a9398d1e 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -104,7 +104,7 @@ This process is then repeated a number of times and returns a converged solution | critical_weight | double | Default 20.0. Weight to apply to critic for near collisions closer than `collision_margin_distance` to prevent near collisions **only** as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from critical collisions. | | repulsion_weight | double | Default 1.5. Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the `inflation_radius` distance from obstacles. | | cost_power | int | Default 1. Power order to apply to term. | - | collision_cost | double | Default 10000.0. Cost to apply to a true collision in a trajectory. | + | collision_cost | double | Default 100000.0. Cost to apply to a true collision in a trajectory. | | collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. | | 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. @@ -124,11 +124,11 @@ Note: There is a "Legacy" version of this critic also available with the same pa #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | Default 2.0. Weight to apply to critic term. | + | cost_weight | double | Default 2.2. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered | | offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. | - | max_angle_to_furthest | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered | + | max_angle_to_furthest | double | Default 0.785398. Angular distance between robot and goal above which path angle cost starts being considered | | mode | int | Default 0 (Forward Preference). Enum type for mode of operations for the path angle critic depending on path input types and behavioral desires. 0: Forward Preference, penalizes high path angles relative to the robot's orientation to incentivize turning towards the path. 1: No directional preference, penalizes high path angles relative to the robot's orientation or mirrored orientation (e.g. reverse), which ever is less, when a particular direction of travel is not preferable. 2: Consider feasible path orientation, when using a feasible path whereas the path points have orientation information (e.g. Smac Planners), consider the path's requested direction of travel to penalize path angles such that the robot will follow the path in the requested direction. | diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index b8d6c898709..274315a06b7 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -1,4 +1,5 @@ // 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. @@ -25,7 +26,7 @@ void ObstaclesCritic::initialize() getParam(power_, "cost_power", 1); getParam(repulsion_weight_, "repulsion_weight", 1.5); getParam(critical_weight_, "critical_weight", 20.0); - getParam(collision_cost_, "collision_cost", 10000.0); + getParam(collision_cost_, "collision_cost", 100000.0); getParam(collision_margin_distance_, "collision_margin_distance", 0.10); getParam(near_goal_distance_, "near_goal_distance", 0.5); @@ -160,8 +161,11 @@ void ObstaclesCritic::score(CriticData & data) // Let near-collision trajectory points be punished severely if (dist_to_obj < collision_margin_distance_) { traj_cost += (collision_margin_distance_ - dist_to_obj); - } else if (!near_goal) { // Generally prefer trajectories further from obstacles - repulsive_cost[i] += (inflation_radius_ - dist_to_obj); + } + + // Generally prefer trajectories further from obstacles + if (!near_goal) { + repulsive_cost[i] += inflation_radius_ - dist_to_obj; } } @@ -169,9 +173,14 @@ void ObstaclesCritic::score(CriticData & data) raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost; } + // Normalize repulsive cost by trajectory length & lowest score to not overweight importance + // This is a preferential cost, not collision cost, to be tuned relative to desired behaviors + auto && repulsive_cost_normalized = + (repulsive_cost - xt::amin(repulsive_cost, immediate)) / traj_len; + data.costs += xt::pow( (critical_weight_ * raw_cost) + - (repulsion_weight_ * repulsive_cost / traj_len), + (repulsion_weight_ * repulsive_cost_normalized), power_); data.fail_flag = all_trajectories_collide; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index cd701ea093d..d334b816735 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -36,13 +36,13 @@ void PathAngleCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(offset_from_furthest_, "offset_from_furthest", 4); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 2.0); + getParam(weight_, "cost_weight", 2.2); getParam( threshold_to_consider_, "threshold_to_consider", 0.5); getParam( max_angle_to_furthest_, - "max_angle_to_furthest", 1.2); + "max_angle_to_furthest", 0.785398); int mode = 0; getParam(mode, "mode", mode); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index f88249aed14..08f50945fd5 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -282,7 +282,7 @@ TEST(CriticTests, PathAngleCritic) path.y(6) = 4.0; critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); - EXPECT_NEAR(costs(0), 3.6315, 1e-2); // atan2(4,-1) [1.81] * 2.0 weight + EXPECT_NEAR(costs(0), 3.9947, 1e-2); // atan2(4,-1) [1.81] * 2.2 weight // Set mode to no directional preferences + reset costs critic.setMode(1); @@ -306,7 +306,7 @@ TEST(CriticTests, PathAngleCritic) critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); // should use reverse orientation as the closer angle in no dir preference mode - EXPECT_NEAR(costs(0), 2.6516, 1e-2); + EXPECT_NEAR(costs(0), 2.9167, 1e-2); // Set mode to consider path directionality + reset costs critic.setMode(2); @@ -330,7 +330,7 @@ TEST(CriticTests, PathAngleCritic) critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); // should use reverse orientation as the closer angle in no dir preference mode - EXPECT_NEAR(costs(0), 2.6516, 1e-2); + EXPECT_NEAR(costs(0), 2.9167, 1e-2); PathAngleMode mode; mode = PathAngleMode::FORWARD_PREFERENCE; From 518b3782fb5a78fc86426403f4e83820de093ebb Mon Sep 17 00:00:00 2001 From: Hao-Li-Bachelorarbeit <141755843+Hao-Li-Bachelorarbeit@users.noreply.github.com> Date: Mon, 18 Dec 2023 23:22:08 +0100 Subject: [PATCH 19/38] Synchronize map size information during map initialization (#4015) * Update costmap size configuration This commit updates the costmap_2d.cpp file to fix a bug where the costmap size wasn't appropriately updated. Two new lines of code have been added to ensure the size of the costmap is correctly configured each time it's instantiated. * Refactor costmap size assignment in Costmap2D class The code refactor eliminates the direct mutation of the size_x_ and size_y_ attributes in the Costmap2D class. Instead, the class uses the size of cells provided during initialization and calculation from map coordinates for better encapsulation and clarity. --- nav2_costmap_2d/src/costmap_2d.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e72807da59b..8914166a5e1 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -49,13 +49,13 @@ namespace nav2_costmap_2d Costmap2D::Costmap2D( unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value) -: size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), +: resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), costmap_(NULL), default_value_(default_value) { access_ = new mutex_t(); // create the costmap - initMaps(size_x_, size_y_); + initMaps(cells_size_x, cells_size_y); resetMaps(); } @@ -102,6 +102,8 @@ void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y) { std::unique_lock lock(*access_); delete[] costmap_; + size_x_ = size_x; + size_y_ = size_y; costmap_ = new unsigned char[size_x * size_y]; } @@ -109,8 +111,6 @@ void Costmap2D::resizeMap( unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y) { - size_x_ = size_x; - size_y_ = size_y; resolution_ = resolution; origin_x_ = origin_x; origin_y_ = origin_y; @@ -166,15 +166,12 @@ bool Costmap2D::copyCostmapWindow( // ROS_ERROR("Cannot window a map that the window bounds don't fit inside of"); return false; } - - size_x_ = upper_right_x - lower_left_x; - size_y_ = upper_right_y - lower_left_y; resolution_ = map.resolution_; origin_x_ = win_origin_x; origin_y_ = win_origin_y; // initialize our various maps and reset markers for inflation - initMaps(size_x_, size_y_); + initMaps(upper_right_x - lower_left_x, upper_right_y - lower_left_y); // copy the window of the static map and the costmap that we're taking copyMapRegion( From 7872bfae096ac548a4af72c12a72c025648050fa Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Wed, 20 Dec 2023 02:37:15 +0800 Subject: [PATCH 20/38] check width&height params (#4017) Co-authored-by: GoesM --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index bc887eeeb3b..501f675736e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -449,6 +449,18 @@ Costmap2DROS::getParameters() footprint_.c_str(), robot_radius_); } } + + // 4. The width and height of map cannot be negative or 0 (to avoid abnoram memory usage) + if (map_width_meters_ <= 0) { + RCLCPP_ERROR( + get_logger(), "You try to set width of map to be negative or zero," + " this isn't allowed, please give a positive value."); + } + if (map_height_meters_ <= 0) { + RCLCPP_ERROR( + get_logger(), "You try to set height of map to be negative or zero," + " this isn't allowed, please give a positive value."); + } } void From 3992eb1e8663d312aeac685901b3283055aeeaeb Mon Sep 17 00:00:00 2001 From: Vortex-TH <3118243+Vortex-TH@users.noreply.github.com> Date: Wed, 20 Dec 2023 23:59:41 +0100 Subject: [PATCH 21/38] Add DriveOnHeading (#4022) * Add DriveOnHeading Signed-off-by: thass <3118243+Vortex-TH@users.noreply.github.com> * Fix style errors Signed-off-by: thass <3118243+Vortex-TH@users.noreply.github.com> --------- Signed-off-by: thass <3118243+Vortex-TH@users.noreply.github.com> --- .../nav2_simple_commander/robot_navigator.py | 29 ++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index a8ded664cd5..17d2f130e84 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -23,7 +23,7 @@ from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState -from nav2_msgs.action import AssistedTeleop, BackUp, Spin +from nav2_msgs.action import AssistedTeleop, BackUp, DriveOnHeading, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose from nav2_msgs.action import ( FollowGPSWaypoints, @@ -88,6 +88,9 @@ def __init__(self, node_name='basic_navigator', namespace=''): self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') self.spin_client = ActionClient(self, Spin, 'spin') self.backup_client = ActionClient(self, BackUp, 'backup') + self.drive_on_heading_client = ActionClient( + self, DriveOnHeading, 'drive_on_heading' + ) self.assisted_teleop_client = ActionClient( self, AssistedTeleop, 'assisted_teleop' ) @@ -127,6 +130,7 @@ def destroy_node(self): self.smoother_client.destroy() self.spin_client.destroy() self.backup_client.destroy() + self.drive_on_heading_client.destroy() super().destroy_node() def setInitialPose(self, initial_pose): @@ -288,6 +292,29 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): self.result_future = self.goal_handle.get_result_async() return True + def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10): + self.debug("Waiting for 'DriveOnHeading' action server") + while not self.backup_client.wait_for_server(timeout_sec=1.0): + self.info("'DriveOnHeading' action server not available, waiting...") + goal_msg = DriveOnHeading.Goal() + goal_msg.target = Point(x=float(dist)) + goal_msg.speed = speed + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info(f'Drive {goal_msg.target.x} m on heading at {goal_msg.speed} m/s....') + send_goal_future = self.drive_on_heading_client.send_goal_async( + goal_msg, self._feedbackCallback + ) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Drive On Heading request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def assistedTeleop(self, time_allowance=30): self.debug("Wainting for 'assisted_teleop' action server") while not self.assisted_teleop_client.wait_for_server(timeout_sec=1.0): From d04de4e1d1f1b922c63b59133b8bb19d20e30cf3 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Thu, 21 Dec 2023 21:13:58 +0100 Subject: [PATCH 22/38] Fix SimpleActionServer nullprt callback (#4025) * add check before calling completion_callback_ * Fix lint --- nav2_util/include/nav2_util/simple_action_server.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index c74dcf55f29..71828321f00 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -244,7 +244,7 @@ class SimpleActionServer node_logging_interface_->get_logger(), "Action server failed while executing action callback: \"%s\"", ex.what()); terminate_all(); - completion_callback_(); + if (completion_callback_) {completion_callback_();} return; } @@ -254,14 +254,14 @@ class SimpleActionServer if (stop_execution_) { warn_msg("Stopping the thread per request."); terminate_all(); - completion_callback_(); + if (completion_callback_) {completion_callback_();} break; } if (is_active(current_handle_)) { warn_msg("Current goal was not completed successfully."); terminate(current_handle_); - completion_callback_(); + if (completion_callback_) {completion_callback_();} } if (is_active(pending_handle_)) { @@ -314,7 +314,7 @@ class SimpleActionServer info_msg("Waiting for async process to finish."); if (steady_clock::now() - start_time >= server_timeout_) { terminate_all(); - completion_callback_(); + if (completion_callback_) {completion_callback_();} throw std::runtime_error("Action callback is still running and missed deadline to stop"); } } From e2c8555ad352d69aa3da1e670b888d00f6eaeb3a Mon Sep 17 00:00:00 2001 From: Abhijith Sampathkrishna <31732304+ajsampathk@users.noreply.github.com> Date: Sat, 23 Dec 2023 01:58:27 +0000 Subject: [PATCH 23/38] Updating polygon parameter format (#4020) * moving libraries and using parser in collision monitor * apply changes from review * linting missed update * unit test update * collision_monitor utests + uncrustify lint * skip copyright linting * Combine error messages * update yaml * circleci update v18 * add invalid points test for cov --- .circleci/config.yml | 6 +- .../nav2_collision_monitor/polygon.hpp | 8 +++ .../params/collision_detector_params.yaml | 2 +- .../params/collision_monitor_params.yaml | 6 +- nav2_collision_monitor/src/polygon.cpp | 71 ++++++++++++------- .../test/collision_detector_node_test.cpp | 7 +- .../test/collision_monitor_node_test.cpp | 7 +- nav2_collision_monitor/test/polygons_test.cpp | 57 +++++++++++---- nav2_costmap_2d/CMakeLists.txt | 1 - nav2_costmap_2d/src/footprint.cpp | 4 +- nav2_costmap_2d/test/unit/CMakeLists.txt | 5 -- nav2_util/CMakeLists.txt | 2 + .../include/nav2_util}/array_parser.hpp | 10 +-- nav2_util/src/CMakeLists.txt | 1 + .../src/array_parser.cpp | 4 +- nav2_util/test/CMakeLists.txt | 3 + .../test/test_array_parser.cpp | 10 +-- 17 files changed, 133 insertions(+), 71 deletions(-) rename {nav2_costmap_2d/include/nav2_costmap_2d => nav2_util/include/nav2_util}/array_parser.hpp (91%) rename {nav2_costmap_2d => nav2_util}/src/array_parser.cpp (98%) rename nav2_costmap_2d/test/unit/array_parser_test.cpp => nav2_util/test/test_array_parser.cpp (88%) diff --git a/.circleci/config.yml b/.circleci/config.yml index ab6312a3681..190ba5d7f12 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v17\ + - "<< parameters.key >>-v18\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v17\ + - "<< parameters.key >>-v18\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v17\ + key: "<< parameters.key >>-v18\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 815694d83c6..b15672807f4 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -210,6 +210,14 @@ class Polygon */ bool isPointInside(const Point & point) const; + /** + * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...] + * @param poly_string Input String containing the verteceis of the polygon + * @param polygon Output Point vector with all the vertecies of the polygon + * @return True if all parameters were obtained or false in failure case + */ + bool getPolygonFromString(std::string & poly_string, std::vector & polygon); + // ----- Variables ----- /// @brief Collision Monitor node diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 68b4cd7b507..eb3ee0a8739 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -9,7 +9,7 @@ collision_detector: polygons: ["PolygonFront"] PolygonFront: type: "polygon" - points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "none" min_points: 4 visualize: True diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index fcb01f54829..ab5b08bbb96 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -17,7 +17,7 @@ collision_monitor: polygons: ["PolygonStop"] PolygonStop: type: "polygon" - points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "stop" min_points: 4 visualize: True @@ -25,7 +25,7 @@ collision_monitor: enabled: True PolygonSlow: type: "polygon" - points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] + points: "[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]" action_type: "slowdown" min_points: 4 slowdown_ratio: 0.3 @@ -34,7 +34,7 @@ collision_monitor: enabled: True PolygonLimit: type: "polygon" - points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] + points: "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]" action_type: "limit" min_points: 4 linear_limit: 0.4 diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 248bdcec6de..f78f9291efe 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -22,6 +22,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/array_parser.hpp" #include "nav2_collision_monitor/kinematics.hpp" @@ -401,34 +402,14 @@ bool Polygon::getParameters( try { // Leave it uninitialized: it will throw an inner exception if the parameter is not set nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); - std::vector poly_row = - node->get_parameter(polygon_name_ + ".points").as_double_array(); - // Check for points format correctness - if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { - RCLCPP_ERROR( - logger_, - "[%s]: Polygon has incorrect points description", - polygon_name_.c_str()); - return false; - } - - // Obtain polygon vertices - Point point; - bool first = true; - for (double val : poly_row) { - if (first) { - point.x = val; - } else { - point.y = val; - poly_.push_back(point); - } - first = !first; - } + node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING); + std::string poly_string = + node->get_parameter(polygon_name_ + ".points").as_string(); // Do not need to proceed further, if "points" parameter is defined. // Static polygon will be used. - return true; + return getPolygonFromString(poly_string, poly_); + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, @@ -562,4 +543,44 @@ inline bool Polygon::isPointInside(const Point & point) const return res; } +bool Polygon::getPolygonFromString( + std::string & poly_string, + std::vector & polygon) +{ + std::string error; + std::vector> vvf = nav2_util::parseVVF(poly_string, error); + + if (error != "") { + RCLCPP_ERROR( + logger_, "Error parsing polygon parameter %s: '%s'", + poly_string.c_str(), error.c_str()); + return false; + } + + // Check for minimum 4 points + if (vvf.size() <= 3) { + RCLCPP_ERROR( + logger_, + "Polygon must have at least three points."); + return false; + } + for (unsigned int i = 0; i < vvf.size(); i++) { + if (vvf[i].size() == 2) { + Point point; + point.x = vvf[i][0]; + point.y = vvf[i][1]; + polygon.push_back(point); + } else { + RCLCPP_ERROR( + logger_, + "Points in the polygon specification must be pairs of numbers" + "Found a point with %d numbers.", + static_cast(vvf[i].size())); + return false; + } + } + + return true; +} + } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index b0b926fde05..1ac519111f1 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -269,8 +269,11 @@ void Tester::addPolygon( cd_->set_parameter( rclcpp::Parameter(polygon_name + ".type", "polygon")); - const std::vector points { - size, size, size, -size, -size, -size, -size, size}; + const std::string points = "[[" + + std::to_string(size) + ", " + std::to_string(size) + "], [" + + std::to_string(size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(size) + "]]"; cd_->declare_parameter( polygon_name + ".points", rclcpp::ParameterValue(points)); cd_->set_parameter( diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 94801a3d2a3..242b9f72db9 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -301,8 +301,11 @@ void Tester::addPolygon( rclcpp::Parameter(polygon_name + ".type", "polygon")); if (at != "approach") { - const std::vector points { - size, size, size, -size, -size, -size, -size, size}; + const std::string points = "[[" + + std::to_string(size) + ", " + std::to_string(size) + "], [" + + std::to_string(size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(size) + "]]"; cm_->declare_parameter( polygon_name + ".points", rclcpp::ParameterValue(points)); cm_->set_parameter( diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 2ab46cdcede..8d3ad3dc5ef 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -50,6 +50,19 @@ static const std::vector SQUARE_POLYGON { 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}; static const std::vector ARBITRARY_POLYGON { 1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 2.0, -1.0, -1.0, -1.0, -1.0, 1.0}; +static const char SQUARE_POLYGON_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"}; +static const char ARBITRARY_POLYGON_STR[]{ + "[[1.0, 1.0], [1.0, 0.0], [2.0, 0.0], [2.0, -1.0], [-1.0, -1.0], [-1.0, 1.0]]"}; +static const char INCORRECT_POINTS_1_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5]]" +}; +static const char INCORRECT_POINTS_2_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5], [0]]" +}; +static const char INVALID_POINTS_STR[]{ + "[[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5], 0]]" +}; static const double CIRCLE_RADIUS{0.5}; static const int MIN_POINTS{2}; static const double SLOWDOWN_RATIO{0.7}; @@ -212,7 +225,7 @@ class Tester : public ::testing::Test // Working with parameters void setCommonParameters(const std::string & polygon_name, const std::string & action_type); void setPolygonParameters( - const std::vector & points, + const char * points, const bool is_static); void setCircleParameters(const double radius); bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); @@ -311,7 +324,7 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st } void Tester::setPolygonParameters( - const std::vector & points, const bool is_static) + const char * points, const bool is_static) { if (is_static) { test_node_->declare_parameter( @@ -360,7 +373,7 @@ bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const st void Tester::createPolygon(const std::string & action_type, const bool is_static) { setCommonParameters(POLYGON_NAME, action_type); - setPolygonParameters(SQUARE_POLYGON, is_static); + setPolygonParameters(SQUARE_POLYGON_STR, is_static); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -546,7 +559,7 @@ TEST_F(Tester, testPolygonUndeclaredPoints) TEST_F(Tester, testPolygonIncorrectActionType) { setCommonParameters(POLYGON_NAME, "incorrect_action_type"); - setPolygonParameters(SQUARE_POLYGON, true); + setPolygonParameters(SQUARE_POLYGON_STR, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -558,12 +571,11 @@ TEST_F(Tester, testPolygonIncorrectPoints1) { setCommonParameters(POLYGON_NAME, "stop"); - std::vector incorrect_points = SQUARE_POLYGON; - incorrect_points.resize(6); // Not enough for triangle + // Triangle points test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INCORRECT_POINTS_1_STR)); test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INCORRECT_POINTS_1_STR)); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -575,12 +587,11 @@ TEST_F(Tester, testPolygonIncorrectPoints2) { setCommonParameters(POLYGON_NAME, "stop"); - std::vector incorrect_points = SQUARE_POLYGON; - incorrect_points.resize(9); // Odd number of points + // Odd number of elements test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INCORRECT_POINTS_2_STR)); test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INCORRECT_POINTS_2_STR)); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -592,7 +603,7 @@ TEST_F(Tester, testPolygonIncorrectPoints2) TEST_F(Tester, testPolygonMaxPoints) { setCommonParameters(POLYGON_NAME, "stop"); - setPolygonParameters(SQUARE_POLYGON, true); + setPolygonParameters(SQUARE_POLYGON_STR, true); const int max_points = 5; test_node_->declare_parameter( @@ -751,7 +762,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge) // Test for checking edge cases in raytracing algorithm. // All points are lie on the edge lines parallel to OX, where the raytracing takes place. setCommonParameters(POLYGON_NAME, "stop"); - setPolygonParameters(ARBITRARY_POLYGON, true); + setPolygonParameters(ARBITRARY_POLYGON_STR, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -884,7 +895,7 @@ TEST_F(Tester, testPolygonDefaultVisualize) std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); - setPolygonParameters(SQUARE_POLYGON, true); + setPolygonParameters(SQUARE_POLYGON_STR, true); // Create new polygon polygon_ = std::make_shared( @@ -900,6 +911,22 @@ TEST_F(Tester, testPolygonDefaultVisualize) ASSERT_EQ(test_node_->waitPolygonReceived(100ms), nullptr); } +TEST_F(Tester, testPolygonInvalidPointsString) +{ + setCommonParameters(POLYGON_NAME, "stop"); + + // Invalid points + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INVALID_POINTS_STR)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INVALID_POINTS_STR)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index f039cf5d42b..df0ddb2dc3a 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -38,7 +38,6 @@ include_directories( add_definitions(${EIGEN3_DEFINITIONS}) add_library(nav2_costmap_2d_core SHARED - src/array_parser.cpp src/costmap_2d.cpp src/layer.cpp src/layered_costmap.cpp diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp index c9259730382..75de1feb917 100644 --- a/nav2_costmap_2d/src/footprint.cpp +++ b/nav2_costmap_2d/src/footprint.cpp @@ -34,7 +34,7 @@ #include #include "geometry_msgs/msg/point32.hpp" -#include "nav2_costmap_2d/array_parser.hpp" +#include "nav2_util/array_parser.hpp" #include "nav2_costmap_2d/costmap_math.hpp" namespace nav2_costmap_2d @@ -177,7 +177,7 @@ bool makeFootprintFromString( std::vector & footprint) { std::string error; - std::vector> vvf = parseVVF(footprint_string, error); + std::vector> vvf = nav2_util::parseVVF(footprint_string, error); if (error != "") { RCLCPP_ERROR( diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index b9480fdffab..e69501277ea 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -1,8 +1,3 @@ -ament_add_gtest(array_parser_test array_parser_test.cpp) -target_link_libraries(array_parser_test - nav2_costmap_2d_core -) - ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test nav2_costmap_2d_core diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 4635bab0b0d..1c8b9b1d64b 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -50,6 +50,8 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_pytest REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) + # skip copyright linting + set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp b/nav2_util/include/nav2_util/array_parser.hpp similarity index 91% rename from nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp rename to nav2_util/include/nav2_util/array_parser.hpp index ca2dd879b6f..f232cf30274 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp +++ b/nav2_util/include/nav2_util/array_parser.hpp @@ -28,13 +28,13 @@ * * author: Dave Hershberger */ -#ifndef NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_ -#define NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_ +#ifndef NAV2_UTIL__ARRAY_PARSER_HPP_ +#define NAV2_UTIL__ARRAY_PARSER_HPP_ #include #include -namespace nav2_costmap_2d +namespace nav2_util { /** @brief Parse a vector of vectors of floats from a string. @@ -46,6 +46,6 @@ namespace nav2_costmap_2d * anything, like part of a successful parse. */ std::vector> parseVVF(const std::string & input, std::string & error_return); -} // end namespace nav2_costmap_2d +} // end namespace nav2_util -#endif // NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_ +#endif // NAV2_UTIL__ARRAY_PARSER_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index a639a0e59e9..104966e2198 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -8,6 +8,7 @@ add_library(${library_name} SHARED robot_utils.cpp node_thread.cpp odometry_utils.cpp + array_parser.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_costmap_2d/src/array_parser.cpp b/nav2_util/src/array_parser.cpp similarity index 98% rename from nav2_costmap_2d/src/array_parser.cpp rename to nav2_util/src/array_parser.cpp index eecd4eebc8c..cfe2f699145 100644 --- a/nav2_costmap_2d/src/array_parser.cpp +++ b/nav2_util/src/array_parser.cpp @@ -34,7 +34,7 @@ #include #include -namespace nav2_costmap_2d +namespace nav2_util { /** @brief Parse a vector of vector of floats from a string. @@ -102,4 +102,4 @@ std::vector> parseVVF(const std::string & input, std::string return result; } -} // end namespace nav2_costmap_2d +} // end namespace nav2_util diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 9f1ae99bbcb..cfd747b9f1b 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,3 +41,6 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) + +ament_add_gtest(test_array_parser test_array_parser.cpp) +target_link_libraries(test_array_parser ${library_name}) diff --git a/nav2_costmap_2d/test/unit/array_parser_test.cpp b/nav2_util/test/test_array_parser.cpp similarity index 88% rename from nav2_costmap_2d/test/unit/array_parser_test.cpp rename to nav2_util/test/test_array_parser.cpp index 3706f0cb775..76a691b7000 100644 --- a/nav2_costmap_2d/test/unit/array_parser_test.cpp +++ b/nav2_util/test/test_array_parser.cpp @@ -31,13 +31,13 @@ #include #include "gtest/gtest.h" -#include "nav2_costmap_2d/array_parser.hpp" +#include "nav2_util/array_parser.hpp" TEST(array_parser, basic_operation) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[[1, 2.2], [.3, -4e4]]", error); + vvf = nav2_util::parseVVF("[[1, 2.2], [.3, -4e4]]", error); EXPECT_EQ(2u, vvf.size() ); EXPECT_EQ(2u, vvf[0].size() ); EXPECT_EQ(2u, vvf[1].size() ); @@ -52,7 +52,7 @@ TEST(array_parser, missing_open) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[1, 2.2], [.3, -4e4]]", error); + vvf = nav2_util::parseVVF("[1, 2.2], [.3, -4e4]]", error); EXPECT_NE(error, ""); } @@ -60,7 +60,7 @@ TEST(array_parser, missing_close) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[[1, 2.2], [.3, -4e4]", error); + vvf = nav2_util::parseVVF("[[1, 2.2], [.3, -4e4]", error); EXPECT_NE(error, ""); } @@ -68,7 +68,7 @@ TEST(array_parser, wrong_depth) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[1, 2.2], [.3, -4e4]", error); + vvf = nav2_util::parseVVF("[1, 2.2], [.3, -4e4]", error); EXPECT_NE(error, ""); } From e3c5c559da6c4982ccb24830f39fa31f52daa424 Mon Sep 17 00:00:00 2001 From: maksymdidukh <142302397+maksymdidukh@users.noreply.github.com> Date: Tue, 2 Jan 2024 22:52:30 +0100 Subject: [PATCH 24/38] BT nodes not able to accept parameter values (#3988) * fix simple cases * fix on_tick type in drive_on_heading node * fix linting and fixed other nodes * Revert "fix linting and fixed other nodes" This reverts commit c1cd575b50580305b56a70b4a0fd0e399acaf39c. * fix linting * fixes for subscription cases in action nodes * fixes in condition nodes * fix input usage * fix uncrustify * . * fix typo in back_up_action.cpp * fix in drive_on_heading_action * remove unnecessary first_use variables * typo * typo * typo * fixed typos * move initialize() method above tick() * revert changes in truncate_path_action node * revert changes in case of creating subscription in constructor * remove global_frame_ in remove_passed_goals_action node * changes in is_path_valid and rate_controller nodes * change bt_cancel_action node * change bt_action_node * add xml_tag parameter * revert changes in bt_action and bt_cancel_action nodes * pre-commit * . --- .../plugins/action/assisted_teleop_action.hpp | 6 +++++ .../plugins/action/back_up_action.hpp | 8 +++++++ .../action/drive_on_heading_action.hpp | 13 ++++++++++ .../action/remove_passed_goals_action.hpp | 5 ++++ .../plugins/action/spin_action.hpp | 6 +++++ .../plugins/action/wait_action.hpp | 8 +++++++ .../condition/distance_traveled_condition.hpp | 6 +++++ .../condition/is_battery_low_condition.hpp | 6 +++++ .../condition/is_path_valid_condition.hpp | 6 +++++ .../condition/time_expired_condition.hpp | 6 +++++ .../transform_available_condition.hpp | 6 +++++ .../plugins/decorator/rate_controller.hpp | 6 +++++ .../plugins/action/assisted_teleop_action.cpp | 11 ++++++++- .../plugins/action/back_up_action.cpp | 12 +++++++++- .../action/drive_on_heading_action.cpp | 15 +++++++++++- .../action/remove_passed_goals_action.cpp | 10 +++++++- .../plugins/action/spin_action.cpp | 13 +++++++++- .../plugins/action/truncate_path_action.cpp | 2 +- .../plugins/action/wait_action.cpp | 12 +++++++++- .../condition/distance_traveled_condition.cpp | 12 +++++++++- .../condition/goal_reached_condition.cpp | 24 +++++++++---------- .../condition/is_battery_low_condition.cpp | 12 +++++++++- .../condition/is_path_valid_condition.cpp | 12 +++++++++- .../path_expiring_timer_condition.cpp | 2 +- .../condition/time_expired_condition.cpp | 12 +++++++++- .../transform_available_condition.cpp | 21 +++++++++++----- .../plugins/control/recovery_node.cpp | 2 +- .../plugins/decorator/rate_controller.cpp | 12 +++++++++- 28 files changed, 234 insertions(+), 32 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp index fb3f683b462..97511608d50 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -64,6 +64,11 @@ class AssistedTeleopAction : public BtActionNode */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -79,6 +84,9 @@ class BackUpAction : public BtActionNode "error_code_id", "The back up behavior server error code") }); } + +private: + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp index 7e5b27af9ce..dd1a6f29086 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp @@ -43,6 +43,11 @@ class DriveOnHeadingAction : public BtActionNode tf_; std::string robot_base_frame_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index 2e442c0e112..4f9b300e445 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -48,6 +48,11 @@ class SpinAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -81,6 +86,7 @@ class SpinAction : public BtActionNode private: bool is_recovery_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index 5143f11351e..fdc320c16b0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -45,6 +45,11 @@ class WaitAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -56,6 +61,9 @@ class WaitAction : public BtActionNode BT::InputPort("wait_duration", 1.0, "Wait time") }); } + +private: + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index d29f40e66d8..3c87c7370c7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -52,6 +52,11 @@ class DistanceTraveledCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -74,6 +79,7 @@ class DistanceTraveledCondition : public BT::ConditionNode double distance_; double transform_tolerance_; std::string global_frame_, robot_base_frame_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 5c4753cf9a1..ff479105807 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -51,6 +51,11 @@ class IsBatteryLowCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -81,6 +86,7 @@ class IsBatteryLowCondition : public BT::ConditionNode double min_battery_; bool is_voltage_; bool is_battery_low_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index a04b1263f4b..36890f2a8a0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -50,6 +50,11 @@ class IsPathValidCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -68,6 +73,7 @@ class IsPathValidCondition : public BT::ConditionNode // The timeout value while waiting for a responce from the // is path valid service std::chrono::milliseconds server_timeout_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp index 9f8c47afab0..356a79857db 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp @@ -48,6 +48,11 @@ class TimeExpiredCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -63,6 +68,7 @@ class TimeExpiredCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; rclcpp::Time start_; double period_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 64572e21b7a..5fdefa9b8bf 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -55,6 +55,11 @@ class TransformAvailableCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -75,6 +80,7 @@ class TransformAvailableCondition : public BT::ConditionNode std::string child_frame_; std::string parent_frame_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index 201f4868cfd..24a0207e3bd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp @@ -38,6 +38,11 @@ class RateController : public BT::DecoratorNode const std::string & name, const BT::NodeConfiguration & conf); + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -59,6 +64,7 @@ class RateController : public BT::DecoratorNode std::chrono::time_point start_; double period_; bool first_time_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index 1d2bad9f376..64eb61d788e 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -24,7 +24,11 @@ AssistedTeleopAction::AssistedTeleopAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{} + +void AssistedTeleopAction::initialize() { double time_allowance; getInput("time_allowance", time_allowance); @@ -32,10 +36,15 @@ AssistedTeleopAction::AssistedTeleopAction( // Populate the input message goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + initialized_ = true; } void AssistedTeleopAction::on_tick() { + if (!initialized_) { + initialize(); + } + if (is_recovery_) { increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index cb15fc93336..09d79c7e230 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -24,7 +24,12 @@ BackUpAction::BackUpAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{ +} + +void nav2_behavior_tree::BackUpAction::initialize() { double dist; getInput("backup_dist", dist); @@ -39,10 +44,15 @@ BackUpAction::BackUpAction( goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + initialized_ = true; } void BackUpAction::on_tick() { + if (!initialized_) { + initialize(); + } + increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index be062357b16..7e0b613f62f 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -24,7 +24,12 @@ DriveOnHeadingAction::DriveOnHeadingAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initalized_(false) +{ +} + +void DriveOnHeadingAction::initialize() { double dist; getInput("dist_to_travel", dist); @@ -39,6 +44,14 @@ DriveOnHeadingAction::DriveOnHeadingAction( goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + initalized_ = true; +} + +void DriveOnHeadingAction::on_tick() +{ + if (!initalized_) { + initialize(); + } } BT::NodeStatus DriveOnHeadingAction::on_success() diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 7a5816973dc..bf8b95c84f6 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -29,7 +29,11 @@ RemovePassedGoals::RemovePassedGoals( const std::string & name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf), - viapoint_achieved_radius_(0.5) + viapoint_achieved_radius_(0.5), + initialized_(false) +{} + +void RemovePassedGoals::initialize() { getInput("radius", viapoint_achieved_radius_); @@ -45,6 +49,10 @@ inline BT::NodeStatus RemovePassedGoals::tick() { setStatus(BT::NodeStatus::RUNNING); + if (!initialized_) { + initialize(); + } + Goals goal_poses; getInput("input_goals", goal_poses); diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index 78fa1311752..cab9c8f8211 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -23,7 +23,12 @@ SpinAction::SpinAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{ +} + +void SpinAction::initialize() { double dist; getInput("spin_dist", dist); @@ -32,10 +37,16 @@ SpinAction::SpinAction( goal_.target_yaw = dist; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); getInput("is_recovery", is_recovery_); + + initialized_ = true; } void SpinAction::on_tick() { + if (!initialized_) { + initialize(); + } + if (is_recovery_) { increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp index 7f919a30f3a..b394a804c73 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -33,12 +33,12 @@ TruncatePath::TruncatePath( : BT::ActionNodeBase(name, conf), distance_(1.0) { - getInput("distance", distance_); } inline BT::NodeStatus TruncatePath::tick() { setStatus(BT::NodeStatus::RUNNING); + getInput("distance", distance_); nav_msgs::msg::Path input_path; diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index f08fed31476..b9317076737 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -25,7 +25,12 @@ WaitAction::WaitAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{ +} + +void WaitAction::initialize() { double duration; getInput("wait_duration", duration); @@ -37,10 +42,15 @@ WaitAction::WaitAction( } goal_.time = rclcpp::Duration::from_seconds(duration); + initialized_ = true; } void WaitAction::on_tick() { + if (!initialized_) { + initialize(); + } + increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 5840b8882df..64394ee3782 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -30,7 +30,12 @@ DistanceTraveledCondition::DistanceTraveledCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), distance_(1.0), - transform_tolerance_(0.1) + transform_tolerance_(0.1), + initialized_(false) +{ +} + +void DistanceTraveledCondition::initialize() { getInput("distance", distance_); @@ -42,10 +47,15 @@ DistanceTraveledCondition::DistanceTraveledCondition( node_, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); + initialized_ = true; } BT::NodeStatus DistanceTraveledCondition::tick() { + if (!initialized_) { + initialize(); + } + if (status() == BT::NodeStatus::IDLE) { if (!nav2_util::getCurrentPose( start_pose_, *tf_, global_frame_, robot_base_frame_, diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index d81df60a9ec..2697680415b 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -42,18 +42,6 @@ GoalReachedCondition::~GoalReachedCondition() cleanup(); } -BT::NodeStatus GoalReachedCondition::tick() -{ - if (!initialized_) { - initialize(); - } - - if (isGoalReached()) { - return BT::NodeStatus::SUCCESS; - } - return BT::NodeStatus::FAILURE; -} - void GoalReachedCondition::initialize() { node_ = config().blackboard->get("node"); @@ -69,6 +57,18 @@ void GoalReachedCondition::initialize() initialized_ = true; } +BT::NodeStatus GoalReachedCondition::tick() +{ + if (!initialized_) { + initialize(); + } + + if (isGoalReached()) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + bool GoalReachedCondition::isGoalReached() { geometry_msgs::msg::PoseStamped goal; diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index b8630261e6e..9d221cf8c81 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -27,7 +27,12 @@ IsBatteryLowCondition::IsBatteryLowCondition( battery_topic_("/battery_status"), min_battery_(0.0), is_voltage_(false), - is_battery_low_(false) + is_battery_low_(false), + initialized_(false) +{ +} + +void IsBatteryLowCondition::initialize() { getInput("min_battery", min_battery_); getInput("battery_topic", battery_topic_); @@ -45,10 +50,15 @@ IsBatteryLowCondition::IsBatteryLowCondition( rclcpp::SystemDefaultsQoS(), std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), sub_option); + initialized_ = true; } BT::NodeStatus IsBatteryLowCondition::tick() { + if (!initialized_) { + initialize(); + } + callback_group_executor_.spin_some(); if (is_battery_low_) { return BT::NodeStatus::SUCCESS; diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 4a02dede9aa..59d593a1475 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -23,17 +23,27 @@ namespace nav2_behavior_tree IsPathValidCondition::IsPathValidCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf) +: BT::ConditionNode(condition_name, conf), + initialized_(false) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); server_timeout_ = config().blackboard->template get("server_timeout"); +} + +void IsPathValidCondition::initialize() +{ getInput("server_timeout", server_timeout_); + initialized_ = true; } BT::NodeStatus IsPathValidCondition::tick() { + if (!initialized_) { + initialize(); + } + nav_msgs::msg::Path path; getInput("path", path); diff --git a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp index e018e025352..1347998602f 100644 --- a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp @@ -29,13 +29,13 @@ PathExpiringTimerCondition::PathExpiringTimerCondition( period_(1.0), first_time_(true) { - getInput("seconds", period_); node_ = config().blackboard->get("node"); } BT::NodeStatus PathExpiringTimerCondition::tick() { if (first_time_) { + getInput("seconds", period_); getInput("path", prev_path_); first_time_ = false; start_ = node_->now(); diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index 4dc7e588937..64afd1a34be 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -27,15 +27,25 @@ TimeExpiredCondition::TimeExpiredCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - period_(1.0) + period_(1.0), + initialized_(false) +{ +} + +void TimeExpiredCondition::initialize() { getInput("seconds", period_); node_ = config().blackboard->get("node"); start_ = node_->now(); + initialized_ = true; } BT::NodeStatus TimeExpiredCondition::tick() { + if (!initialized_) { + initialize(); + } + if (status() == BT::NodeStatus::IDLE) { start_ = node_->now(); return BT::NodeStatus::FAILURE; diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 8daca5afede..33e94178deb 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -27,11 +27,20 @@ TransformAvailableCondition::TransformAvailableCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - was_found_(false) + was_found_(false), + initialized_(false) { node_ = config().blackboard->get("node"); tf_ = config().blackboard->get>("tf_buffer"); +} +TransformAvailableCondition::~TransformAvailableCondition() +{ + RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node"); +} + +void TransformAvailableCondition::initialize() +{ getInput("child", child_frame_); getInput("parent", parent_frame_); @@ -43,15 +52,15 @@ TransformAvailableCondition::TransformAvailableCondition( } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); -} - -TransformAvailableCondition::~TransformAvailableCondition() -{ - RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node"); + initialized_ = true; } BT::NodeStatus TransformAvailableCondition::tick() { + if (!initialized_) { + initialize(); + } + if (was_found_) { return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 42fc8076be6..44e56dc55ec 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -26,11 +26,11 @@ RecoveryNode::RecoveryNode( number_of_retries_(1), retry_count_(0) { - getInput("number_of_retries", number_of_retries_); } BT::NodeStatus RecoveryNode::tick() { + getInput("number_of_retries", number_of_retries_); const unsigned children_count = children_nodes_.size(); if (children_count != 2) { diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index 13e98e0d29d..b710ec30091 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -24,15 +24,25 @@ RateController::RateController( const std::string & name, const BT::NodeConfiguration & conf) : BT::DecoratorNode(name, conf), - first_time_(false) + first_time_(false), + initialized_(false) +{ +} + +void RateController::initialize() { double hz = 1.0; getInput("hz", hz); period_ = 1.0 / hz; + initialized_ = true; } BT::NodeStatus RateController::tick() { + if (!initialized_) { + initialize(); + } + if (status() == BT::NodeStatus::IDLE) { // Reset the starting point since we're starting a new iteration of // the rate controller (moving from IDLE to RUNNING) From ef0d773ae999985e4688e209c5cc8ff19bab11c2 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 2 Jan 2024 19:10:39 -0800 Subject: [PATCH 25/38] Create CONTRIBUTING.md Signed-off-by: Steve Macenski --- CONTRIBUTING.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 CONTRIBUTING.md diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000000..cfba094dace --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). From 7647ba77d1b7bbba866e481ddd58f172736f8fef Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 4 Jan 2024 12:38:42 -0500 Subject: [PATCH 26/38] footprint checks (#4030) * footprint checks Signed-off-by: gg * lint fix Signed-off-by: gg --------- Signed-off-by: gg --- .../include/nav2_planner/planner_server.hpp | 3 ++ nav2_planner/src/planner_server.cpp | 37 +++++++++++++++---- 2 files changed, 33 insertions(+), 7 deletions(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 2413e2eb3f0..7d2c9d03f00 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -39,6 +39,7 @@ #include "pluginlib/class_list_macros.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_core/planner_exceptions.hpp" namespace nav2_planner @@ -248,6 +249,8 @@ class PlannerServer : public nav2_util::LifecycleNode std::shared_ptr costmap_ros_; std::unique_ptr costmap_thread_; nav2_costmap_2d::Costmap2D * costmap_; + std::unique_ptr> + collision_checker_; // Publishers for the path rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index d087cbef758..cb2e87b7463 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -52,7 +52,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); - declare_parameter("action_server_result_timeout", 10.0); get_parameter("planner_plugins", planner_ids_); @@ -86,6 +85,12 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) costmap_ros_->configure(); costmap_ = costmap_ros_->getCostmap(); + if (!costmap_ros_->getUseRadius()) { + collision_checker_ = + std::make_unique>( + costmap_); + } + // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); @@ -644,16 +649,34 @@ void PlannerServer::isPathValid( std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; unsigned int my = 0; + + bool use_radius = costmap_ros_->getUseRadius(); + + unsigned int cost = nav2_costmap_2d::FREE_SPACE; for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { - costmap_->worldToMap( - request->path.poses[i].pose.position.x, - request->path.poses[i].pose.position.y, mx, my); - unsigned int cost = costmap_->getCost(mx, my); + auto & position = request->path.poses[i].pose.position; + if (use_radius) { + if (costmap_->worldToMap(position.x, position.y, mx, my)) { + cost = costmap_->getCost(mx, my); + } else { + cost = nav2_costmap_2d::LETHAL_OBSTACLE; + } + } else { + nav2_costmap_2d::Footprint footprint = costmap_ros_->getRobotFootprint(); + auto theta = tf2::getYaw(request->path.poses[i].pose.orientation); + cost = static_cast(collision_checker_->footprintCostAtPose( + position.x, position.y, theta, footprint)); + } - if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + if (use_radius && + (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) { response->is_valid = false; + break; + } else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) { + response->is_valid = false; + break; } } } From 305f7e039f1183b94981ec6a7f294f0ed7b89d07 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 4 Jan 2024 18:14:46 -0500 Subject: [PATCH 27/38] Is path valid doc (#4032) * docs Signed-off-by: gg * update Signed-off-by: gg --------- Signed-off-by: gg --- nav2_planner/src/planner_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index cb2e87b7463..af28b1b1eb0 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -644,7 +644,8 @@ void PlannerServer::isPathValid( /** * The lethal check starts at the closest point to avoid points that have already been passed - * and may have become occupied + * and may have become occupied. The method for collision detection is based on the shape of + * the footprint. */ std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; From bc76734348e8a4d5429debd496fd1e7d35a97ae1 Mon Sep 17 00:00:00 2001 From: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com> Date: Fri, 5 Jan 2024 18:44:55 +0100 Subject: [PATCH 28/38] Direction change index computation during initialization (#4026) * modified direction change index computation point prior the direction change index was being computed in ever getNeighbor call, now its done at parse time. Signed-off-by: Anil Kumar Chavali * changed computation point of direction_change_index * moved computation to getMotionPrimitives function * updated getMotionPrimitives function to return index * updated node lattice tests with modified function * removed obsolete code Signed-off-by: Anil Kumar Chavali * updated docstring for getMotionPrimitives func. Signed-off-by: Anil Kumar Chavali * linting fixes using ament_uncrustify Signed-off-by: Anil Kumar Chavali --------- Signed-off-by: Anil Kumar Chavali --- .../nav2_smac_planner/node_lattice.hpp | 5 ++++- nav2_smac_planner/src/node_lattice.cpp | 22 +++++++++---------- nav2_smac_planner/test/test_nodelattice.cpp | 15 +++++++++---- 3 files changed, 26 insertions(+), 16 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 4fe6a284cf4..577cc28e192 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -67,9 +67,12 @@ struct LatticeMotionTable /** * @brief Get projections of motion models * @param node Ptr to NodeLattice + * @param Reference direction change index * @return A set of motion poses */ - MotionPrimitivePtrs getMotionPrimitives(const NodeLattice * node); + MotionPrimitivePtrs getMotionPrimitives( + const NodeLattice * node, + unsigned int & direction_change_index); /** * @brief Get file metadata needed diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index c852a96dd32..dfdf47ff4bf 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -111,7 +111,9 @@ void LatticeMotionTable::initMotionModel( } } -MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * node) +MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives( + const NodeLattice * node, + unsigned int & direction_change_index) { MotionPrimitives & prims_at_heading = motion_primitives[node->pose.theta]; MotionPrimitivePtrs primitive_projection_list; @@ -119,6 +121,9 @@ MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * primitive_projection_list.push_back(&prims_at_heading[i]); } + // direction change index + direction_change_index = static_cast(primitive_projection_list.size()); + if (allow_reverse_expansion) { // Find normalized heading bin of the reverse expansion double reserve_heading = node->pose.theta - (num_angle_quantization / 2); @@ -470,17 +475,12 @@ void NodeLattice::getNeighbors( bool backwards = false; NodePtr neighbor = nullptr; Coordinates initial_node_coords, motion_projection; - MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives(this); + unsigned int direction_change_index = 0; + MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives( + this, + direction_change_index); const float & grid_resolution = motion_table.lattice_metadata.grid_resolution; - unsigned int direction_change_idx = 1e9; - for (unsigned int i = 0; i != motion_primitives.size(); i++) { - if (motion_primitives[0]->start_angle != motion_primitives[i]->start_angle) { - direction_change_idx = i; - break; - } - } - for (unsigned int i = 0; i != motion_primitives.size(); i++) { const MotionPose & end_pose = motion_primitives[i]->poses.back(); motion_projection.x = this->pose.x + (end_pose._x / grid_resolution); @@ -502,7 +502,7 @@ void NodeLattice::getNeighbors( // account in case the robot base footprint is asymmetric. backwards = false; angle = motion_projection.theta; - if (i >= direction_change_idx) { + if (i >= direction_change_index) { backwards = true; angle = motion_projection.theta - (motion_table.num_angle_quantization / 2); if (angle < 0) { diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 16674d9dadc..f64f022f716 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -120,9 +120,12 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info); nav2_smac_planner::NodeLattice aNode(0); + unsigned int direction_change_index = 0; aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0)); nav2_smac_planner::MotionPrimitivePtrs projections = - nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&aNode); + nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives( + &aNode, + direction_change_index); EXPECT_NEAR(projections[0]->poses.back()._x, 0.5, 0.01); EXPECT_NEAR(projections[0]->poses.back()._y, -0.35, 0.01); @@ -130,7 +133,9 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) EXPECT_NEAR( nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata( - filePath).grid_resolution, 0.05, 0.005); + filePath) + .grid_resolution, + 0.05, 0.005); } TEST(NodeLatticeTest, test_node_lattice_conversions) @@ -248,7 +253,6 @@ TEST(NodeLatticeTest, test_node_lattice) delete costmapA; } - TEST(NodeLatticeTest, test_get_neighbors) { auto lnode = std::make_shared("test"); @@ -356,9 +360,12 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint) node.pose.x = 20; node.pose.y = 20; node.pose.theta = 0; + + // initialize direction change index + unsigned int direction_change_index = 0; // Test that the node is valid though all motion primitives poses for custom footprint nav2_smac_planner::MotionPrimitivePtrs motion_primitives = - nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node); + nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node, direction_change_index); EXPECT_GT(motion_primitives.size(), 0u); for (unsigned int i = 0; i < motion_primitives.size(); i++) { EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], false), true); From 133d1a90647688b4820d20f371fb1da6523078fc Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 5 Jan 2024 17:46:14 -0800 Subject: [PATCH 29/38] adding github action lint (#4036) * adding github action lint * move linting file Signed-off-by: Steve Macenski * try all wildcard * stringify * removing copyright check --------- Signed-off-by: Steve Macenski --- .github/workflows/lint.yml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .github/workflows/lint.yml diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml new file mode 100644 index 00000000000..635086a5227 --- /dev/null +++ b/.github/workflows/lint.yml @@ -0,0 +1,21 @@ +name: Lint +on: + pull_request: + +jobs: + ament_lint_general: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-latest + container: + image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + strategy: + fail-fast: false + matrix: + linter: [xmllint, cpplint, uncrustify, pep257, flake8] + steps: + - uses: actions/checkout@v2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + linter: ${{ matrix.linter }} + distribution: rolling + package-name: "*" From 8e90477c8a7afbc9f55de7b571ad6a6a0f381c00 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 5 Jan 2024 18:16:41 -0800 Subject: [PATCH 30/38] debug visualizations for lattice planner (#4034) * debug visualizations for lattice planner Signed-off-by: Steve Macenski * fix a typo * fix test * Update planner_server.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_planner/src/planner_server.cpp | 4 +- nav2_smac_planner/README.md | 2 +- .../smac_planner_lattice.hpp | 6 ++ .../src/smac_planner_lattice.cpp | 70 ++++++++++++++++++- nav2_smac_planner/test/test_smac_hybrid.cpp | 1 + nav2_smac_planner/test/test_smac_lattice.cpp | 1 + 6 files changed, 80 insertions(+), 4 deletions(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index af28b1b1eb0..9a7b7912ae3 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -644,8 +644,8 @@ void PlannerServer::isPathValid( /** * The lethal check starts at the closest point to avoid points that have already been passed - * and may have become occupied. The method for collision detection is based on the shape of - * the footprint. + * and may have become occupied. The method for collision detection is based on the shape of + * the footprint. */ std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 5f437ce546a..5eee580bc8e 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -126,7 +126,7 @@ planner_server: cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. - debug_visualizations: True # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + debug_visualizations: True # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. smoother: max_iterations: 1000 w_smooth: 0.3 diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index f5207cd45c5..ce8bafd2fd0 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -27,6 +27,7 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" @@ -111,6 +112,11 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; double _lookup_table_size; + bool _debug_visualizations; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _planned_footprints_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _expansions_publisher; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index d3ddf5bdef2..392c75d6198 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -126,6 +126,9 @@ void SmacPlannerLattice::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".allow_reverse_expansion", rclcpp::ParameterValue(false)); node->get_parameter(name + ".allow_reverse_expansion", _search_info.allow_reverse_expansion); + nav2_util::declare_parameter_if_not_declared( + node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".debug_visualizations", _debug_visualizations); _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = @@ -193,6 +196,12 @@ void SmacPlannerLattice::configure( _smoother->initialize(_metadata.min_turning_radius); } + if (_debug_visualizations) { + _expansions_publisher = node->create_publisher("expansions", 1); + _planned_footprints_publisher = node->create_publisher( + "planned_footprints", 1); + } + RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerLattice with " "maximum iterations %i, max on approach iterations %i, " @@ -208,6 +217,10 @@ void SmacPlannerLattice::activate() _logger, "Activating plugin %s of type SmacPlannerLattice", _name.c_str()); _raw_plan_publisher->on_activate(); + if (_debug_visualizations) { + _expansions_publisher->on_activate(); + _planned_footprints_publisher->on_activate(); + } auto node = _node.lock(); // Add callback for dynamic parameters _dyn_params_handler = node->add_on_set_parameters_callback( @@ -220,6 +233,10 @@ void SmacPlannerLattice::deactivate() _logger, "Deactivating plugin %s of type SmacPlannerLattice", _name.c_str()); _raw_plan_publisher->on_deactivate(); + if (_debug_visualizations) { + _expansions_publisher->on_deactivate(); + _planned_footprints_publisher->on_deactivate(); + } _dyn_params_handler.reset(); } @@ -282,11 +299,30 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::CoordinateVector path; int num_iterations = 0; std::string error; + std::unique_ptr>> expansions = nullptr; + if (_debug_visualizations) { + expansions = std::make_unique>>(); + } // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + path, num_iterations, + _tolerance / static_cast(_costmap->getResolution()), expansions.get())) { + if (_debug_visualizations) { + geometry_msgs::msg::PoseArray msg; + geometry_msgs::msg::Pose msg_pose; + msg.header.stamp = _clock->now(); + msg.header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg.poses.push_back(msg_pose); + } + _expansions_publisher->publish(msg); + } + // Note: If the start is blocked only one iteration will occur before failure if (num_iterations == 1) { throw nav2_core::StartOccupied("Start occupied"); @@ -324,6 +360,38 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( _raw_plan_publisher->publish(plan); } + if (_debug_visualizations) { + // Publish expansions for debug + geometry_msgs::msg::PoseArray msg; + geometry_msgs::msg::Pose msg_pose; + msg.header.stamp = _clock->now(); + msg.header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg.poses.push_back(msg_pose); + } + _expansions_publisher->publish(msg); + + // plot footprint path planned for debug + if (_planned_footprints_publisher->get_subscription_count() > 0) { + auto marker_array = std::make_unique(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now())); + } + + if (marker_array->markers.empty()) { + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + } + _planned_footprints_publisher->publish(std::move(marker_array)); + } + } + // Find how much time we have left to do smoothing steady_clock::time_point b = steady_clock::now(); duration time_span = duration_cast>(b - a); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 3be1a9e897f..aae4666edc5 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -45,6 +45,7 @@ TEST(SmacTest, test_smac_se2) { rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 = std::make_shared("SmacSE2Test"); + nodeSE2->declare_parameter("test.debug_visualizations", rclcpp::ParameterValue(true)); std::shared_ptr costmap_ros = std::make_shared("global_costmap"); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index 9ce99a46b1c..b791f9961c2 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -54,6 +54,7 @@ TEST(SmacTest, test_smac_lattice) { rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice = std::make_shared("SmacLatticeTest"); + nodeLattice->declare_parameter("test.debug_visualizations", rclcpp::ParameterValue(true)); std::shared_ptr costmap_ros = std::make_shared("global_costmap"); From 870b9536fa5572370c62155c96c7987dfc2e3468 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 8 Jan 2024 11:31:08 -0800 Subject: [PATCH 31/38] Update vision_opencv's branch for rolling Signed-off-by: Steve Macenski --- tools/underlay.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index 2e092f25175..96772eafd19 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -14,7 +14,7 @@ repositories: # ros-perception/vision_opencv: # type: git # url: https://github.com/ros-perception/vision_opencv.git - # version: ros2 + # version: rolling # ros/bond_core: # type: git # url: https://github.com/ros/bond_core.git From b4ea7f5843c0c33f9daa874b7c694d19d052cb70 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 8 Jan 2024 16:00:28 -0800 Subject: [PATCH 32/38] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20actions/ch?= =?UTF-8?q?eckout=20from=202=20to=204=20(#4040)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bumps [actions/checkout](https://github.com/actions/checkout) from 2 to 4. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v2...v4) --- updated-dependencies: - dependency-name: actions/checkout dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/lint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 635086a5227..8732b239d9b 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -13,7 +13,7 @@ jobs: matrix: linter: [xmllint, cpplint, uncrustify, pep257, flake8] steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - uses: ros-tooling/action-ros-lint@v0.1 with: linter: ${{ matrix.linter }} From 5f7f83a12168b74d64c3f054de09f2eb1a23adc0 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 10 Jan 2024 11:54:55 -0800 Subject: [PATCH 33/38] linting for new lint bots catching issues not previously reported (#4044) * linting for new lint bots * moar * moar2 * try without dashes * more * more --- nav2_amcl/src/amcl_node.cpp | 10 +++--- nav2_bringup/params/nav2_params.yaml | 32 ++++++++++++++++--- nav2_collision_monitor/src/polygon.cpp | 1 - .../test/collision_detector_node_test.cpp | 4 +-- .../test/collision_monitor_node_test.cpp | 3 +- .../nav2_common/launch/rewritten_yaml.py | 1 + .../nav2_costmap_2d/costmap_2d_ros.hpp | 6 ++-- .../denoise/image_processing.hpp | 2 +- .../include/nav2_costmap_2d/footprint.hpp | 1 + .../nav2_costmap_2d/inflation_layer.hpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 8 +++-- .../integration/test_costmap_2d_publisher.cpp | 2 +- .../integration/test_costmap_subscriber.cpp | 18 +++++++++-- .../test/regression/order_layer.hpp | 6 ++-- .../test/regression/plugin_api_order.cpp | 4 +-- .../test/unit/denoise_layer_test.cpp | 4 +-- .../test/unit/image_processing_test.cpp | 4 +-- nav2_costmap_2d/test/unit/image_test.cpp | 2 +- .../test/unit/image_tests_helper.hpp | 6 ++-- nav2_costmap_2d/test/unit/lifecycle_test.cpp | 4 ++- .../nav2_simple_commander/costmap_2d.py | 8 ----- .../nav2_simple_commander/robot_navigator.py | 1 + .../test/test_footprint_collision_checker.py | 1 + .../test/test_line_iterator.py | 1 + .../src/costmap_filters/tester_node.py | 1 + .../src/gps_navigation/tester.py | 1 + .../system/nav_through_poses_tester_node.py | 1 + .../src/system/nav_to_pose_tester_node.py | 1 + .../src/system_failure/tester_node.py | 1 + .../src/waypoint_follower/tester.py | 1 + 30 files changed, 90 insertions(+), 47 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index dd03b3be022..85dc80b39cb 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1167,7 +1167,7 @@ AmclNode::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { alpha1_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha1_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha1 to be negative," @@ -1177,7 +1177,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha2_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha2 to be negative," @@ -1187,7 +1187,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha3_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha3 to be negative," @@ -1197,7 +1197,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha4_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha4 to be negative," @@ -1207,7 +1207,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha5_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha5 to be negative," diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 5f0e6e775d1..0e3427a17b8 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -277,12 +277,34 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true + GridBased: + plugin: "nav2_smac_planner/SmacPlannerLattice" + allow_unknown: true # Allow traveling in unknown space + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # Max time in s for planner to plan, smooth + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + debug_visualizations: true + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 smoother_server: ros__parameters: smoother_plugins: ["simple_smoother"] diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index f78f9291efe..53480a0974f 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -409,7 +409,6 @@ bool Polygon::getParameters( // Do not need to proceed further, if "points" parameter is defined. // Static polygon will be used. return getPolygonFromString(poly_string, poly_); - } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 1ac519111f1..e36ef6e583e 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -162,9 +162,9 @@ class Tester : public ::testing::Test nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; // CollisionMonitor collision points markers - rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + rclcpp::Subscription::SharedPtr + collision_points_marker_sub_; visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; - }; // Tester Tester::Tester() diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 242b9f72db9..757956f3f8c 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -195,7 +195,8 @@ class Tester : public ::testing::Test nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; // CollisionMonitor collision points markers - rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + rclcpp::Subscription::SharedPtr + collision_points_marker_sub_; visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; // Service client for setting CollisionMonitor parameters diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index c28dc88cc00..11aa77227e3 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -20,6 +20,7 @@ class DictItemReference: + def __init__(self, dictionary, key): self.dictionary = dictionary self.dictKey = key 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 b1ddedceeb2..69f2cfefa15 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 @@ -77,7 +77,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode * @brief Constructor for the wrapper * @param options Additional options to control creation of the node. */ - Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + explicit Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Constructor for the wrapper, the node will @@ -399,12 +399,12 @@ class Costmap2DROS : public nav2_util::LifecycleNode double resolution_{0}; std::string robot_base_frame_; ///< The frame_id of the robot base double robot_radius_; - bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap + bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap bool track_unknown_space_{false}; double transform_tolerance_{0}; ///< The timeout before transform errors double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors - bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node + bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node // Derived parameters bool use_radius_{false}; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp index 99d24240d2c..3a2f6a77d25 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp @@ -81,7 +81,7 @@ void morphologyOperation( using ShapeBuffer3x3 = std::array; // NOLINT inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity); -} // namespace imgproc_impl +} // namespace imgproc_impl /** * @brief Perform morphological dilation diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp index 8a5fc206f16..6590a4682ec 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index aa0e0f13c23..0ce4b008d98 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -131,7 +131,7 @@ class InflationLayer : public Layer /** * @brief If clearing operations should be processed on this layer or not */ - virtual bool isClearable() override {return false;} + bool isClearable() override {return false;} /** * @brief Reset this costmap diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 501f675736e..2a36bab959c 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -289,7 +289,9 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) // Check timeout if (now() > initial_transform_timeout_point) { RCLCPP_ERROR( - get_logger(), "Failed to activate %s because transform from %s to %s did not become available before timeout", + get_logger(), + "Failed to activate %s because " + "transform from %s to %s did not become available before timeout", get_name(), robot_base_frame_.c_str(), global_frame_.c_str()); return nav2_util::CallbackReturn::FAILURE; @@ -523,7 +525,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); - for (auto & layer_pub: layer_publishers_) { + for (auto & layer_pub : layer_publishers_) { layer_pub->updateBounds(x0, xn, y0, yn); } @@ -535,7 +537,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); - for (auto & layer_pub: layer_publishers_) { + for (auto & layer_pub : layer_publishers_) { layer_pub->publishCostmap(); } diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 790466f38b0..68a9875ef61 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -162,7 +162,7 @@ TEST_F(CostmapRosTestFixture, costmap_pub_test) auto costmap_raw = future.get(); // Check first 20 cells of the 10by10 map - ASSERT_TRUE(costmap_raw->data.size() == 100); + ASSERT_EQ(costmap_raw->data.size(), 100u); unsigned int i = 0; for (; i < 7; ++i) { EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE); diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index 65fa120cd60..d674ee76691 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -1,3 +1,17 @@ +// Copyright (c) 2020 Samsung Research +// +// 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 "rclcpp/rclcpp.hpp" @@ -119,7 +133,7 @@ TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) node, costmapToSend.get(), "", topicName, always_send_full_costmap); costmapPublisher->on_activate(); - for (const auto & mapChange :mapChanges) { + for (const auto & mapChange : mapChanges) { for (const auto & observation : mapChange.observations) { costmapToSend->setCost(observation.x, observation.y, observation.cost); } @@ -153,7 +167,7 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) node, costmapToSend.get(), "", topicName, always_send_full_costmap); costmapPublisher->on_activate(); - for (const auto & mapChange :mapChanges) { + for (const auto & mapChange : mapChanges) { for (const auto & observation : mapChange.observations) { costmapToSend->setCost(observation.x, observation.y, observation.cost); } diff --git a/nav2_costmap_2d/test/regression/order_layer.hpp b/nav2_costmap_2d/test/regression/order_layer.hpp index 35bb2aeabb7..01955f0ba9c 100644 --- a/nav2_costmap_2d/test/regression/order_layer.hpp +++ b/nav2_costmap_2d/test/regression/order_layer.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef REGRESSION__ORDER_LAYER_HPP_ +#define REGRESSION__ORDER_LAYER_HPP_ #include "nav2_costmap_2d/layer.hpp" @@ -43,4 +43,4 @@ class OrderLayer : public nav2_costmap_2d::Layer } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ +#endif // REGRESSION__ORDER_LAYER_HPP_ diff --git a/nav2_costmap_2d/test/regression/plugin_api_order.cpp b/nav2_costmap_2d/test/regression/plugin_api_order.cpp index c7baf34453a..f7a00e90d24 100644 --- a/nav2_costmap_2d/test/regression/plugin_api_order.cpp +++ b/nav2_costmap_2d/test/regression/plugin_api_order.cpp @@ -16,8 +16,8 @@ #include #include -#include -#include +#include "gtest/gtest.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" TEST(CostmapPluginsTester, checkPluginAPIOrder) { diff --git a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp index 37c277f900d..558fb26361b 100644 --- a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp +++ b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp @@ -102,9 +102,9 @@ class DenoiseLayerTester : public ::testing::Test nav2_costmap_2d::DenoiseLayer denoise_; }; -} +} // namespace nav2_costmap_2d -using namespace nav2_costmap_2d; +using namespace nav2_costmap_2d; // NOLINT TEST_F(DenoiseLayerTester, removeSinglePixels4way) { const auto in = imageFromString( diff --git a/nav2_costmap_2d/test/unit/image_processing_test.cpp b/nav2_costmap_2d/test/unit/image_processing_test.cpp index cc177d699a2..f56ca581498 100644 --- a/nav2_costmap_2d/test/unit/image_processing_test.cpp +++ b/nav2_costmap_2d/test/unit/image_processing_test.cpp @@ -18,8 +18,8 @@ #include "nav2_costmap_2d/denoise/image_processing.hpp" #include "image_tests_helper.hpp" -using namespace nav2_costmap_2d; -using namespace imgproc_impl; +using namespace nav2_costmap_2d; // NOLINT +using namespace imgproc_impl; // NOLINT struct ImageProcTester : public ::testing::Test { diff --git a/nav2_costmap_2d/test/unit/image_test.cpp b/nav2_costmap_2d/test/unit/image_test.cpp index 83912d06228..7ab796e155c 100644 --- a/nav2_costmap_2d/test/unit/image_test.cpp +++ b/nav2_costmap_2d/test/unit/image_test.cpp @@ -17,7 +17,7 @@ #include "nav2_costmap_2d/denoise/image.hpp" #include "image_tests_helper.hpp" -using namespace nav2_costmap_2d; +using namespace nav2_costmap_2d; // NOLINT struct ImageTester : public ::testing::Test { diff --git a/nav2_costmap_2d/test/unit/image_tests_helper.hpp b/nav2_costmap_2d/test/unit/image_tests_helper.hpp index 3eee735f7ea..2ffe8196baa 100644 --- a/nav2_costmap_2d/test/unit/image_tests_helper.hpp +++ b/nav2_costmap_2d/test/unit/image_tests_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_ -#define NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_ +#ifndef UNIT__IMAGE_TESTS_HELPER_HPP_ +#define UNIT__IMAGE_TESTS_HELPER_HPP_ #include "nav2_costmap_2d/denoise/image.hpp" #include @@ -121,4 +121,4 @@ std::ostream & operator<<(std::ostream & out, const Image & image) } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_ +#endif // UNIT__IMAGE_TESTS_HELPER_HPP_ diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp index a8ab817df3e..e54264ebeae 100644 --- a/nav2_costmap_2d/test/unit/lifecycle_test.cpp +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -1,3 +1,5 @@ +// Copyright (c) 2020 Samsung Research +// // 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 @@ -8,7 +10,7 @@ // 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. +// limitations under the License. Reserved. #include diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py index 1ae7e808b6a..2ddc9cb00fc 100644 --- a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py +++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py @@ -34,14 +34,6 @@ class PyCostmap2D: """ def __init__(self, occupancy_map): - """ - Initialize costmap2D. - - Args: - ---- - occupancy_map (OccupancyGrid): 2D OccupancyGrid Map - - """ self.size_x = occupancy_map.info.width self.size_y = occupancy_map.info.height self.resolution = occupancy_map.info.resolution diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 17d2f130e84..5f9e355a2fe 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -51,6 +51,7 @@ class TaskResult(Enum): class BasicNavigator(Node): + def __init__(self, node_name='basic_navigator', namespace=''): super().__init__(node_name=node_name, namespace=namespace) self.initial_pose = PoseStamped() diff --git a/nav2_simple_commander/test/test_footprint_collision_checker.py b/nav2_simple_commander/test/test_footprint_collision_checker.py index 070a7bc6e4b..8ffc4b653b9 100644 --- a/nav2_simple_commander/test/test_footprint_collision_checker.py +++ b/nav2_simple_commander/test/test_footprint_collision_checker.py @@ -23,6 +23,7 @@ class TestFootprintCollisionChecker(unittest.TestCase): + def test_no_costmap(self): # Test if a type error raised when costmap is not specified yet fcc_ = FootprintCollisionChecker() diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py index 1360c183e7d..8bfb91588b1 100644 --- a/nav2_simple_commander/test/test_line_iterator.py +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -19,6 +19,7 @@ class TestLineIterator(unittest.TestCase): + def test_type_error(self): # Test if a type error raised when passing invalid arguements types self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index da8ab82a20b..3b3f723095b 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -47,6 +47,7 @@ class TestType(Enum): class FilterMask: + def __init__(self, filter_mask: OccupancyGrid): self.filter_mask = filter_mask diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 5e765981ab2..b9fb4725efb 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -30,6 +30,7 @@ class GpsWaypointFollowerTest(Node): + def __init__(self): super().__init__(node_name='nav2_gps_waypoint_tester', namespace='') self.waypoints = None diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py index 5e587ad8fa9..2bcea319313 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -37,6 +37,7 @@ class NavTester(Node): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index cbb2ac20e6b..8fd1bc60342 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -38,6 +38,7 @@ class NavTester(Node): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index 765535c6e5b..9cea2f6118d 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -37,6 +37,7 @@ class NavTester(Node): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 7eb46457065..2a3ef3df537 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -31,6 +31,7 @@ class WaypointFollowerTest(Node): + def __init__(self): super().__init__(node_name='nav2_waypoint_tester', namespace='') self.waypoints = None From 370dbfd5c8b26ebdb45d99bb3613166b0c5e79b8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 11 Jan 2024 00:59:35 +0100 Subject: [PATCH 34/38] Bring back pep257 docstring (#4045) * test Signed-off-by: Tony Najjar * . Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- .../nav2_simple_commander/costmap_2d.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py index 2ddc9cb00fc..ad4014522aa 100644 --- a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py +++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py @@ -34,6 +34,18 @@ class PyCostmap2D: """ def __init__(self, occupancy_map): + """ + Initialize costmap2D. + + Args + ---- + occupancy_map (OccupancyGrid): 2D OccupancyGrid Map + + Returns + ------- + None + + """ self.size_x = occupancy_map.info.width self.size_y = occupancy_map.info.height self.resolution = occupancy_map.info.resolution From 569f1672609c22bc52c4c4c486818fde3bd99646 Mon Sep 17 00:00:00 2001 From: Sebastian Solarte <89881453+Sunart24@users.noreply.github.com> Date: Thu, 11 Jan 2024 14:08:57 -0500 Subject: [PATCH 35/38] handle dynamically changes in parameters. (#4046) Signed-off-by: Sebastian Solarte --- nav2_mppi_controller/src/optimizer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3b053f45771..3c5d2032892 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -123,6 +123,8 @@ void Optimizer::reset() control_history_[2] = {0.0, 0.0, 0.0}; control_history_[3] = {0.0, 0.0, 0.0}; + settings_.constraints = settings_.base_constraints; + costs_ = xt::zeros({settings_.batch_size}); generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); From 29321d661f6acb505192d4b24aa983fe10a733c5 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Fri, 12 Jan 2024 13:02:39 +0900 Subject: [PATCH 36/38] Add inflation_layer_name param (#4047) Signed-off-by: Renan Salles --- nav2_mppi_controller/README.md | 1 + .../nav2_mppi_controller/critics/obstacles_critic.hpp | 4 +++- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 6 +++++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 4f5a9398d1e..fd7ad802142 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -107,6 +107,7 @@ This process is then repeated a number of times and returns a converged solution | collision_cost | double | Default 100000.0. Cost to apply to a true collision in a trajectory. | | collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. | | 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 | diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index fd6eeab0128..358994c238b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -16,9 +16,10 @@ #define NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_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" @@ -96,6 +97,7 @@ class ObstaclesCritic : public CriticFunction unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; + std::string inflation_layer_name_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 274315a06b7..ecb22b295b5 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -29,6 +29,7 @@ void ObstaclesCritic::initialize() getParam(collision_cost_, "collision_cost", 100000.0); getParam(collision_margin_distance_, "collision_margin_distance", 0.10); getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); @@ -69,7 +70,10 @@ float ObstaclesCritic::findCircumscribedCost( ++layer) { auto inflation_layer = std::dynamic_pointer_cast(*layer); - if (!inflation_layer) { + if (!inflation_layer || + (!inflation_layer_name_.empty() && + inflation_layer->getName() != inflation_layer_name_)) + { continue; } From a8a0c3a7feae45a3c44e0d22d69c0c4d13ba0c05 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Sun, 14 Jan 2024 16:01:37 +0100 Subject: [PATCH 37/38] missing urdf dep (#4050) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- nav2_rviz_plugins/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 3de0d3c9f54..b4d0a8ed01f 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -25,6 +25,7 @@ rviz_rendering std_msgs tf2_geometry_msgs + urdf visualization_msgs yaml_cpp_vendor From 6a540f197da316bafb34182ed92f06f7a6622b1b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Sat, 20 Jan 2024 11:59:35 +0000 Subject: [PATCH 38/38] 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 --- nav2_smac_planner/src/collision_checker.cpp | 4 ++-- nav2_smac_planner/test/test_a_star.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 20d288809da..328a88f9db3 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -107,7 +107,7 @@ bool GridCollisionChecker::inCollision( // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision footprint_cost_ = costmap_->getCost( - static_cast(x), static_cast(y)); + static_cast(x + 0.5), static_cast(y + 0.5)); if (footprint_cost_ < possible_inscribed_cost_) { if (possible_inscribed_cost_ > 0) { @@ -157,7 +157,7 @@ bool GridCollisionChecker::inCollision( } else { // if radius, then we can check the center of the cost assuming inflation is used footprint_cost_ = costmap_->getCost( - static_cast(x), static_cast(y)); + static_cast(x + 0.5), static_cast(y + 0.5)); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index f048cc55503..730193f8027 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -164,8 +164,8 @@ TEST(AStarTest, test_a_star_se2) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, expansions.get())); // check path is the right size and collision free - EXPECT_EQ(num_it, 3186); - EXPECT_EQ(path.size(), 64u); + EXPECT_EQ(num_it, 3146); + EXPECT_EQ(path.size(), 63u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -229,8 +229,8 @@ TEST(AStarTest, test_a_star_lattice) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); // check path is the right size and collision free - EXPECT_EQ(num_it, 26); - EXPECT_GT(path.size(), 47u); + EXPECT_EQ(num_it, 22); + EXPECT_GT(path.size(), 45u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); }