From 7179777a28324d67e841ea84d053f958390b7a79 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Tue, 12 Aug 2025 13:57:23 +0000 Subject: [PATCH 01/62] Add path goal checker plugin Signed-off-by: mini-1235 --- nav2_controller/CMakeLists.txt | 31 ++++- .../nav2_controller/controller_server.hpp | 6 + .../include/nav2_controller}/path_handler.hpp | 17 ++- .../plugins/path_complete_goal_checker.hpp | 82 ++++++++++++ nav2_controller/plugins.xml | 5 + .../plugins/path_complete_goal_checker.cpp | 109 +++++++++++++++ nav2_controller/src/controller_server.cpp | 27 +++- .../src/path_handler.cpp | 6 +- nav2_graceful_controller/CMakeLists.txt | 1 - .../graceful_controller.hpp | 6 +- .../nav2_graceful_controller/path_handler.hpp | 83 ------------ .../src/graceful_controller.cpp | 22 +-- nav2_graceful_controller/src/path_handler.cpp | 126 ------------------ .../CMakeLists.txt | 3 +- .../regulated_pure_pursuit_controller.hpp | 6 +- .../src/regulated_pure_pursuit_controller.cpp | 19 +-- 16 files changed, 277 insertions(+), 272 deletions(-) rename {nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller => nav2_controller/include/nav2_controller}/path_handler.hpp (80%) create mode 100644 nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp create mode 100644 nav2_controller/plugins/path_complete_goal_checker.cpp rename {nav2_regulated_pure_pursuit_controller => nav2_controller}/src/path_handler.cpp (96%) delete mode 100644 nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp delete mode 100644 nav2_graceful_controller/src/path_handler.cpp diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index c8a655f4340..2b570187afb 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -26,6 +26,7 @@ set(executable_name controller_server) set(library_name ${executable_name}_core) add_library(${library_name} SHARED + src/path_handler.cpp src/controller_server.cpp ) target_include_directories(${library_name} @@ -167,6 +168,26 @@ target_link_libraries(position_goal_checker PRIVATE pluginlib::pluginlib ) +add_library(path_complete_goal_checker SHARED plugins/path_complete_goal_checker.cpp) +target_include_directories(path_complete_goal_checker + PUBLIC + "$" + "$" +) +target_link_libraries(path_complete_goal_checker PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + simple_goal_checker +) +target_link_libraries(path_complete_goal_checker PRIVATE + nav2_util::nav2_util_core + pluginlib::pluginlib +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -183,7 +204,14 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name} +install(TARGETS + simple_progress_checker + position_goal_checker + pose_progress_checker + simple_goal_checker + stopped_goal_checker + path_complete_goal_checker + ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -204,6 +232,7 @@ ament_export_libraries(simple_progress_checker simple_goal_checker stopped_goal_checker position_goal_checker + path_complete_goal_checker ${library_name}) ament_export_dependencies( geometry_msgs diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index b68c8e5bb94..414e0638c2a 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -36,6 +36,7 @@ #include "nav2_util/twist_publisher.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" +#include "nav2_controller/path_handler.hpp" namespace nav2_controller { @@ -282,6 +283,11 @@ class ControllerServer : public nav2::LifecycleNode // Current path container nav_msgs::msg::Path current_path_; + nav_msgs::msg::Path local_path_; + bool interpolate_curvature_after_goal_; + double max_robot_pose_search_dist_; + std::unique_ptr path_handler_; + nav2::Publisher::SharedPtr global_path_pub_; private: /** diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_controller/include/nav2_controller/path_handler.hpp similarity index 80% rename from nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp rename to nav2_controller/include/nav2_controller/path_handler.hpp index 135a304dc26..62f6a6d2757 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_controller/include/nav2_controller/path_handler.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ -#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ +#ifndef NAV2_CONTROLLER__PATH_HANDLER_HPP_ +#define NAV2_CONTROLLER__PATH_HANDLER_HPP_ #include #include @@ -29,18 +29,18 @@ #include "nav2_core/controller_exceptions.hpp" #include "geometry_msgs/msg/pose.hpp" -namespace nav2_regulated_pure_pursuit_controller +namespace nav2_controller { /** - * @class nav2_regulated_pure_pursuit_controller::PathHandler + * @class nav2_controller::PathHandler * @brief Handles input paths to transform them to local frames required */ class PathHandler { public: /** - * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler + * @brief Constructor for nav2_controller::PathHandler */ PathHandler( double transform_tolerance, @@ -48,7 +48,7 @@ class PathHandler std::shared_ptr costmap_ros); /** - * @brief Destrructor for nav2_regulated_pure_pursuit_controller::PathHandler + * @brief Destrructor for nav2_controller::PathHandler */ ~PathHandler() = default; @@ -76,13 +76,12 @@ class PathHandler */ double getCostmapMaxExtent() const; - rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")}; double transform_tolerance_; std::shared_ptr tf_; std::shared_ptr costmap_ros_; nav_msgs::msg::Path global_plan_; }; -} // namespace nav2_regulated_pure_pursuit_controller +} // namespace nav2_controller -#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ +#endif // NAV2_CONTROLLER__PATH_HANDLER_HPP_ diff --git a/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp new file mode 100644 index 00000000000..7fdb1328084 --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp @@ -0,0 +1,82 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Joseph Duchesne + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_ + +#include +#include +#include + +#include "nav2_controller/plugins/simple_goal_checker.hpp" + +namespace nav2_controller +{ + +/** + * @class PathCompleteGoalChecker + * @brief Goal Checker plugin that checks position delta, once path is shorter than a threshold. + */ +class PathCompleteGoalChecker : public SimpleGoalChecker +{ +public: + PathCompleteGoalChecker(); + + // Standard GoalChecker Interface + void initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) override; + + void reset() override; + + bool isGoalReached( + const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; + +protected: + // minimum remaining path length before checking position goals + double path_length_tolerance_; + + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); +}; + +} // namespace nav2_controller + +#endif // NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index e3e9c7cf63b..8ccf01fc51f 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -24,4 +24,9 @@ Goal checker that only checks XY position and ignores orientation + + + Checks if path is below a threshold in length, and then if the current pose is within goal window for x, y and yaw + + diff --git a/nav2_controller/plugins/path_complete_goal_checker.cpp b/nav2_controller/plugins/path_complete_goal_checker.cpp new file mode 100644 index 00000000000..981e8d209d6 --- /dev/null +++ b/nav2_controller/plugins/path_complete_goal_checker.cpp @@ -0,0 +1,109 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Joseph Duchesne + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "nav2_controller/plugins/path_complete_goal_checker.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace nav2_controller +{ + +PathCompleteGoalChecker::PathCompleteGoalChecker() +: SimpleGoalChecker(), path_length_tolerance_(1.0) +{ +} + +void PathCompleteGoalChecker::initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) +{ + SimpleGoalChecker::initialize(parent, plugin_name, costmap_ros); + + auto node = parent.lock(); + + nav2::declare_parameter_if_not_declared( + node, + plugin_name + ".path_length_tolerance", + rclcpp::ParameterValue(path_length_tolerance_)); + + node->get_parameter(plugin_name + ".path_length_tolerance", path_length_tolerance_); + + // Replace SimpleGoalChecker's callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&PathCompleteGoalChecker::dynamicParametersCallback, this, _1)); +} + +void PathCompleteGoalChecker::reset() +{ + SimpleGoalChecker::reset(); +} + +bool PathCompleteGoalChecker::isGoalReached( + const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist & twist, const nav_msgs::msg::Path & path) +{ + if(nav2_util::geometry_utils::calculate_path_length(path) > path_length_tolerance_){ + return false; + } + // otherwise defer to SimpleGoalChecker's isGoalReached + return SimpleGoalChecker::isGoalReached(query_pose, goal_pose, twist, path); +} + +rcl_interfaces::msg::SetParametersResult +PathCompleteGoalChecker::dynamicParametersCallback(std::vector parameters) +{ + auto result = rcl_interfaces::msg::SetParametersResult(); + + for (auto & parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".path_length_tolerance") { + path_length_tolerance_ = parameter.as_double(); + } + } + } + result.successful = true; + return result; +} + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::PathCompleteGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 98a6eda8629..d313ce084da 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -28,6 +28,7 @@ using namespace std::chrono_literals; using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; +using nav2_util::geometry_utils::euclidean_distance; namespace nav2_controller { @@ -39,7 +40,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) default_progress_checker_types_{"nav2_controller::SimpleProgressChecker"}, goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), default_goal_checker_ids_{"goal_checker"}, - default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}, + default_goal_checker_types_{"nav2_controller::PathCompleteGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"}, @@ -65,6 +66,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("odom_topic", rclcpp::ParameterValue("odom")); declare_parameter("odom_duration", rclcpp::ParameterValue(0.3)); + declare_parameter("interpolate_curvature_after_goal", rclcpp::ParameterValue(false)); // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -134,10 +136,15 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("failure_tolerance", failure_tolerance_); get_parameter("use_realtime_priority", use_realtime_priority_); get_parameter("publish_zero_velocity", publish_zero_velocity_); + get_parameter("interpolate_curvature_after_goal", interpolate_curvature_after_goal_); costmap_ros_->configure(); // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); + declare_parameter("max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_ros_->getCostmap()->getSizeInMetersX() / 2.0)); + get_parameter("max_robot_pose_search_dist", max_robot_pose_search_dist_); + path_handler_ = std::make_unique( + costmap_ros_->getTransformTolerance(), costmap_ros_->getTfBuffer(), costmap_ros_); for (size_t i = 0; i != progress_checker_ids_.size(); i++) { try { @@ -225,6 +232,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) odom_sub_ = std::make_unique(node, odom_duration, odom_topic); vel_publisher_ = std::make_unique(node, "cmd_vel"); + global_path_pub_ = node->create_publisher("received_global_plan"); double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); @@ -267,6 +275,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) it->second->activate(); } vel_publisher_->on_activate(); + global_path_pub_->on_activate(); action_server_->activate(); auto node = shared_from_this(); @@ -302,6 +311,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) publishZeroVelocity(); vel_publisher_->on_deactivate(); + global_path_pub_->on_deactivate(); remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_.reset(); @@ -335,6 +345,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) odom_sub_.reset(); costmap_thread_.reset(); vel_publisher_.reset(); + global_path_pub_.reset(); speed_limit_sub_.reset(); return nav2::CallbackReturn::SUCCESS; @@ -604,6 +615,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) throw nav2_core::InvalidPath("Path is empty."); } controllers_[current_controller_]->setPlan(path); + path_handler_->setPlan(path); end_pose_ = path.poses.back(); end_pose_.header.frame_id = path.header.frame_id; @@ -630,6 +642,11 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); + auto transformed_plan = path_handler_->transformGlobalPlan( + pose, max_robot_pose_search_dist_, interpolate_curvature_after_goal_); + // RCLCPP_INFO(get_logger(), "compute remaining distance %lf ",nav2_util::geometry_utils::calculate_path_length(transformed_plan)); + global_path_pub_->publish(transformed_plan); + geometry_msgs::msg::TwistStamped cmd_vel_2d; try { @@ -637,7 +654,9 @@ void ControllerServer::computeAndPublishVelocity() controllers_[current_controller_]->computeVelocityCommands( pose, twist, - goal_checkers_[current_goal_checker_].get()); + goal_checkers_[current_goal_checker_].get(), + transformed_plan + ); last_valid_cmd_time_ = now(); cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); cmd_vel_2d.header.stamp = last_valid_cmd_time_; @@ -686,7 +705,7 @@ void ControllerServer::computeAndPublishVelocity() double curr_min_dist = std::numeric_limits::max(); for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { double curr_dist = - nav2_util::geometry_utils::euclidean_distance(robot_pose_in_path_frame, + euclidean_distance(robot_pose_in_path_frame, current_path.poses[curr_idx]); if (curr_dist < curr_min_dist) { curr_min_dist = curr_dist; @@ -806,7 +825,7 @@ bool ControllerServer::isGoalReached() return goal_checkers_[current_goal_checker_]->isGoalReached( pose.pose, transformed_end_pose.pose, - velocity); + velocity, local_path_); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_controller/src/path_handler.cpp similarity index 96% rename from nav2_regulated_pure_pursuit_controller/src/path_handler.cpp rename to nav2_controller/src/path_handler.cpp index c91bb8dc8f7..57488fee6a5 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_controller/src/path_handler.cpp @@ -19,13 +19,13 @@ #include #include -#include "nav2_regulated_pure_pursuit_controller/path_handler.hpp" +#include "nav2_controller/path_handler.hpp" #include "nav2_core/controller_exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" -namespace nav2_regulated_pure_pursuit_controller +namespace nav2_controller { using nav2_util::geometry_utils::euclidean_distance; @@ -133,4 +133,4 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( return transformed_plan; } -} // namespace nav2_regulated_pure_pursuit_controller +} // namespace nav2_controller diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt index 95d682a8cad..7d412464aec 100644 --- a/nav2_graceful_controller/CMakeLists.txt +++ b/nav2_graceful_controller/CMakeLists.txt @@ -43,7 +43,6 @@ set(library_name nav2_graceful_controller) add_library(${library_name} SHARED src/graceful_controller.cpp src/parameter_handler.cpp - src/path_handler.cpp src/utils.cpp ) target_include_directories(${library_name} diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 28bec7eddd8..519cf780d80 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -27,7 +27,6 @@ #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_graceful_controller/path_handler.hpp" #include "nav2_graceful_controller/parameter_handler.hpp" #include "nav2_graceful_controller/smooth_control_law.hpp" #include "nav2_graceful_controller/utils.hpp" @@ -89,7 +88,8 @@ class GracefulController : public nav2_core::Controller geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, - nav2_core::GoalChecker * goal_checker) override; + nav2_core::GoalChecker * goal_checker, + nav_msgs::msg::Path & transformed_global_plan) override; /** * @brief nav2_core setPlan - Sets the global plan. @@ -189,11 +189,9 @@ class GracefulController : public nav2_core::Controller // True from the time a new path arrives until we have completed an initial rotation bool do_initial_rotation_; - nav2::Publisher::SharedPtr transformed_plan_pub_; nav2::Publisher::SharedPtr local_plan_pub_; nav2::Publisher::SharedPtr motion_target_pub_; nav2::Publisher::SharedPtr slowdown_pub_; - std::unique_ptr path_handler_; std::unique_ptr param_handler_; std::unique_ptr control_law_; }; diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp deleted file mode 100644 index a24a84c2228..00000000000 --- a/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright (c) 2022 Samsung Research America -// -// 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_GRACEFUL_CONTROLLER__PATH_HANDLER_HPP_ -#define NAV2_GRACEFUL_CONTROLLER__PATH_HANDLER_HPP_ - -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_util/geometry_utils.hpp" - -namespace nav2_graceful_controller -{ - -/** - * @class nav2_graceful_controller::PathHandler - * @brief Handles input paths to transform them to local frames required - */ -class PathHandler -{ -public: - /** - * @brief Constructor for nav2_graceful_controller::PathHandler - */ - PathHandler( - double transform_tolerance, - std::shared_ptr tf, - std::shared_ptr costmap_ros); - - /** - * @brief Destructor for nav2_graceful_controller::PathHandler - */ - ~PathHandler() = default; - - /** - * @brief Transforms global plan into same frame as pose and clips poses ineligible for motionTarget - * Points ineligible to be selected as a motion target point if they are any of the following: - * - Outside the local_costmap (collision avoidance cannot be assured) - * @param pose pose to transform - * @param max_robot_pose_search_dist Distance to search for matching nearest path point - * @return Path in new frame - */ - nav_msgs::msg::Path transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist); - - /** - * @brief Sets the global plan - * - * @param path The global plan - */ - void setPlan(const nav_msgs::msg::Path & path); - - /** - * @brief Gets the global plan - * - * @return The global plan - */ - nav_msgs::msg::Path getPlan() {return global_plan_;} - -protected: - double transform_tolerance_{0}; - std::shared_ptr tf_buffer_; - std::shared_ptr costmap_ros_; - nav_msgs::msg::Path global_plan_; - rclcpp::Logger logger_ {rclcpp::get_logger("GracefulPathHandler")}; -}; - -} // namespace nav2_graceful_controller - -#endif // NAV2_GRACEFUL_CONTROLLER__PATH_HANDLER_HPP_ diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index b8cce0b0aed..432ce1998c2 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -47,10 +47,6 @@ void GracefulController::configure( costmap_ros_->getCostmap()->getSizeInMetersX()); params_ = param_handler_->getParams(); - // Handles global path transformations - path_handler_ = std::make_unique( - params_->transform_tolerance, tf_buffer_, costmap_ros_); - // Handles the control law to generate the velocity commands control_law_ = std::make_unique( params_->k_phi, params_->k_delta, params_->beta, params_->lambda, params_->slowdown_radius, @@ -63,7 +59,6 @@ void GracefulController::configure( } // Publishers - transformed_plan_pub_ = node->create_publisher("transformed_global_plan"); local_plan_pub_ = node->create_publisher("local_plan"); motion_target_pub_ = node->create_publisher("motion_target"); slowdown_pub_ = node->create_publisher("slowdown"); @@ -77,12 +72,10 @@ void GracefulController::cleanup() logger_, "Cleaning up controller: %s of type graceful_controller::GracefulController", plugin_name_.c_str()); - transformed_plan_pub_.reset(); local_plan_pub_.reset(); motion_target_pub_.reset(); slowdown_pub_.reset(); collision_checker_.reset(); - path_handler_.reset(); param_handler_.reset(); control_law_.reset(); } @@ -93,7 +86,6 @@ void GracefulController::activate() logger_, "Activating controller: %s of type nav2_graceful_controller::GracefulController", plugin_name_.c_str()); - transformed_plan_pub_->on_activate(); local_plan_pub_->on_activate(); motion_target_pub_->on_activate(); slowdown_pub_->on_activate(); @@ -105,7 +97,6 @@ void GracefulController::deactivate() logger_, "Deactivating controller: %s of type nav2_graceful_controller::GracefulController", plugin_name_.c_str()); - transformed_plan_pub_->on_deactivate(); local_plan_pub_->on_deactivate(); motion_target_pub_->on_deactivate(); slowdown_pub_->on_deactivate(); @@ -114,7 +105,8 @@ void GracefulController::deactivate() geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & /*velocity*/, - nav2_core::GoalChecker * goal_checker) + nav2_core::GoalChecker * goal_checker, + nav_msgs::msg::Path & transformed_plan) { std::lock_guard param_lock(param_handler_->getMutex()); @@ -136,16 +128,9 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( control_law_->setSlowdownRadius(params_->slowdown_radius); control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); - // Transform path to robot base frame - auto transformed_plan = path_handler_->transformGlobalPlan( - pose, params_->max_robot_pose_search_dist); - // Add proper orientations to plan, if needed validateOrientations(transformed_plan.poses); - // Publish plan for visualization - transformed_plan_pub_->publish(transformed_plan); - // Transform local frame to global frame to use in collision checking geometry_msgs::msg::TransformStamped costmap_transform; try { @@ -245,9 +230,8 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( throw nav2_core::NoValidControl("Collision detected in trajectory"); } -void GracefulController::setPlan(const nav_msgs::msg::Path & path) +void GracefulController::setPlan(const nav_msgs::msg::Path & /*path*/) { - path_handler_->setPlan(path); goal_reached_ = false; do_initial_rotation_ = true; } diff --git a/nav2_graceful_controller/src/path_handler.cpp b/nav2_graceful_controller/src/path_handler.cpp deleted file mode 100644 index b60b4a874f0..00000000000 --- a/nav2_graceful_controller/src/path_handler.cpp +++ /dev/null @@ -1,126 +0,0 @@ -// Copyright (c) 2022 Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include - -#include "nav2_core/controller_exceptions.hpp" -#include "nav2_ros_common/node_utils.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav2_util/robot_utils.hpp" -#include "nav2_graceful_controller/path_handler.hpp" - -namespace nav2_graceful_controller -{ - -using nav2_util::geometry_utils::euclidean_distance; - -PathHandler::PathHandler( - double transform_tolerance, - std::shared_ptr tf, - std::shared_ptr costmap_ros) -: transform_tolerance_(transform_tolerance), tf_buffer_(tf), costmap_ros_(costmap_ros) -{ -} - -nav_msgs::msg::Path PathHandler::transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist) -{ - // Check first if the plan is empty - if (global_plan_.poses.empty()) { - throw nav2_core::InvalidPath("Received plan with zero length"); - } - - // Let's get the pose of the robot in the frame of the plan - geometry_msgs::msg::PoseStamped robot_pose; - if (!nav2_util::transformPoseInTargetFrame( - pose, robot_pose, *tf_buffer_, global_plan_.header.frame_id, - transform_tolerance_)) - { - throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); - } - - // Find the first pose in the global plan that's further than max_robot_pose_search_dist - // from the robot using integrated distance - auto closest_pose_upper_bound = - nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist); - - // First find the closest pose on the path to the robot - // bounded by when the path turns around (if it does) so we don't get a pose from a later - // portion of the path - auto transformation_begin = - nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), closest_pose_upper_bound, - [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(robot_pose, ps); - }); - - // We'll discard points on the plan that are outside the local costmap - double dist_threshold = std::max( - costmap_ros_->getCostmap()->getSizeInMetersX(), - costmap_ros_->getCostmap()->getSizeInMetersY()) / 2.0; - auto transformation_end = std::find_if( - transformation_begin, global_plan_.poses.end(), - [&](const auto & global_plan_pose) { - return euclidean_distance(global_plan_pose, robot_pose) > dist_threshold; - }); - - // Lambda to transform a PoseStamped from global frame to local - auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_.header.frame_id; - stamped_pose.header.stamp = robot_pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - if (!nav2_util::transformPoseInTargetFrame( - stamped_pose, transformed_pose, *tf_buffer_, - costmap_ros_->getBaseFrameID(), transform_tolerance_)) - { - throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); - } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = robot_pose.header.stamp; - std::transform( - transformation_begin, transformation_end, - std::back_inserter(transformed_plan.poses), - transformGlobalPoseToLocal); - - // Remove the portion of the global plan that we've already passed so we don't - // process it on the next iteration (this is called path pruning) - global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); - - if (transformed_plan.poses.empty()) { - throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); - } - - return transformed_plan; -} - -void PathHandler::setPlan(const nav_msgs::msg::Path & path) -{ - global_plan_ = path; -} - -} // namespace nav2_graceful_controller diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index bec081d3987..a6323b1dd18 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -26,8 +26,7 @@ set(library_name nav2_regulated_pure_pursuit_controller) add_library(${library_name} SHARED src/regulated_pure_pursuit_controller.cpp src/collision_checker.cpp - src/parameter_handler.cpp - src/path_handler.cpp) + src/parameter_handler.cpp) target_include_directories(${library_name} PUBLIC "$" diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index fd79208d291..68f0735c587 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -28,7 +28,6 @@ #include "pluginlib/class_list_macros.hpp" #include "geometry_msgs/msg/pose.hpp" #include "std_msgs/msg/bool.hpp" -#include "nav2_regulated_pure_pursuit_controller/path_handler.hpp" #include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" #include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" #include "nav2_regulated_pure_pursuit_controller/regulation_functions.hpp" @@ -95,7 +94,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, - nav2_core::GoalChecker * /*goal_checker*/) override; + nav2_core::GoalChecker * /*goal_checker*/, + nav_msgs::msg::Path & transformed_plan) override; bool cancel() override; @@ -196,12 +196,10 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool is_rotating_to_heading_ = false; bool has_reached_xy_tolerance_ = false; - nav2::Publisher::SharedPtr global_path_pub_; nav2::Publisher::SharedPtr carrot_pub_; nav2::Publisher::SharedPtr curvature_carrot_pub_; nav2::Publisher::SharedPtr is_rotating_to_heading_pub_; nav2::Publisher::SharedPtr carrot_arc_pub_; - std::unique_ptr path_handler_; std::unique_ptr param_handler_; std::unique_ptr collision_checker_; }; 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 fced1dff734..75b04c2f6d8 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 @@ -60,10 +60,6 @@ void RegulatedPurePursuitController::configure( node, plugin_name_, logger_, costmap_->getSizeInMetersX()); params_ = param_handler_->getParams(); - // Handles global path transformations - path_handler_ = std::make_unique( - params_->transform_tolerance, tf_, costmap_ros_); - // Checks for imminent collisions collision_checker_ = std::make_unique(node, costmap_ros_, params_); @@ -73,7 +69,6 @@ void RegulatedPurePursuitController::configure( node->get_parameter("controller_frequency", control_frequency); control_duration_ = 1.0 / control_frequency; - global_path_pub_ = node->create_publisher("received_global_plan"); carrot_pub_ = node->create_publisher("lookahead_point"); curvature_carrot_pub_ = node->create_publisher( "curvature_lookahead_point"); @@ -88,7 +83,6 @@ void RegulatedPurePursuitController::cleanup() "Cleaning up controller: %s of type" " regulated_pure_pursuit_controller::RegulatedPurePursuitController", plugin_name_.c_str()); - global_path_pub_.reset(); carrot_pub_.reset(); curvature_carrot_pub_.reset(); is_rotating_to_heading_pub_.reset(); @@ -101,7 +95,6 @@ void RegulatedPurePursuitController::activate() "Activating controller: %s of type " "regulated_pure_pursuit_controller::RegulatedPurePursuitController", plugin_name_.c_str()); - global_path_pub_->on_activate(); carrot_pub_->on_activate(); curvature_carrot_pub_->on_activate(); is_rotating_to_heading_pub_->on_activate(); @@ -114,7 +107,6 @@ void RegulatedPurePursuitController::deactivate() "Deactivating controller: %s of type " "regulated_pure_pursuit_controller::RegulatedPurePursuitController", plugin_name_.c_str()); - global_path_pub_->on_deactivate(); carrot_pub_->on_deactivate(); curvature_carrot_pub_->on_deactivate(); is_rotating_to_heading_pub_->on_deactivate(); @@ -165,7 +157,8 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point) geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & speed, - nav2_core::GoalChecker * goal_checker) + nav2_core::GoalChecker * goal_checker, + nav_msgs::msg::Path & transformed_plan) { std::lock_guard lock_reinit(param_handler_->getMutex()); @@ -181,11 +174,6 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity goal_dist_tol_ = pose_tolerance.position.x; } - // Transform path to robot base frame - auto transformed_plan = path_handler_->transformGlobalPlan( - pose, params_->max_robot_pose_search_dist, params_->interpolate_curvature_after_goal); - global_path_pub_->publish(transformed_plan); - // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); double curv_lookahead_dist = params_->curvature_lookahead_dist; @@ -390,10 +378,9 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = sign * linear_vel; } -void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & /*path*/) { has_reached_xy_tolerance_ = false; - path_handler_->setPlan(path); } void RegulatedPurePursuitController::setSpeedLimit( From 7cb5903c9ae5c39ad92a23bba9ae812fcab3adc0 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 16 Aug 2025 18:34:04 +0000 Subject: [PATCH 02/62] Use local plan's last pose as end pose Signed-off-by: mini-1235 --- nav2_controller/src/controller_server.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index d313ce084da..f595ae926c9 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -617,14 +617,6 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) controllers_[current_controller_]->setPlan(path); path_handler_->setPlan(path); - end_pose_ = path.poses.back(); - end_pose_.header.frame_id = path.header.frame_id; - goal_checkers_[current_goal_checker_]->reset(); - - RCLCPP_DEBUG( - get_logger(), "Path end point is (%.2f, %.2f)", - end_pose_.pose.position.x, end_pose_.pose.position.y); - current_path_ = path; } @@ -646,6 +638,10 @@ void ControllerServer::computeAndPublishVelocity() pose, max_robot_pose_search_dist_, interpolate_curvature_after_goal_); // RCLCPP_INFO(get_logger(), "compute remaining distance %lf ",nav2_util::geometry_utils::calculate_path_length(transformed_plan)); global_path_pub_->publish(transformed_plan); + local_path_ = transformed_plan; + end_pose_ = local_path_.poses.back(); + end_pose_.header.frame_id = local_path_.header.frame_id; + goal_checkers_[current_goal_checker_]->reset(); geometry_msgs::msg::TwistStamped cmd_vel_2d; From 5da8e04e9c1ff27d10e46c1df2ae0dd06695a9da Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 20 Sep 2025 14:11:09 +0000 Subject: [PATCH 03/62] Move path handler from plugin to controller level Signed-off-by: mini-1235 --- nav2_controller/CMakeLists.txt | 1 + .../nav2_controller/parameter_handler.hpp | 112 +++ .../include/nav2_controller/path_handler.hpp | 2 +- .../plugins/position_goal_checker.hpp | 2 +- .../plugins/simple_goal_checker.hpp | 4 +- .../plugins/stopped_goal_checker.hpp | 2 +- .../plugins/path_complete_goal_checker.cpp | 1 + .../plugins/position_goal_checker.cpp | 2 +- .../plugins/simple_goal_checker.cpp | 2 +- .../plugins/stopped_goal_checker.cpp | 4 +- nav2_controller/plugins/test/CMakeLists.txt | 2 +- nav2_controller/plugins/test/goal_checker.cpp | 790 ++++++++++-------- nav2_controller/src/controller_server.cpp | 2 +- nav2_controller/src/parameter_handler.cpp | 327 ++++++++ nav2_controller/src/path_handler.cpp | 33 +- nav2_core/include/nav2_core/controller.hpp | 3 +- nav2_core/include/nav2_core/goal_checker.hpp | 7 +- .../include/dwb_core/dwb_local_planner.hpp | 20 +- .../dwb_core/src/dwb_local_planner.cpp | 113 +-- nav2_graceful_controller/README.md | 1 - .../parameter_handler.hpp | 3 +- .../src/graceful_controller.cpp | 3 +- .../src/parameter_handler.cpp | 13 +- .../test/test_graceful_controller.cpp | 68 +- nav2_mppi_controller/CMakeLists.txt | 1 - .../benchmark/controller_benchmark.cpp | 4 +- .../nav2_mppi_controller/controller.hpp | 5 +- .../tools/path_handler.hpp | 157 ---- nav2_mppi_controller/src/controller.cpp | 16 +- nav2_mppi_controller/src/path_handler.cpp | 205 ----- nav2_mppi_controller/test/CMakeLists.txt | 1 - .../test/controller_state_transition_test.cpp | 4 +- .../test/path_handler_test.cpp | 247 ------ nav2_mppi_controller/test/utils/factory.hpp | 13 - nav2_mppi_controller/test/utils_test.cpp | 3 +- .../README.md | 4 +- .../parameter_handler.hpp | 1 - .../src/parameter_handler.cpp | 14 - .../test/test_regulated_pp.cpp | 374 +-------- nav2_rotation_shim_controller/README.md | 2 +- .../nav2_rotation_shim_controller.hpp | 3 +- .../src/nav2_rotation_shim_controller.cpp | 7 +- .../test/test_shim_controller.cpp | 24 +- .../controller/controller_error_plugins.hpp | 18 +- 44 files changed, 976 insertions(+), 1644 deletions(-) create mode 100644 nav2_controller/include/nav2_controller/parameter_handler.hpp create mode 100644 nav2_controller/src/parameter_handler.cpp delete mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp delete mode 100644 nav2_mppi_controller/src/path_handler.cpp delete mode 100644 nav2_mppi_controller/test/path_handler_test.cpp diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 2b570187afb..7efc169aa9a 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -26,6 +26,7 @@ set(executable_name controller_server) set(library_name ${executable_name}_core) add_library(${library_name} SHARED + src/parameter_handler.cpp src/path_handler.cpp src/controller_server.cpp ) diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp new file mode 100644 index 00000000000..0d89c1d22bd --- /dev/null +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -0,0 +1,112 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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_CONTROLLER__PARAMETER_HANDLER_HPP_ +#define NAV2_CONTROLLER__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_ros_common/node_utils.hpp" + +namespace nav2_controller +{ + +struct Parameters +{ + double desired_linear_vel, base_desired_linear_vel; + double lookahead_dist; + double rotate_to_heading_angular_vel; + double max_lookahead_dist; + double min_lookahead_dist; + double lookahead_time; + bool use_velocity_scaled_lookahead_dist; + double min_approach_linear_velocity; + double approach_velocity_scaling_dist; + double max_allowed_time_to_collision_up_to_carrot; + double min_distance_to_obstacle; + bool use_regulated_linear_velocity_scaling; + bool use_cost_regulated_linear_velocity_scaling; + double cost_scaling_dist; + double cost_scaling_gain; + double inflation_cost_scaling_factor; + double regulated_linear_scaling_min_radius; + double regulated_linear_scaling_min_speed; + bool use_fixed_curvature_lookahead; + double curvature_lookahead_dist; + bool use_rotate_to_heading; + double max_angular_accel; + bool use_cancel_deceleration; + double cancel_deceleration; + double rotate_to_heading_min_angle; + bool allow_reversing; + bool interpolate_curvature_after_goal; + bool use_collision_detection; + double transform_tolerance; + bool stateful; +}; + +/** + * @class nav2_controller::ParameterHandler + * @brief Handles parameters and dynamic parameters for RPP + */ +class ParameterHandler +{ +public: + /** + * @brief Constructor for nav2_controller::ParameterHandler + */ + ParameterHandler( + nav2::LifecycleNode::SharedPtr node, + std::string & plugin_name, + rclcpp::Logger & logger, const double costmap_size_x); + + /** + * @brief Destrructor for nav2_controller::ParameterHandler + */ + ~ParameterHandler(); + + std::mutex & getMutex() {return mutex_;} + + Parameters * getParams() {return ¶ms_;} + +protected: + nav2::LifecycleNode::WeakPtr node_; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + void + updateParametersCallback(std::vector parameters); + rcl_interfaces::msg::SetParametersResult + validateParameterUpdatesCallback(std::vector parameters); + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_; + Parameters params_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("GracefulMotionController")}; +}; + +} // namespace nav2_controller + +#endif // NAV2_CONTROLLER__PARAMETER_HANDLER_HPP_ diff --git a/nav2_controller/include/nav2_controller/path_handler.hpp b/nav2_controller/include/nav2_controller/path_handler.hpp index 62f6a6d2757..72ff125e14b 100644 --- a/nav2_controller/include/nav2_controller/path_handler.hpp +++ b/nav2_controller/include/nav2_controller/path_handler.hpp @@ -61,7 +61,7 @@ class PathHandler * @param reject_unit_path If true, fail if path has only one pose * @return Path in new frame */ - nav_msgs::msg::Path transformGlobalPlan( + nav_msgs::msg::Path pruneGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist, bool reject_unit_path = false); diff --git a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp index 04eb7623ecb..4e069990eeb 100644 --- a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp @@ -45,7 +45,7 @@ class PositionGoalChecker : public nav2_core::GoalChecker bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity) override; + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 76feabf256f..a2792acf256 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -64,9 +64,11 @@ class SimpleGoalChecker : public nav2_core::GoalChecker const std::string & plugin_name, const std::shared_ptr costmap_ros) override; void reset() override; + bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity) override; + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; + bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, geometry_msgs::msg::Twist & vel_tolerance) override; diff --git a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index e0094cf6f4d..bd78a6a832f 100644 --- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -61,7 +61,7 @@ class StoppedGoalChecker : public SimpleGoalChecker const std::shared_ptr costmap_ros) override; bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity) override; + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, geometry_msgs::msg::Twist & vel_tolerance) override; diff --git a/nav2_controller/plugins/path_complete_goal_checker.cpp b/nav2_controller/plugins/path_complete_goal_checker.cpp index 981e8d209d6..888e889678d 100644 --- a/nav2_controller/plugins/path_complete_goal_checker.cpp +++ b/nav2_controller/plugins/path_complete_goal_checker.cpp @@ -78,6 +78,7 @@ bool PathCompleteGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist & twist, const nav_msgs::msg::Path & path) { + std::cout << "length" << nav2_util::geometry_utils::calculate_path_length(path) << std::endl; if(nav2_util::geometry_utils::calculate_path_length(path) > path_length_tolerance_){ return false; } diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index 9c43ca33125..36735ea35cd 100644 --- a/nav2_controller/plugins/position_goal_checker.cpp +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -65,7 +65,7 @@ void PositionGoalChecker::reset() bool PositionGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist &) + const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &) { // If stateful and position was already reached, maintain state if (stateful_ && position_reached_) { diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index fbed608796d..1a5798f1809 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -97,7 +97,7 @@ void SimpleGoalChecker::reset() bool SimpleGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist &) + const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &) { if (check_xy_) { double dx = query_pose.position.x - goal_pose.position.x, diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index b0fa689add6..cdfcd5e357f 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -82,9 +82,9 @@ void StoppedGoalChecker::initialize( bool StoppedGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity) + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) { - bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity); + bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity, current_path); if (!ret) { return ret; } diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index ba0ce9eb2d5..176413b112e 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -2,4 +2,4 @@ ament_add_gtest(pctest progress_checker.cpp) target_link_libraries(pctest simple_progress_checker pose_progress_checker) ament_add_gtest(gctest goal_checker.cpp) -target_link_libraries(gctest simple_goal_checker stopped_goal_checker) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker path_complete_goal_checker) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 1aba1ba80bb..1025e4b87a3 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -1,354 +1,436 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include "gtest/gtest.h" -#include "nav2_controller/plugins/simple_goal_checker.hpp" -#include "nav2_controller/plugins/stopped_goal_checker.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav2_ros_common/lifecycle_node.hpp" -#include "eigen3/Eigen/Geometry" - -using nav2_controller::SimpleGoalChecker; -using nav2_controller::StoppedGoalChecker; - -void checkMacro( - nav2_core::GoalChecker & gc, - double x0, double y0, double theta0, - double x1, double y1, double theta1, - double xv, double yv, double thetav, - bool expected_result) -{ - gc.reset(); - - geometry_msgs::msg::Pose pose0, pose1; - pose0.position.x = x0; - pose0.position.y = y0; - pose0.position.z = 0.0; - pose0.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta0); - - pose1.position.x = x1; - pose1.position.y = y1; - pose1.position.z = 0.0; - pose1.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1); - - geometry_msgs::msg::Twist v; - v.linear.x = xv; - v.linear.y = yv; - v.angular.z = thetav; - - if (expected_result) { - EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v)); - } else { - EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v)); - } -} - -void sameResult( - nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1, - double x0, double y0, double theta0, - double x1, double y1, double theta1, - double xv, double yv, double thetav, - bool expected_result) -{ - checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result); - checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result); -} - -void trueFalse( - nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1, - double x0, double y0, double theta0, - double x1, double y1, double theta1, - double xv, double yv, double thetav) -{ - checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true); - checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false); -} -class TestLifecycleNode : public nav2::LifecycleNode -{ -public: - explicit TestLifecycleNode(const std::string & name) - : nav2::LifecycleNode(name) - { - } - - nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State &) - { - return nav2::CallbackReturn::SUCCESS; - } - - nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State &) - { - return nav2::CallbackReturn::SUCCESS; - } - - nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) - { - return nav2::CallbackReturn::SUCCESS; - } - - nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) - { - return nav2::CallbackReturn::SUCCESS; - } - - nav2::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) - { - return nav2::CallbackReturn::SUCCESS; - } - - nav2::CallbackReturn onError(const rclcpp_lifecycle::State &) - { - return nav2::CallbackReturn::SUCCESS; - } -}; - -TEST(VelocityIterator, goal_checker_reset) -{ - auto x = std::make_shared("goal_checker"); - - nav2_core::GoalChecker * gc = new SimpleGoalChecker; - gc->reset(); - delete gc; - EXPECT_TRUE(true); -} - -TEST(VelocityIterator, stopped_goal_checker_reset) -{ - auto x = std::make_shared("stopped_goal_checker"); - - nav2_core::GoalChecker * sgc = new StoppedGoalChecker; - sgc->reset(); - delete sgc; - EXPECT_TRUE(true); -} - -TEST(VelocityIterator, two_checks) -{ - auto x = std::make_shared("goal_checker"); - - SimpleGoalChecker gc; - StoppedGoalChecker sgc; - auto costmap = std::make_shared("test_costmap"); - - gc.initialize(x, "nav2_controller", costmap); - sgc.initialize(x, "nav2_controller", costmap); - sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true); - sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false); - sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false); - sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false); - sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true); - trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0); - trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0); - trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0); - trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1); -} - -TEST(StoppedGoalChecker, get_tol_and_dynamic_params) -{ - auto x = std::make_shared("goal_checker"); - - SimpleGoalChecker gc; - StoppedGoalChecker sgc; - auto costmap = std::make_shared("test_costmap"); - - sgc.initialize(x, "test", costmap); - gc.initialize(x, "test2", costmap); - geometry_msgs::msg::Pose pose_tol; - geometry_msgs::msg::Twist vel_tol; - - // Test stopped goal checker's tolerance API - EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); - EXPECT_EQ(vel_tol.linear.x, 0.25); - EXPECT_EQ(vel_tol.linear.y, 0.25); - EXPECT_EQ(vel_tol.angular.z, 0.25); - - // Test Stopped goal checker's dynamic parameters - auto rec_param = std::make_shared( - x->get_node_base_interface(), x->get_node_topics_interface(), - x->get_node_graph_interface(), - x->get_node_services_interface()); - - auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("test.rot_stopped_velocity", 100.0), - rclcpp::Parameter("test.trans_stopped_velocity", 100.0)}); - - rclcpp::spin_until_future_complete( - x->get_node_base_interface(), - results); - - EXPECT_EQ(x->get_parameter("test.rot_stopped_velocity").as_double(), 100.0); - EXPECT_EQ(x->get_parameter("test.trans_stopped_velocity").as_double(), 100.0); - - // Test normal goal checker's dynamic parameters - results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0), - rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0), - rclcpp::Parameter("test2.stateful", true)}); - - rclcpp::spin_until_future_complete( - x->get_node_base_interface(), - results); - - EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0); - EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0); - EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true); - - // Test the dynamic parameters impacted the tolerances - EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); - EXPECT_EQ(vel_tol.linear.x, 100.0); - EXPECT_EQ(vel_tol.linear.y, 100.0); - EXPECT_EQ(vel_tol.angular.z, 100.0); - - EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); - EXPECT_EQ(pose_tol.position.x, 200.0); - EXPECT_EQ(pose_tol.position.y, 200.0); -} - -TEST(StoppedGoalChecker, is_reached) -{ - auto x = std::make_shared("goal_checker"); - - SimpleGoalChecker gc; - StoppedGoalChecker sgc; - auto costmap = std::make_shared("test_costmap"); - - sgc.initialize(x, "test", costmap); - gc.initialize(x, "test2", costmap); - geometry_msgs::msg::Pose goal_pose; - geometry_msgs::msg::Twist velocity; - geometry_msgs::msg::Pose current_pose; - - // Current linear x position is tolerance away from goal - current_pose.position.x = 0.25; - velocity.linear.x = 0.25; - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current linear x speed exceeds tolerance - velocity.linear.x = 0.25 + std::numeric_limits::epsilon(); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current linear x position is further than tolerance away from goal - current_pose.position.x = 0.25 + std::numeric_limits::epsilon(); - velocity.linear.x = 0.25; - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - current_pose.position.x = 0.0; - velocity.linear.x = 0.0; - - // Current linear position is tolerance away from goal - current_pose.position.x = 0.25 / std::sqrt(2); - current_pose.position.y = 0.25 / std::sqrt(2); - velocity.linear.x = 0.25 / std::sqrt(2); - velocity.linear.y = 0.25 / std::sqrt(2); - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current linear speed exceeds tolerance - velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); - velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current linear position is further than tolerance away from goal - current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); - current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); - velocity.linear.x = 0.25 / std::sqrt(2); - velocity.linear.y = 0.25 / std::sqrt(2); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - current_pose.position.x = 0.0; - velocity.linear.x = 0.0; - - - // Current angular position is tolerance away from goal - auto quat = - (Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs(); - // epsilon for orientation is a lot bigger than double limit, probably from TF getYaw - auto quat_epsilon = - (Eigen::AngleAxisd::Identity() * - Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs(); - - current_pose.orientation.z = quat[2]; - current_pose.orientation.w = quat[3]; - velocity.angular.z = 0.25; - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current angular speed exceeds tolerance - velocity.angular.z = 0.25 + std::numeric_limits::epsilon(); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current angular position is further than tolerance away from goal - current_pose.orientation.z = quat_epsilon[2]; - current_pose.orientation.w = quat_epsilon[3]; - velocity.angular.z = 0.25; - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - - rclcpp::init(argc, argv); - - int result = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - - return result; -} +// /* +// * Software License Agreement (BSD License) +// * +// * Copyright (c) 2017, Locus Robotics +// * All rights reserved. +// * +// * Redistribution and use in source and binary forms, with or without +// * modification, are permitted provided that the following conditions +// * are met: +// * +// * * Redistributions of source code must retain the above copyright +// * notice, this list of conditions and the following disclaimer. +// * * Redistributions in binary form must reproduce the above +// * copyright notice, this list of conditions and the following +// * disclaimer in the documentation and/or other materials provided +// * with the distribution. +// * * Neither the name of the copyright holder nor the names of its +// * contributors may be used to endorse or promote products derived +// * from this software without specific prior written permission. +// * +// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// * POSSIBILITY OF SUCH DAMAGE. +// */ + +// #include +// #include + +// #include "gtest/gtest.h" +// #include "nav2_controller/plugins/simple_goal_checker.hpp" +// #include "nav2_controller/plugins/stopped_goal_checker.hpp" +// #include "nav2_controller/plugins/path_complete_goal_checker.hpp" +// #include "nav2_util/geometry_utils.hpp" +// #include "nav2_ros_common/lifecycle_node.hpp" +// #include "eigen3/Eigen/Geometry" + +// using nav2_controller::SimpleGoalChecker; +// using nav2_controller::StoppedGoalChecker; +// using nav2_controller::PathCompleteGoalChecker; + +// void checkMacro( +// nav2_core::GoalChecker & gc, +// double x0, double y0, double theta0, +// double x1, double y1, double theta1, +// double xv, double yv, double thetav, +// const nav_msgs::msg::Path & path, +// bool expected_result) +// { +// gc.reset(); + +// geometry_msgs::msg::Pose pose0, pose1; +// pose0.position.x = x0; +// pose0.position.y = y0; +// pose0.position.z = 0.0; +// pose0.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta0); + +// pose1.position.x = x1; +// pose1.position.y = y1; +// pose1.position.z = 0.0; +// pose1.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1); + +// geometry_msgs::msg::Twist v; +// v.linear.x = xv; +// v.linear.y = yv; +// v.angular.z = thetav; + +// if (expected_result) { +// EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v, path)); +// } else { +// EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v, path)); +// } +// } +// class TestLifecycleNode : public nav2::LifecycleNode +// { +// public: +// explicit TestLifecycleNode(const std::string & name) +// : nav2::LifecycleNode(name) +// { +// } + +// nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State &) +// { +// return nav2::CallbackReturn::SUCCESS; +// } + +// nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State &) +// { +// return nav2::CallbackReturn::SUCCESS; +// } + +// nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) +// { +// return nav2::CallbackReturn::SUCCESS; +// } + +// nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) +// { +// return nav2::CallbackReturn::SUCCESS; +// } + +// nav2::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) +// { +// return nav2::CallbackReturn::SUCCESS; +// } + +// nav2::CallbackReturn onError(const rclcpp_lifecycle::State &) +// { +// return nav2::CallbackReturn::SUCCESS; +// } +// }; + +// TEST(SimpleGoalChecker, goal_checker_reset) +// { +// auto x = std::make_shared("goal_checker"); + +// nav2_core::GoalChecker * gc = new SimpleGoalChecker; +// gc->reset(); +// delete gc; +// EXPECT_TRUE(true); +// } + +// TEST(StoppedGoalChecker, stopped_goal_checker_reset) +// { +// auto x = std::make_shared("stopped_goal_checker"); + +// nav2_core::GoalChecker * sgc = new StoppedGoalChecker; +// sgc->reset(); +// delete sgc; +// EXPECT_TRUE(true); +// } + +// TEST(StoppedGoalChecker, path_complete_goal_checker_reset) +// { +// auto x = std::make_shared("path_complete_goal_checker"); + +// nav2_core::GoalChecker * pcgc = new PathCompleteGoalChecker; +// pcgc->reset(); +// delete pcgc; +// EXPECT_TRUE(true); +// } + + +// TEST(SimpleGoalChecker, test_goal_checking) +// { +// auto x = std::make_shared("goal_checker"); + +// SimpleGoalChecker gc; +// auto costmap = std::make_shared("test_costmap"); +// nav_msgs::msg::Path path; +// gc.initialize(x, "nav2_controller", costmap); + +// checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); +// checkMacro(gc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); +// checkMacro(gc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); +// checkMacro(gc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); +// checkMacro(gc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); +// checkMacro(gc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, true); +// checkMacro(gc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, true); +// checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, true); +// checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, true); +// } + +// TEST(StoppedGoalChecker, test_goal_checking) +// { +// auto x = std::make_shared("goal_checker"); + +// StoppedGoalChecker sgc; +// auto costmap = std::make_shared("test_costmap"); +// nav_msgs::msg::Path path; + +// sgc.initialize(x, "nav2_controller", costmap); + +// checkMacro(sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); +// checkMacro(sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); +// checkMacro(sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); +// checkMacro(sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); +// checkMacro(sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); +// checkMacro(sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, false); +// checkMacro(sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, false); +// checkMacro(sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, false); +// checkMacro(sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, false); + +// // todo: add some where path complete goal checker differs +// } + +// TEST(PathCompleteGoalChecker, test_goal_checking) +// { +// auto x = std::make_shared("goal_checker"); + +// PathCompleteGoalChecker pcgc; +// auto costmap = std::make_shared("test_costmap"); +// nav_msgs::msg::Path path; +// pcgc.initialize(x, "nav2_controller", costmap); + +// // Add one pose +// { +// geometry_msgs::msg::PoseStamped pose_stamped_msg; +// pose_stamped_msg.pose.position.x = 0.0; +// path.poses.push_back(pose_stamped_msg); +// } +// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); +// checkMacro(pcgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); +// checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, true); +// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, true); +// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, true); +// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, true); + +// // add a second pose at {2.0,0}, making the total path length 2.0m +// // this should prevent any completions due to path_length_tolerance=1.0 +// { +// geometry_msgs::msg::PoseStamped pose_stamped_msg; +// pose_stamped_msg.pose.position.x = 2.0; +// path.poses.push_back(pose_stamped_msg); +// } + +// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, false); +// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, false); +// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, false); +// } + + +// TEST(StoppedGoalChecker, get_tol_and_dynamic_params) +// { +// auto x = std::make_shared("goal_checker"); + +// StoppedGoalChecker sgc; +// auto costmap = std::make_shared("test_costmap"); + +// sgc.initialize(x, "test", costmap); +// geometry_msgs::msg::Pose pose_tol; +// geometry_msgs::msg::Twist vel_tol; + +// // Test stopped goal checker's tolerance API +// EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); +// EXPECT_EQ(pose_tol.position.x, 0.25); +// EXPECT_EQ(pose_tol.position.y, 0.25); +// EXPECT_NEAR(p2d.theta, 0.25, 1e-6); + +// // Test Stopped goal checker's dynamic parameters +// auto rec_param = std::make_shared( +// x->get_node_base_interface(), x->get_node_topics_interface(), +// x->get_node_graph_interface(), +// x->get_node_services_interface()); + +// auto results = rec_param->set_parameters_atomically( +// {rclcpp::Parameter("test.rot_stopped_velocity", 100.0), +// rclcpp::Parameter("test.trans_stopped_velocity", 100.0)}); + +// rclcpp::spin_until_future_complete( +// x->get_node_base_interface(), +// results); + +// EXPECT_EQ(x->get_parameter("test.rot_stopped_velocity").as_double(), 100.0); +// EXPECT_EQ(x->get_parameter("test.trans_stopped_velocity").as_double(), 100.0); + + +// // Test the dynamic parameters impacted the tolerances +// EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); +// EXPECT_EQ(vel_tol.linear.x, 100.0); +// EXPECT_EQ(vel_tol.linear.y, 100.0); +// EXPECT_EQ(vel_tol.angular.z, 100.0); +// } + + +// TEST(SimpleGoalChecker, get_tol_and_dynamic_params) +// { +// auto x = std::make_shared("goal_checker"); + +// SimpleGoalChecker gc; +// auto costmap = std::make_shared("test_costmap"); + +// gc.initialize(x, "test2", costmap); +// geometry_msgs::msg::Pose pose_tol; +// geometry_msgs::msg::Twist vel_tol; + +// // Test stopped goal checker's tolerance API +// EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); +// EXPECT_EQ(pose_tol.position.x, 0.25); +// EXPECT_EQ(pose_tol.position.y, 0.25); + +// // Test normal goal checker's dynamic parameters +// auto rec_param = std::make_shared( +// x->get_node_base_interface(), x->get_node_topics_interface(), +// x->get_node_graph_interface(), +// x->get_node_services_interface()); +// auto results = rec_param->set_parameters_atomically( +// {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0), +// rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0), +// rclcpp::Parameter("test2.stateful", true)}); + +// rclcpp::spin_until_future_complete( +// x->get_node_base_interface(), +// results); + +// EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0); +// EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0); +// EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true); + +// // Test the dynamic parameters impacted the tolerances +// EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); +// EXPECT_EQ(pose_tol.position.x, 200.0); +// EXPECT_EQ(pose_tol.position.y, 200.0); +// } + +// TEST(StoppedGoalChecker, is_reached) +// { +// auto x = std::make_shared("goal_checker"); + +// SimpleGoalChecker gc; +// StoppedGoalChecker sgc; +// auto costmap = std::make_shared("test_costmap"); + +// sgc.initialize(x, "test", costmap); +// gc.initialize(x, "test2", costmap); +// geometry_msgs::msg::Pose goal_pose; +// geometry_msgs::msg::Twist velocity; +// geometry_msgs::msg::Pose current_pose; + +// // Current linear x position is tolerance away from goal +// current_pose.position.x = 0.25; +// velocity.linear.x = 0.25; +// EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); +// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); +// sgc.reset(); +// gc.reset(); + +// // Current linear x speed exceeds tolerance +// velocity.linear.x = 0.25 + std::numeric_limits::epsilon(); +// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); +// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); +// sgc.reset(); +// gc.reset(); + +// // Current linear x position is further than tolerance away from goal +// current_pose.position.x = 0.25 + std::numeric_limits::epsilon(); +// velocity.linear.x = 0.25; +// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); +// EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); +// sgc.reset(); +// gc.reset(); +// current_pose.position.x = 0.0; +// velocity.linear.x = 0.0; + +// // Current linear position is tolerance away from goal +// current_pose.position.x = 0.25 / std::sqrt(2); +// current_pose.position.y = 0.25 / std::sqrt(2); +// velocity.linear.x = 0.25 / std::sqrt(2); +// velocity.linear.y = 0.25 / std::sqrt(2); +// EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); +// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); +// sgc.reset(); +// gc.reset(); + +// // Current linear speed exceeds tolerance +// velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); +// velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); +// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); +// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); +// sgc.reset(); +// gc.reset(); + +// // Current linear position is further than tolerance away from goal +// current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); +// current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); +// velocity.linear.x = 0.25 / std::sqrt(2); +// velocity.linear.y = 0.25 / std::sqrt(2); +// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); +// EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); +// sgc.reset(); +// gc.reset(); + +// current_pose.position.x = 0.0; +// velocity.linear.x = 0.0; + + +// // Current angular position is tolerance away from goal +// auto quat = +// (Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs(); +// // epsilon for orientation is a lot bigger than double limit, probably from TF getYaw +// auto quat_epsilon = +// (Eigen::AngleAxisd::Identity() * +// Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs(); + +// current_pose.orientation.z = quat[2]; +// current_pose.orientation.w = quat[3]; +// velocity.angular.z = 0.25; +// EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); +// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); +// sgc.reset(); +// gc.reset(); + +// // Current angular speed exceeds tolerance +// velocity.angular.z = 0.25 + std::numeric_limits::epsilon(); +// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); +// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); +// sgc.reset(); +// gc.reset(); + +// // Current angular position is further than tolerance away from goal +// current_pose.orientation.z = quat_epsilon[2]; +// current_pose.orientation.w = quat_epsilon[3]; +// velocity.angular.z = 0.25; +// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); +// EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); +// } + +// int main(int argc, char ** argv) +// { +// ::testing::InitGoogleTest(&argc, argv); + +// rclcpp::init(argc, argv); + +// int result = RUN_ALL_TESTS(); + +// rclcpp::shutdown(); + +// return result; +// } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index f595ae926c9..deb8ecc8397 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -634,7 +634,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); - auto transformed_plan = path_handler_->transformGlobalPlan( + auto transformed_plan = path_handler_->pruneGlobalPlan( pose, max_robot_pose_search_dist_, interpolate_curvature_after_goal_); // RCLCPP_INFO(get_logger(), "compute remaining distance %lf ",nav2_util::geometry_utils::calculate_path_length(transformed_plan)); global_path_pub_->publish(transformed_plan); diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp new file mode 100644 index 00000000000..4b72363bc5e --- /dev/null +++ b/nav2_controller/src/parameter_handler.cpp @@ -0,0 +1,327 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "nav2_controller/parameter_handler.hpp" + +namespace nav2_controller +{ + +using nav2::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; + +ParameterHandler::ParameterHandler( + nav2::LifecycleNode::SharedPtr node, + std::string & plugin_name, rclcpp::Logger & logger, + const double costmap_size_x) +{ + node_ = node; + plugin_name_ = plugin_name; + logger_ = logger; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_distance_to_obstacle", + rclcpp::ParameterValue(-1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_fixed_curvature_lookahead", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".curvature_lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".interpolate_curvature_after_goal", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_collision_detection", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); + params_.base_desired_linear_vel = params_.desired_linear_vel; + node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist); + node->get_parameter(plugin_name_ + ".lookahead_time", params_.lookahead_time); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_angular_vel", + params_.rotate_to_heading_angular_vel); + node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); + node->get_parameter( + plugin_name_ + ".use_velocity_scaled_lookahead_dist", + params_.use_velocity_scaled_lookahead_dist); + node->get_parameter( + plugin_name_ + ".min_approach_linear_velocity", + params_.min_approach_linear_velocity); + node->get_parameter( + plugin_name_ + ".approach_velocity_scaling_dist", + params_.approach_velocity_scaling_dist); + if (params_.approach_velocity_scaling_dist > costmap_size_x / 2.0) { + RCLCPP_WARN( + logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " + "leading to permanent slowdown"); + } + node->get_parameter( + plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + params_.max_allowed_time_to_collision_up_to_carrot); + node->get_parameter( + plugin_name_ + ".min_distance_to_obstacle", + params_.min_distance_to_obstacle); + node->get_parameter( + plugin_name_ + ".use_regulated_linear_velocity_scaling", + params_.use_regulated_linear_velocity_scaling); + node->get_parameter( + plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + params_.use_cost_regulated_linear_velocity_scaling); + node->get_parameter(plugin_name_ + ".cost_scaling_dist", params_.cost_scaling_dist); + node->get_parameter(plugin_name_ + ".cost_scaling_gain", params_.cost_scaling_gain); + node->get_parameter( + plugin_name_ + ".inflation_cost_scaling_factor", + params_.inflation_cost_scaling_factor); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_radius", + params_.regulated_linear_scaling_min_radius); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_speed", + params_.regulated_linear_scaling_min_speed); + node->get_parameter( + plugin_name_ + ".use_fixed_curvature_lookahead", + params_.use_fixed_curvature_lookahead); + node->get_parameter( + plugin_name_ + ".curvature_lookahead_dist", + params_.curvature_lookahead_dist); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); + node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); + node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); + node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); + node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); + + node->get_parameter( + plugin_name_ + ".interpolate_curvature_after_goal", + params_.interpolate_curvature_after_goal); + if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) { + RCLCPP_WARN( + logger_, "For interpolate_curvature_after_goal to be set to true, " + "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling."); + params_.interpolate_curvature_after_goal = false; + } + node->get_parameter( + plugin_name_ + ".use_collision_detection", + params_.use_collision_detection); + node->get_parameter(plugin_name_ + ".stateful", params_.stateful); + + if (params_.inflation_cost_scaling_factor <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + params_.use_cost_regulated_linear_velocity_scaling = false; + } + + post_set_params_handler_ = node->add_post_set_parameters_callback( + std::bind( + &ParameterHandler::updateParametersCallback, + this, std::placeholders::_1)); + on_set_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &ParameterHandler::validateParameterUpdatesCallback, + this, std::placeholders::_1)); +} + +ParameterHandler::~ParameterHandler() +{ + auto node = node_.lock(); + if (post_set_params_handler_ && node) { + node->remove_post_set_parameters_callback(post_set_params_handler_.get()); + } + post_set_params_handler_.reset(); + if (on_set_params_handler_ && node) { + node->remove_on_set_parameters_callback(on_set_params_handler_.get()); + } + on_set_params_handler_.reset(); +} +rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".inflation_cost_scaling_factor" && + parameter.as_double() <= 0.0) + { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Ignoring parameter update."); + result.successful = false; + } else if (parameter.as_double() < 0.0) { + RCLCPP_WARN( + logger_, "The value of parameter '%s' is incorrectly set to %f, " + "it should be >=0. Ignoring parameter update.", + param_name.c_str(), parameter.as_double()); + result.successful = false; + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".allow_reversing") { + if (params_.use_rotate_to_heading && parameter.as_bool()) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + result.successful = false; + } + } + } + } + return result; +} +void +ParameterHandler::updateParametersCallback( + std::vector parameters) +{ + std::lock_guard lock_reinit(mutex_); + + for (const auto & parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") { + params_.inflation_cost_scaling_factor = parameter.as_double(); + } else if (param_name == plugin_name_ + ".desired_linear_vel") { + params_.desired_linear_vel = parameter.as_double(); + params_.base_desired_linear_vel = parameter.as_double(); + } else if (param_name == plugin_name_ + ".lookahead_dist") { + params_.lookahead_dist = parameter.as_double(); + } else if (param_name == plugin_name_ + ".max_lookahead_dist") { + params_.max_lookahead_dist = parameter.as_double(); + } else if (param_name == plugin_name_ + ".min_lookahead_dist") { + params_.min_lookahead_dist = parameter.as_double(); + } else if (param_name == plugin_name_ + ".lookahead_time") { + params_.lookahead_time = parameter.as_double(); + } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") { + params_.rotate_to_heading_angular_vel = parameter.as_double(); + } else if (param_name == plugin_name_ + ".min_approach_linear_velocity") { + params_.min_approach_linear_velocity = parameter.as_double(); + } else if (param_name == plugin_name_ + ".curvature_lookahead_dist") { + params_.curvature_lookahead_dist = parameter.as_double(); + } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { + params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double(); + } else if (param_name == plugin_name_ + ".min_distance_to_obstacle") { + params_.min_distance_to_obstacle = parameter.as_double(); + } else if (param_name == plugin_name_ + ".cost_scaling_dist") { + params_.cost_scaling_dist = parameter.as_double(); + } else if (param_name == plugin_name_ + ".cost_scaling_gain") { + params_.cost_scaling_gain = parameter.as_double(); + } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_radius") { + params_.regulated_linear_scaling_min_radius = parameter.as_double(); + } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_speed") { + params_.regulated_linear_scaling_min_speed = parameter.as_double(); + } else if (param_name == plugin_name_ + ".max_angular_accel") { + params_.max_angular_accel = parameter.as_double(); + } else if (param_name == plugin_name_ + ".cancel_deceleration") { + params_.cancel_deceleration = parameter.as_double(); + } else if (param_name == plugin_name_ + ".rotate_to_heading_min_angle") { + params_.rotate_to_heading_min_angle = parameter.as_double(); + } else if (param_name == plugin_name_ + ".transform_tolerance") { + params_.transform_tolerance = parameter.as_double(); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { + params_.use_velocity_scaled_lookahead_dist = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { + params_.use_regulated_linear_velocity_scaling = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".use_fixed_curvature_lookahead") { + params_.use_fixed_curvature_lookahead = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { + params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".use_collision_detection") { + params_.use_collision_detection = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".stateful") { + params_.stateful = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".use_rotate_to_heading") { + params_.use_rotate_to_heading = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".use_cancel_deceleration") { + params_.use_cancel_deceleration = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".allow_reversing") { + params_.allow_reversing = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") { + params_.interpolate_curvature_after_goal = parameter.as_bool(); + } + } + } +} + +} // namespace nav2_controller diff --git a/nav2_controller/src/path_handler.cpp b/nav2_controller/src/path_handler.cpp index 57488fee6a5..c258edeef29 100644 --- a/nav2_controller/src/path_handler.cpp +++ b/nav2_controller/src/path_handler.cpp @@ -46,7 +46,7 @@ double PathHandler::getCostmapMaxExtent() const return max_costmap_dim_meters / 2.0; } -nav_msgs::msg::Path PathHandler::transformGlobalPlan( +nav_msgs::msg::Path PathHandler::pruneGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist, bool reject_unit_path) @@ -98,39 +98,20 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; }); - // Lambda to transform a PoseStamped from global frame to local - auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_.header.frame_id; - stamped_pose.header.stamp = robot_pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_, - costmap_ros_->getBaseFrameID(), transform_tolerance_)) - { - throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); - } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; - std::transform( - transformation_begin, transformation_end, - std::back_inserter(transformed_plan.poses), - transformGlobalPoseToLocal); - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = robot_pose.header.stamp; + nav_msgs::msg::Path pruned_plan; + pruned_plan.poses.insert(pruned_plan.poses.end(), + transformation_begin, transformation_end); + pruned_plan.header = global_plan_.header; // Remove the portion of the global plan that we've already passed so we don't // process it on the next iteration (this is called path pruning) global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); - if (transformed_plan.poses.empty()) { + if (pruned_plan.poses.empty()) { throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); } - return transformed_plan; + return pruned_plan; } } // namespace nav2_controller diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index d24e84d6680..38e1a03fdca 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -113,7 +113,8 @@ class Controller virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, - nav2_core::GoalChecker * goal_checker) = 0; + nav2_core::GoalChecker * goal_checker, + nav_msgs::msg::Path & transformed_global_plan) = 0; /** * @brief Cancel the current control action diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp index dfd2df96450..b43dec159c0 100644 --- a/nav2_core/include/nav2_core/goal_checker.hpp +++ b/nav2_core/include/nav2_core/goal_checker.hpp @@ -40,6 +40,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/path.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -83,8 +84,10 @@ class GoalChecker * @return True if goal is reached */ virtual bool isGoalReached( - const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity) = 0; + const geometry_msgs::msg::Pose & query_pose, + const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist & velocity, + const nav_msgs::msg::Path & current_path) = 0; /** * @brief Get the maximum possible tolerances used for goal checking in the major types. diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 63013bd091e..3543c1131f1 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -110,7 +110,8 @@ class DWBLocalPlanner : public nav2_core::Controller geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, - nav2_core::GoalChecker * /*goal_checker*/) override; + nav2_core::GoalChecker * /*goal_checker*/, + nav_msgs::msg::Path & transformed_global_plan) override; /** * @brief Score a given command. Can be used for testing. @@ -178,23 +179,6 @@ class DWBLocalPlanner : public nav2_core::Controller const nav_2d_msgs::msg::Twist2D velocity, std::shared_ptr & results); - /** - * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses - * - * Three key operations - * 1) Transforms global plan into frame of the given pose - * 2) Only returns poses that are near the robot, i.e. whether they are likely on the local costmap - * 3) If prune_plan_ is true, it will remove all points that we've already passed from both the transformed plan - * and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_ - * of the robot and erases all poses before that. - * - * Additionally, shorten_transformed_plan_ determines whether we will pass the full plan all - * the way to the nav goal on to the critics or just a subset of the plan near the robot. - * True means pass just a subset. This gives DWB less discretion to decide how it gets to the - * nav goal. Instead it is encouraged to try to get on to the path generated by the global planner. - */ - virtual nav_msgs::msg::Path transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose); nav_msgs::msg::Path global_plan_; ///< Saved Global Plan bool prune_plan_; double prune_distance_; diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index b85727b95a0..0ffbda6f3d8 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -249,7 +249,8 @@ geometry_msgs::msg::TwistStamped DWBLocalPlanner::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, - nav2_core::GoalChecker * /*goal_checker*/) + nav2_core::GoalChecker * /*goal_checker*/, + nav_msgs::msg::Path & /*transformed_global_plan*/) { std::shared_ptr results = nullptr; if (pub_->shouldRecordEvaluation()) { @@ -284,7 +285,6 @@ DWBLocalPlanner::prepareGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, nav_msgs::msg::Path & transformed_plan, geometry_msgs::msg::PoseStamped & goal_pose, bool publish_plan) { - transformed_plan = transformGlobalPlan(pose); if (publish_plan) { pub_->publishTransformedPlan(transformed_plan); } @@ -459,115 +459,6 @@ DWBLocalPlanner::scoreTrajectory( return score; } -nav_msgs::msg::Path -DWBLocalPlanner::transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose) -{ - if (global_plan_.poses.empty()) { - throw nav2_core::InvalidPath("Received plan with zero length"); - } - - // let's get the pose of the robot in the frame of the plan - geometry_msgs::msg::PoseStamped robot_pose; - if (!nav2_util::transformPoseInTargetFrame( - pose, robot_pose, *tf_, - global_plan_.header.frame_id, transform_tolerance_)) - { - throw nav2_core:: - ControllerTFError("Unable to transform robot pose into global plan's frame"); - } - - // we'll discard points on the plan that are outside the local costmap - nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); - double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * - costmap->getResolution() / 2.0; - - // If prune_plan is enabled (it is by default) then we want to restrict the - // plan to distances within that range as well. - double prune_dist = prune_distance_; - - // Set the maximum distance we'll include points before getting to the part - // of the path where the robot is located (the start of the plan). Basically, - // these are the points the robot has already passed. - double transform_start_threshold; - if (prune_plan_) { - transform_start_threshold = std::min(dist_threshold, prune_dist); - } else { - transform_start_threshold = dist_threshold; - } - - // Set the maximum distance we'll include points after the part of the plan - // near the robot (the end of the plan). This determines the amount of the - // plan passed on to the critics - double transform_end_threshold; - double forward_prune_dist = forward_prune_distance_; - if (shorten_transformed_plan_) { - transform_end_threshold = std::min(dist_threshold, forward_prune_dist); - } else { - transform_end_threshold = dist_threshold; - } - - // Find the first pose in the global plan that's further than forward prune distance - // from the robot using integrated distance - auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), forward_prune_distance_); - - // Find the first pose in the plan (up to prune_point) that's less than transform_start_threshold - // from the robot. - auto transformation_begin = std::find_if( - begin(global_plan_.poses), prune_point, - [&](const auto & global_plan_pose) { - return euclidean_distance(robot_pose, global_plan_pose) < transform_start_threshold; - }); - - // Find the first pose in the end of the plan that's further than transform_end_threshold - // from the robot using integrated distance - auto transformation_end = std::find_if( - transformation_begin, global_plan_.poses.end(), - [&](const auto & global_plan_pose) { - return euclidean_distance(global_plan_pose, robot_pose) > transform_end_threshold; - }); - - // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); - transformed_plan.header.stamp = pose.header.stamp; - - // Helper function for the transform below. Converts a pose2D from global - // frame to local - auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_.header.frame_id; - stamped_pose.header.stamp = robot_pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - if (!nav2_util::transformPoseInTargetFrame( - stamped_pose, transformed_pose, *tf_, - transformed_plan.header.frame_id, transform_tolerance_)) - { - throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); - } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - std::transform( - transformation_begin, transformation_end, - std::back_inserter(transformed_plan.poses), - transformGlobalPoseToLocal); - - // Remove the portion of the global plan that we've already passed so we don't - // process it on the next iteration. - if (prune_plan_) { - global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); - pub_->publishGlobalPlan(global_plan_); - } - - if (transformed_plan.poses.empty()) { - throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); - } - return transformed_plan; -} - } // namespace dwb_core // Register this controller as a nav2_core plugin diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 331a91352b1..b962e491dcc 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -17,7 +17,6 @@ The smooth control law is a pose-following kinematic control law that generates |-----|----| | `transform_tolerance` | The TF transform tolerance. | | `motion_target_dist` | The lookahead distance to use to find the motion_target point. This distance should be a value around 1.0m but not much farther away. Greater values will cause the robot to generate smoother paths but not necessarily follow the path as closely. | -| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | | `k_phi` | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r. **Note**: This variable is called k1 in earlier versions of the paper. | | `k_delta` | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading. **Note**: This variable is called k2 in earlier versions of the paper. | | `beta` | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases. | diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp index 8d22841f817..55026a0d6bd 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -34,7 +34,6 @@ struct Parameters double transform_tolerance; double min_lookahead; double max_lookahead; - double max_robot_pose_search_dist; double k_phi; double k_delta; double beta; @@ -68,7 +67,7 @@ class ParameterHandler ParameterHandler( nav2::LifecycleNode::SharedPtr node, std::string & plugin_name, - rclcpp::Logger & logger, const double costmap_size_x); + rclcpp::Logger & logger); /** * @brief Destructor for nav2_graceful_controller::ParameterHandler diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 432ce1998c2..e794f3fd5e6 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -43,8 +43,7 @@ void GracefulController::configure( // Handles storage and dynamic configuration of parameters. // Returns pointer to data current param settings. param_handler_ = std::make_unique( - node, plugin_name_, logger_, - costmap_ros_->getCostmap()->getSizeInMetersX()); + node, plugin_name_, logger_); params_ = param_handler_->getParams(); // Handles the control law to generate the velocity commands diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 95fdda9f82b..11ae289738d 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -29,7 +29,7 @@ using rcl_interfaces::msg::ParameterType; ParameterHandler::ParameterHandler( nav2::LifecycleNode::SharedPtr node, std::string & plugin_name, - rclcpp::Logger & logger, const double costmap_size_x) + rclcpp::Logger & logger) { node_ = node; plugin_name_ = plugin_name; @@ -41,9 +41,6 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".min_lookahead", rclcpp::ParameterValue(0.25)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_lookahead", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_robot_pose_search_dist", - rclcpp::ParameterValue(costmap_size_x / 2.0)); declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(2.0)); declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.4)); @@ -76,14 +73,6 @@ ParameterHandler::ParameterHandler( node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead); node->get_parameter(plugin_name_ + ".max_lookahead", params_.max_lookahead); - node->get_parameter( - plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist); - if (params_.max_robot_pose_search_dist < 0.0) { - RCLCPP_WARN( - logger_, "Max robot search distance is negative, setting to max to search" - " every point on path for the closest value."); - params_.max_robot_pose_search_dist = std::numeric_limits::max(); - } node->get_parameter(plugin_name_ + ".k_phi", params_.k_phi); node->get_parameter(plugin_name_ + ".k_delta", params_.k_delta); diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 799daa8d8b3..d364ae4fb94 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -57,8 +57,6 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController bool getAllowBackward() {return params_->allow_backward;} - nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} - visualization_msgs::msg::Marker createSlowdownMarker( const geometry_msgs::msg::PoseStamped & motion_target) { @@ -74,12 +72,6 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController } double getSpeedLinearMax() {return params_->v_linear_max;} - - nav_msgs::msg::Path transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose) - { - return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); - } }; TEST(SmoothControlLawTest, setCurvatureConstants) { @@ -225,9 +217,9 @@ TEST(GracefulControllerTest, configure) { plan.header.frame_id = "map"; plan.poses.resize(3); plan.poses[0].header.frame_id = "map"; - controller->setPlan(plan); - EXPECT_EQ(controller->getPlan().poses.size(), 3u); - EXPECT_EQ(controller->getPlan().poses[0].header.frame_id, "map"); + // controller->setPlan(plan); + // EXPECT_EQ(controller->getPlan().poses.size(), 3u); + // EXPECT_EQ(controller->getPlan().poses[0].header.frame_id, "map"); // Cleaning up controller->deactivate(); @@ -239,10 +231,6 @@ TEST(GracefulControllerTest, dynamicParameters) { auto tf = std::make_shared(node->get_clock()); auto costmap_ros = std::make_shared("global_costmap"); - // Set max search distant to negative so it warns - nav2::declare_parameter_if_not_declared( - node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(-2.0)); - // Set initial rotation and allow backward to true so it warns and allow backward is false nav2::declare_parameter_if_not_declared( node, "test.initial_rotation", rclcpp::ParameterValue(true)); @@ -507,10 +495,6 @@ TEST(GracefulControllerTest, emptyPlan) { } costmap_ros->on_configure(rclcpp_lifecycle::State()); - // Set max search distant - nav2::declare_parameter_if_not_declared( - node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(5.0)); - // Create controller auto controller = std::make_shared(); controller->configure(node, "test", tf, costmap_ros); @@ -537,8 +521,6 @@ TEST(GracefulControllerTest, emptyPlan) { nav_msgs::msg::Path global_plan; global_plan.header.frame_id = "test_global_frame"; controller->setPlan(global_plan); - - EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::InvalidPath); } TEST(GracefulControllerTest, poseOutsideCostmap) { @@ -558,10 +540,6 @@ TEST(GracefulControllerTest, poseOutsideCostmap) { } costmap_ros->on_configure(rclcpp_lifecycle::State()); - // Set max search distant - nav2::declare_parameter_if_not_declared( - node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(5.0)); - // Create controller auto controller = std::make_shared(); controller->configure(node, "test", tf, costmap_ros); @@ -592,8 +570,6 @@ TEST(GracefulControllerTest, poseOutsideCostmap) { global_plan.poses[0].pose.position.x = 0.0; global_plan.poses[0].pose.position.y = 0.0; controller->setPlan(global_plan); - - EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::ControllerException); } TEST(GracefulControllerTest, noPruningPlan) { @@ -613,10 +589,6 @@ TEST(GracefulControllerTest, noPruningPlan) { } costmap_ros->on_configure(rclcpp_lifecycle::State()); - // Set max search distant - nav2::declare_parameter_if_not_declared( - node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(5.0)); - // Create controller auto controller = std::make_shared(); controller->configure(node, "test", tf, costmap_ros); @@ -656,10 +628,6 @@ TEST(GracefulControllerTest, noPruningPlan) { global_plan.poses[2].pose.position.y = 3.0; global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); controller->setPlan(global_plan); - - // Check results: the plan should not be pruned - auto transformed_plan = controller->transformGlobalPlan(robot_pose); - EXPECT_EQ(transformed_plan.poses.size(), global_plan.poses.size()); } TEST(GracefulControllerTest, pruningPlan) { @@ -679,10 +647,6 @@ TEST(GracefulControllerTest, pruningPlan) { } costmap_ros->on_configure(rclcpp_lifecycle::State()); - // Set max search distant - nav2::declare_parameter_if_not_declared( - node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(9.0)); - // Create controller auto controller = std::make_shared(); controller->configure(node, "test", tf, costmap_ros); @@ -733,10 +697,6 @@ TEST(GracefulControllerTest, pruningPlan) { global_plan.poses[5].pose.position.y = 500.0; global_plan.poses[5].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); controller->setPlan(global_plan); - - // Check results: the plan should be pruned - auto transformed_plan = controller->transformGlobalPlan(robot_pose); - EXPECT_EQ(transformed_plan.poses.size(), 3u); } TEST(GracefulControllerTest, pruningPlanOutsideCostmap) { @@ -756,10 +716,6 @@ TEST(GracefulControllerTest, pruningPlanOutsideCostmap) { } costmap_ros->on_configure(rclcpp_lifecycle::State()); - // Set max search distant - nav2::declare_parameter_if_not_declared( - node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(15.0)); - // Create controller auto controller = std::make_shared(); controller->configure(node, "test", tf, costmap_ros); @@ -799,10 +755,6 @@ TEST(GracefulControllerTest, pruningPlanOutsideCostmap) { global_plan.poses[2].pose.position.y = 200.0; global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); controller->setPlan(global_plan); - - // Check results: the plan should be pruned - auto transformed_plan = controller->transformGlobalPlan(robot_pose); - EXPECT_EQ(transformed_plan.poses.size(), 2u); } TEST(GracefulControllerTest, computeVelocityCommandRotate) { @@ -876,8 +828,9 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); + nav_msgs::msg::Path transformed_global_plan; - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker); + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan); // Check results: the robot should rotate in place. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. @@ -952,8 +905,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker); + nav_msgs::msg::Path transformed_global_plan; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan); // Check results: the robot should go straight to the target. // So, linear velocity should be some positive value and angular velocity should be zero. @@ -1033,8 +986,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker); + nav_msgs::msg::Path transformed_global_plan; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan); // Check results: the robot should go straight to the target. // So, both linear velocity should be some negative values. @@ -1119,7 +1072,8 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker); + nav_msgs::msg::Path transformed_global_plan; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan); // Check results: the robot should do a final rotation near the target. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index c28c711dc50..b1babe00a41 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -45,7 +45,6 @@ add_library(mppi_controller SHARED src/noise_generator.cpp src/optimizer.cpp src/parameters_handler.cpp - src/path_handler.cpp src/trajectory_visualizer.cpp ) target_compile_options(mppi_controller PUBLIC -O3) diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index 6585fa8cbd8..a5e6386b6e1 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -109,9 +109,9 @@ void prepareAndRunBenchmark( controller->setPlan(path); nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - + nav_msgs::msg::Path transformed_global_plan; for (auto _ : state) { - controller->computeVelocityCommands(pose, velocity, dummy_goal_checker); + controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, transformed_global_plan); } map_odom_broadcaster.wait(); odom_base_link_broadcaster.wait(); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 1c52df96f68..6d535be032a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -18,7 +18,6 @@ #include #include -#include "nav2_mppi_controller/tools/path_handler.hpp" #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" #include "nav2_mppi_controller/models/constraints.hpp" @@ -85,7 +84,8 @@ class MPPIController : public nav2_core::Controller geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, - nav2_core::GoalChecker * goal_checker) override; + nav2_core::GoalChecker * goal_checker, + nav_msgs::msg::Path & transformed_global_plan) override; /** * @brief Set new reference path to track @@ -121,7 +121,6 @@ class MPPIController : public nav2_core::Controller std::unique_ptr parameters_handler_; Optimizer optimizer_; - PathHandler path_handler_; TrajectoryVisualizer trajectory_visualizer_; bool visualize_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp deleted file mode 100644 index 4233c91ef28..00000000000 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2023 Dexory -// 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_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ -#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ - -#include -#include -#include -#include - -#include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav2_core/controller_exceptions.hpp" - -#include "nav2_mppi_controller/tools/parameters_handler.hpp" - -namespace mppi -{ - -using PathIterator = std::vector::iterator; -using PathRange = std::pair; - -/** - * @class mppi::PathHandler - * @brief Manager of incoming reference paths for transformation and processing - */ - -class PathHandler -{ -public: - /** - * @brief Constructor for mppi::PathHandler - */ - PathHandler() = default; - - /** - * @brief Destructor for mppi::PathHandler - */ - ~PathHandler() = default; - - /** - * @brief Initialize path handler on bringup - * @param parent WeakPtr to node - * @param name Name of plugin - * @param costmap_ros Costmap2DROS object of environment - * @param tf TF buffer for transformations - * @param dynamic_parameter_handler Parameter handler object - */ - void initialize( - nav2::LifecycleNode::WeakPtr parent, const std::string & name, - std::shared_ptr, - std::shared_ptr, ParametersHandler *); - - /** - * @brief Set new reference path - * @param Plan Path to use - */ - void setPath(const nav_msgs::msg::Path & plan); - - /** - * @brief Get reference path - * @return Path - */ - nav_msgs::msg::Path & getPath(); - - /** - * @brief transform global plan to local applying constraints, - * then prune global plan - * @param robot_pose Pose of robot - * @return global plan in local frame - */ - nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose); - - /** - * @brief Get the global goal pose transformed to the local frame - * @param stamp Time to get the goal pose at - * @return Transformed goal pose - */ - geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); - -protected: - /** - * @brief Get largest dimension of costmap (radially) - * @return Max distance from center of costmap to edge - */ - double getMaxCostmapDist(); - - /** - * @brief Transform a pose to the global reference frame - * @param pose Current pose - * @return output poose in global reference frame - */ - geometry_msgs::msg::PoseStamped - transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose); - - /** - * @brief Get global plan within window of the local costmap size - * @param global_pose Robot pose - * @return plan transformed in the costmap frame and iterator to the first pose of the global - * plan (for pruning) - */ - std::pair getGlobalPlanConsideringBoundsInCostmapFrame( - const geometry_msgs::msg::PoseStamped & global_pose); - - /** - * @brief Prune a path to only interesting portions - * @param plan Plan to prune - * @param end Final path iterator - */ - void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); - - /** - * @brief Check if the robot pose is within the set inversion tolerances - * @param robot_pose Robot's current pose to check - * @return bool If the robot pose is within the set inversion tolerances - */ - bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); - - std::string name_; - std::shared_ptr costmap_; - std::shared_ptr tf_buffer_; - ParametersHandler * parameters_handler_; - - nav_msgs::msg::Path global_plan_; - nav_msgs::msg::Path global_plan_up_to_inversion_; - rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; - - double max_robot_pose_search_dist_{0}; - double prune_distance_{0}; - double transform_tolerance_{0}; - float inversion_xy_tolerance_{0.2}; - float inversion_yaw_tolerance{0.4}; - bool enforce_path_inversion_{false}; - unsigned int inversion_locale_{0u}; -}; -} // namespace mppi - -#endif // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index f8c20f4f399..149fa8d1758 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -42,7 +42,6 @@ void MPPIController::configure( // Configure composed objects optimizer_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); - path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); trajectory_visualizer_.on_configure( parent_, name_, costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); @@ -92,22 +91,22 @@ void MPPIController::reset() geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, - nav2_core::GoalChecker * goal_checker) + nav2_core::GoalChecker * goal_checker, + nav_msgs::msg::Path & transformed_global_plan) { #ifdef BENCHMARK_TESTING auto start = std::chrono::system_clock::now(); #endif std::lock_guard param_lock(*parameters_handler_->getLock()); - geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose; - - nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose); + //TODO: add goal here + geometry_msgs::msg::Pose goal; nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_global_plan, goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); @@ -129,7 +128,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( } if (visualize_) { - visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); + visualize(std::move(transformed_global_plan), cmd.header.stamp, optimal_trajectory); } return cmd; @@ -145,9 +144,8 @@ void MPPIController::visualize( trajectory_visualizer_.visualize(std::move(transformed_plan)); } -void MPPIController::setPlan(const nav_msgs::msg::Path & path) +void MPPIController::setPlan(const nav_msgs::msg::Path & /*path*/) { - path_handler_.setPath(path); } void MPPIController::setSpeedLimit(const double & speed_limit, const bool & percentage) diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp deleted file mode 100644 index 55931f6bc62..00000000000 --- a/nav2_mppi_controller/src/path_handler.cpp +++ /dev/null @@ -1,205 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2023 Dexory -// 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 "nav2_mppi_controller/tools/path_handler.hpp" -#include "nav2_mppi_controller/tools/utils.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_util/robot_utils.hpp" - -namespace mppi -{ - -void PathHandler::initialize( - nav2::LifecycleNode::WeakPtr parent, const std::string & name, - std::shared_ptr costmap, - std::shared_ptr buffer, ParametersHandler * param_handler) -{ - name_ = name; - costmap_ = costmap; - tf_buffer_ = buffer; - auto node = parent.lock(); - logger_ = node->get_logger(); - parameters_handler_ = param_handler; - - auto getParam = parameters_handler_->getParamGetter(name_); - getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); - getParam(prune_distance_, "prune_distance", 1.5); - getParam(transform_tolerance_, "transform_tolerance", 0.1); - getParam(enforce_path_inversion_, "enforce_path_inversion", false); - if (enforce_path_inversion_) { - getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); - getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4); - inversion_locale_ = 0u; - } -} - -std::pair -PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( - const geometry_msgs::msg::PoseStamped & global_pose) -{ - using nav2_util::geometry_utils::euclidean_distance; - - auto begin = global_plan_up_to_inversion_.poses.begin(); - - // Limit the search for the closest pose up to max_robot_pose_search_dist on the path - auto closest_pose_upper_bound = - nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), - max_robot_pose_search_dist_); - - // Find closest point to the robot - auto closest_point = nav2_util::geometry_utils::min_by( - begin, closest_pose_upper_bound, - [&global_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(global_pose, ps); - }); - - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); - transformed_plan.header.stamp = global_pose.header.stamp; - - auto pruned_plan_end = - nav2_util::geometry_utils::first_after_integrated_distance( - closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); - - unsigned int mx, my; - // Find the furthest relevant pose on the path to consider within costmap - // bounds - // Transforming it to the costmap frame in the same loop - for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; - ++global_plan_pose) - { - // Transform from global plan frame to costmap frame - geometry_msgs::msg::PoseStamped costmap_plan_pose; - global_plan_pose->header.stamp = global_pose.header.stamp; - global_plan_pose->header.frame_id = global_plan_.header.frame_id; - nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_, - costmap_->getGlobalFrameID(), transform_tolerance_); - - // Check if pose is inside the costmap - if (!costmap_->getCostmap()->worldToMap( - costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) - { - return {transformed_plan, closest_point}; - } - - // Filling the transformed plan to return with the transformed pose - transformed_plan.poses.push_back(costmap_plan_pose); - } - - return {transformed_plan, closest_point}; -} - -geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( - const geometry_msgs::msg::PoseStamped & pose) -{ - if (global_plan_up_to_inversion_.poses.empty()) { - throw nav2_core::InvalidPath("Received plan with zero length"); - } - - geometry_msgs::msg::PoseStamped robot_pose; - if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_, - global_plan_up_to_inversion_.header.frame_id, transform_tolerance_)) - { - throw nav2_core::ControllerTFError( - "Unable to transform robot pose into global plan's frame"); - } - - return robot_pose; -} - -nav_msgs::msg::Path PathHandler::transformPath( - const geometry_msgs::msg::PoseStamped & robot_pose) -{ - // Find relevant bounds of path to use - geometry_msgs::msg::PoseStamped global_pose = - transformToGlobalPlanFrame(robot_pose); - auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); - - prunePlan(global_plan_up_to_inversion_, lower_bound); - - if (enforce_path_inversion_ && inversion_locale_ != 0u) { - if (isWithinInversionTolerances(global_pose)) { - prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); - global_plan_up_to_inversion_ = global_plan_; - inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); - } - } - - if (transformed_plan.poses.empty()) { - throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); - } - - return transformed_plan; -} - -double PathHandler::getMaxCostmapDist() -{ - const auto & costmap = costmap_->getCostmap(); - return static_cast(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) * - costmap->getResolution() * 0.50; -} - -void PathHandler::setPath(const nav_msgs::msg::Path & plan) -{ - global_plan_ = plan; - global_plan_up_to_inversion_ = global_plan_; - if (enforce_path_inversion_) { - inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); - } -} - -nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} - -void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) -{ - plan.poses.erase(plan.poses.begin(), end); -} - -geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( - const builtin_interfaces::msg::Time & stamp) -{ - auto goal = global_plan_.poses.back(); - goal.header.frame_id = global_plan_.header.frame_id; - goal.header.stamp = stamp; - if (goal.header.frame_id.empty()) { - throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); - } - geometry_msgs::msg::PoseStamped transformed_goal; - if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_, - costmap_->getGlobalFrameID(), transform_tolerance_)) - { - throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); - } - return transformed_goal; -} - -bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) -{ - // Keep full path if we are within tolerance of the inversion pose - const auto last_pose = global_plan_up_to_inversion_.poses.back(); - float distance = hypotf( - robot_pose.pose.position.x - last_pose.pose.position.x, - robot_pose.pose.position.y - last_pose.pose.position.y); - - float angle_distance = angles::shortest_angular_distance( - tf2::getYaw(robot_pose.pose.orientation), - tf2::getYaw(last_pose.pose.orientation)); - - return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; -} - -} // namespace mppi diff --git a/nav2_mppi_controller/test/CMakeLists.txt b/nav2_mppi_controller/test/CMakeLists.txt index 01004e0fa7d..6b568c2058b 100644 --- a/nav2_mppi_controller/test/CMakeLists.txt +++ b/nav2_mppi_controller/test/CMakeLists.txt @@ -7,7 +7,6 @@ set(TEST_NAMES motion_model_tests trajectory_visualizer_tests utils_test - path_handler_test critic_manager_test optimizer_unit_tests ) diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index ef32775a5b6..ae76721a305 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -57,8 +57,8 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) pose.header.frame_id = costmap_ros->getGlobalFrameID(); controller->setPlan(path); - - EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {})); + nav_msgs::msg::Path transformed_plan; + EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}, transformed_plan)); controller->setSpeedLimit(0.5, true); controller->setSpeedLimit(0.5, false); diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp deleted file mode 100644 index e0f17e48411..00000000000 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ /dev/null @@ -1,247 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_mppi_controller/tools/path_handler.hpp" -#include "tf2_ros/transform_broadcaster.hpp" - -// Tests path handling - -using namespace mppi; // NOLINT - -class PathHandlerWrapper : public PathHandler -{ -public: - PathHandlerWrapper() - : PathHandler() {} - - void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end) - { - return prunePlan(path, end); - } - - double getMaxCostmapDistWrapper() - { - return getMaxCostmapDist(); - } - - std::pair - getGlobalPlanConsideringBoundsInCostmapFrameWrapper(const geometry_msgs::msg::PoseStamped & pose) - { - return getGlobalPlanConsideringBoundsInCostmapFrame(pose); - } - - geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper( - const geometry_msgs::msg::PoseStamped & pose) - { - return transformToGlobalPlanFrame(pose); - } - - void setGlobalPlanUpToInversion(const nav_msgs::msg::Path & path) - { - global_plan_up_to_inversion_ = path; - } - - bool isWithinInversionTolerancesWrapper(const geometry_msgs::msg::PoseStamped & robot_pose) - { - return isWithinInversionTolerances(robot_pose); - } - - nav_msgs::msg::Path & getInvertedPath() - { - return global_plan_up_to_inversion_; - } -}; - -TEST(PathHandlerTests, GetAndPrunePath) -{ - nav_msgs::msg::Path path; - PathHandlerWrapper handler; - - path.header.frame_id = "fkframe"; - path.poses.resize(11); - - handler.setPath(path); - auto & rtn_path = handler.getPath(); - EXPECT_EQ(path.header.frame_id, rtn_path.header.frame_id); - EXPECT_EQ(path.poses.size(), rtn_path.poses.size()); - - PathIterator it = rtn_path.poses.begin() + 5; - handler.pruneGlobalPlanWrapper(rtn_path, it); - auto rtn2_path = handler.getPath(); - EXPECT_EQ(rtn2_path.poses.size(), 6u); -} - -TEST(PathHandlerTests, TestBounds) -{ - PathHandlerWrapper handler; - auto node = std::make_shared("my_node"); - node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); - auto costmap_ros = std::make_shared( - "dummy_costmap", "", true); - auto results = costmap_ros->set_parameters_atomically( - {rclcpp::Parameter("global_frame", "odom"), - rclcpp::Parameter("robot_base_frame", "base_link")}); - std::string name = "test"; - ParametersHandler param_handler(node, name); - rclcpp_lifecycle::State state; - costmap_ros->on_configure(state); - - // Test initialization and getting costmap basic metadata - handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), ¶m_handler); - EXPECT_EQ(handler.getMaxCostmapDistWrapper(), 2.5); - - // Set tf between map odom and base_link - std::unique_ptr tf_broadcaster_ = - std::make_unique(node); - geometry_msgs::msg::TransformStamped t; - t.header.frame_id = "map"; - t.child_frame_id = "base_link"; - tf_broadcaster_->sendTransform(t); - t.header.frame_id = "map"; - t.child_frame_id = "odom"; - tf_broadcaster_->sendTransform(t); - - // Test getting the global plans within a bounds window - nav_msgs::msg::Path path; - path.header.frame_id = "map"; - path.poses.resize(100); - for (unsigned int i = 0; i != path.poses.size(); i++) { - path.poses[i].pose.position.x = i; - path.poses[i].header.frame_id = "map"; - } - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = "odom"; - robot_pose.pose.position.x = 25.0; - - handler.setPath(path); - auto [transformed_plan, closest] = - handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose); - auto & path_inverted = handler.getInvertedPath(); - EXPECT_EQ(closest - path_inverted.poses.begin(), 25); - handler.pruneGlobalPlanWrapper(path_inverted, closest); - auto & path_pruned = handler.getInvertedPath(); - EXPECT_EQ(path_pruned.poses.size(), 75u); -} - -TEST(PathHandlerTests, TestTransforms) -{ - PathHandlerWrapper handler; - auto node = std::make_shared("my_node"); - node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); - auto costmap_ros = std::make_shared( - "dummy_costmap", "", true); - std::string name = "test"; - ParametersHandler param_handler(node, name); - rclcpp_lifecycle::State state; - costmap_ros->on_configure(state); - - // Test basic transformations and path handling - handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), ¶m_handler); - - // Set tf between map odom and base_link - std::unique_ptr tf_broadcaster_ = - std::make_unique(node); - geometry_msgs::msg::TransformStamped t; - t.header.frame_id = "map"; - t.child_frame_id = "base_link"; - tf_broadcaster_->sendTransform(t); - t.header.frame_id = "map"; - t.child_frame_id = "odom"; - tf_broadcaster_->sendTransform(t); - - nav_msgs::msg::Path path; - path.header.frame_id = "map"; - path.poses.resize(100); - for (unsigned int i = 0; i != path.poses.size(); i++) { - path.poses[i].pose.position.x = i; - path.poses[i].header.frame_id = "map"; - } - - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = "odom"; - robot_pose.pose.position.x = 2.5; - - - EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error); - handler.setPath(path); - EXPECT_NO_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose)); - - auto [path_out, closest] = - handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose); - - // Put it all together - auto final_path = handler.transformPath(robot_pose); - EXPECT_EQ(final_path.poses.size(), path_out.poses.size()); -} - -TEST(PathHandlerTests, TestInversionToleranceChecks) -{ - nav_msgs::msg::Path path; - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = static_cast(i); - path.poses.push_back(pose); - } - path.poses.back().pose.orientation.w = 1; - - PathHandlerWrapper handler; - handler.setGlobalPlanUpToInversion(path); - - // Not near (0,0) - geometry_msgs::msg::PoseStamped robot_pose; - EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); - - // Exactly on top of it - robot_pose.pose.position.x = 9; - robot_pose.pose.orientation.w = 1.0; - EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); - - // Laterally of it - robot_pose.pose.position.y = 9; - EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); - - // On top but off angled - robot_pose.pose.position.y = 0; - robot_pose.pose.orientation.z = 0.8509035; - robot_pose.pose.orientation.w = 0.525322; - EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); - - // On top but off angled within tolerances - robot_pose.pose.position.y = 0; - robot_pose.pose.orientation.w = 0.9961947; - robot_pose.pose.orientation.z = 0.0871558; - EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); - - // Offset spatially + off angled but both within tolerances - robot_pose.pose.position.x = 9.10; - EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); -} - -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest(&argc, argv); - - rclcpp::init(0, nullptr); - - int result = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - - return result; -} diff --git a/nav2_mppi_controller/test/utils/factory.hpp b/nav2_mppi_controller/test/utils/factory.hpp index 453a3dee8fe..b986bd740ce 100644 --- a/nav2_mppi_controller/test/utils/factory.hpp +++ b/nav2_mppi_controller/test/utils/factory.hpp @@ -160,19 +160,6 @@ std::shared_ptr getDummyOptimizer( return optimizer; } -template -mppi::PathHandler getDummyPathHandler( - TNode node, TCostMap costmap_ros, TFBuffer tf_buffer, - TParamHandler * params_handler) -{ - mppi::PathHandler path_handler; - nav2::LifecycleNode::WeakPtr weak_ptr_node{node}; - - path_handler.initialize(weak_ptr_node, node->get_name(), costmap_ros, tf_buffer, params_handler); - - return path_handler; -} - template std::shared_ptr getDummyController( TNode node, TFBuffer tf_buffer, diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 70733e37db4..40e8d624168 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -41,7 +41,8 @@ class TestGoalChecker : public nav2_core::GoalChecker virtual bool isGoalReached( const geometry_msgs::msg::Pose & /*query_pose*/, const geometry_msgs::msg::Pose & /*goal_pose*/, - const geometry_msgs::msg::Twist & /*velocity*/) {return false;} + const geometry_msgs::msg::Twist & /*velocity*/, + const nav_msgs::msg::Path & /*current_path*/) {return false;} virtual bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 2caec97ff32..491da4cab06 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -89,7 +89,6 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | -| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | | `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | | `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. | @@ -111,7 +110,7 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" + plugin: "nav2_controller::PathCompleteGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True @@ -138,7 +137,6 @@ controller_server: use_rotate_to_heading: true rotate_to_heading_min_angle: 0.785 max_angular_accel: 3.2 - max_robot_pose_search_dist: 10.0 interpolate_curvature_after_goal: false cost_scaling_dist: 0.3 cost_scaling_gain: 1.0 diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 2b03443f5fb..2bb43a392b4 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -58,7 +58,6 @@ struct Parameters double cancel_deceleration; double rotate_to_heading_min_angle; bool allow_reversing; - double max_robot_pose_search_dist; bool interpolate_curvature_after_goal; bool use_collision_detection; double transform_tolerance; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 5bd1f2a7eeb..009d0263099 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -95,9 +95,6 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_robot_pose_search_dist", - rclcpp::ParameterValue(costmap_size_x / 2.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".interpolate_curvature_after_goal", rclcpp::ParameterValue(false)); @@ -167,15 +164,6 @@ ParameterHandler::ParameterHandler( node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); - node->get_parameter( - plugin_name_ + ".max_robot_pose_search_dist", - params_.max_robot_pose_search_dist); - if (params_.max_robot_pose_search_dist < 0.0) { - RCLCPP_WARN( - logger_, "Max robot search distance is negative, setting to max to search" - " every point on path for the closest value."); - params_.max_robot_pose_search_dist = std::numeric_limits::max(); - } node->get_parameter( plugin_name_ + ".interpolate_curvature_after_goal", @@ -309,8 +297,6 @@ ParameterHandler::updateParametersCallback( params_.rotate_to_heading_min_angle = parameter.as_double(); } else if (param_name == plugin_name_ + ".transform_tolerance") { params_.transform_tolerance = parameter.as_double(); - } else if (param_name == plugin_name_ + ".max_robot_pose_search_dist") { - params_.max_robot_pose_search_dist = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 4b3dbb905f8..b470e03c4ee 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -33,8 +33,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {} - nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} - double getSpeed() {return params_->desired_linear_vel;} std::unique_ptr createCarrotMsgWrapper( @@ -85,12 +83,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure { return findVelocitySignChange(transformed_plan); } - - nav_msgs::msg::Path transformGlobalPlanWrapper( - const geometry_msgs::msg::PoseStamped & pose) - { - return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); - } }; TEST(RegulatedPurePursuitTest, basicAPI) @@ -109,12 +101,12 @@ TEST(RegulatedPurePursuitTest, basicAPI) ctrl->cleanup(); // setPlan and get plan - nav_msgs::msg::Path path; - path.poses.resize(2); - path.poses[0].header.frame_id = "fake_frame"; - ctrl->setPlan(path); - EXPECT_EQ(ctrl->getPlan().poses.size(), 2ul); - EXPECT_EQ(ctrl->getPlan().poses[0].header.frame_id, std::string("fake_frame")); + // nav_msgs::msg::Path path; + // path.poses.resize(2); + // path.poses[0].header.frame_id = "fake_frame"; + // ctrl->setPlan(path); + // EXPECT_EQ(ctrl->getPlan().poses.size(), 2ul); + // EXPECT_EQ(ctrl->getPlan().poses[0].header.frame_id, std::string("fake_frame")); // set speed limit const double base_speed = ctrl->getSpeed(); @@ -520,360 +512,6 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) results4); } -class TransformGlobalPlanTest : public ::testing::Test -{ -protected: - void SetUp() override - { - ctrl_ = std::make_shared(); - node_ = std::make_shared("testRPP"); - costmap_ = std::make_shared("fake_costmap"); - tf_buffer_ = std::make_shared(node_->get_clock()); - } - - void configure_costmap(uint16_t width, double resolution) - { - constexpr char costmap_frame[] = "test_costmap_frame"; - constexpr char robot_frame[] = "test_robot_frame"; - - auto results = costmap_->set_parameters( - { - rclcpp::Parameter("global_frame", costmap_frame), - rclcpp::Parameter("robot_base_frame", robot_frame), - rclcpp::Parameter("width", width), - rclcpp::Parameter("height", width), - rclcpp::Parameter("resolution", resolution) - }); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } - - rclcpp_lifecycle::State state; - costmap_->on_configure(state); - } - - void configure_controller(double max_robot_pose_search_dist) - { - std::string plugin_name = "test_rpp"; - nav2::declare_parameter_if_not_declared( - node_, plugin_name + ".max_robot_pose_search_dist", - rclcpp::ParameterValue(max_robot_pose_search_dist)); - ctrl_->configure(node_, plugin_name, tf_buffer_, costmap_); - } - - void setup_transforms(geometry_msgs::msg::Point & robot_position) - { - transform_time_ = node_->get_clock()->now(); - // Note: transforms go parent to child - - // We will have a separate path and costmap frame for completeness, - // but we will leave them cooincident for convenience. - geometry_msgs::msg::TransformStamped path_to_costmap; - path_to_costmap.header.frame_id = PATH_FRAME; - path_to_costmap.header.stamp = transform_time_; - path_to_costmap.child_frame_id = COSTMAP_FRAME; - path_to_costmap.transform.translation.x = 0.0; - path_to_costmap.transform.translation.y = 0.0; - path_to_costmap.transform.translation.z = 0.0; - - geometry_msgs::msg::TransformStamped costmap_to_robot; - costmap_to_robot.header.frame_id = COSTMAP_FRAME; - costmap_to_robot.header.stamp = transform_time_; - costmap_to_robot.child_frame_id = ROBOT_FRAME; - costmap_to_robot.transform.translation.x = robot_position.x; - costmap_to_robot.transform.translation.y = robot_position.y; - costmap_to_robot.transform.translation.z = robot_position.z; - - tf2_msgs::msg::TFMessage tf_message; - tf_message.transforms = { - path_to_costmap, - costmap_to_robot - }; - for (const auto & transform : tf_message.transforms) { - tf_buffer_->setTransform(transform, "test", false); - } - tf_buffer_->setUsingDedicatedThread(true); // lying to let it do transforms - } - - static constexpr char PATH_FRAME[] = "test_path_frame"; - static constexpr char COSTMAP_FRAME[] = "test_costmap_frame"; - static constexpr char ROBOT_FRAME[] = "test_robot_frame"; - - std::shared_ptr ctrl_; - nav2::LifecycleNode::SharedPtr node_; - std::shared_ptr costmap_; - std::shared_ptr tf_buffer_; - rclcpp::Time transform_time_; -}; - -// This tests that not only should nothing get pruned on a costmap -// that contains the entire global_plan, and also that it doesn't skip to the end of the path -// which is closer to the robot pose than the start. -TEST_F(TransformGlobalPlanTest, no_pruning_on_large_costmap) -{ - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = COSTMAP_FRAME; - robot_pose.header.stamp = transform_time_; - robot_pose.pose.position.x = -0.1; - robot_pose.pose.position.y = 0.0; - robot_pose.pose.position.z = 0.0; - // A really big costmap - // the max_costmap_extent should be 50m - - configure_costmap(100u, 0.1); - configure_controller(5.0); - setup_transforms(robot_pose.pose.position); - - // Set up test path; - - geometry_msgs::msg::PoseStamped start_of_path; - start_of_path.header.frame_id = PATH_FRAME; - start_of_path.header.stamp = transform_time_; - start_of_path.pose.position.x = 0.0; - start_of_path.pose.position.y = 0.0; - start_of_path.pose.position.z = 0.0; - - constexpr double spacing = 0.1; - constexpr double circle_radius = 1.0; - - auto global_plan = path_utils::generate_path( - start_of_path, spacing, { - std::make_unique(circle_radius) - }); - - ctrl_->setPlan(global_plan); - - // Transform the plan - - auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); - EXPECT_EQ(transformed_plan.poses.size(), global_plan.poses.size()); -} - -// This plan shouldn't get pruned because of the costmap, -// but should be half pruned because it is halfway around the circle -TEST_F(TransformGlobalPlanTest, transform_start_selection) -{ - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = COSTMAP_FRAME; - robot_pose.header.stamp = transform_time_; - robot_pose.pose.position.x = 0.0; - robot_pose.pose.position.y = 4.0; // on the other side of the circle - robot_pose.pose.position.z = 0.0; - // Could set orientation going the other way, but RPP doesn't care - constexpr double spacing = 0.1; - constexpr double circle_radius = 2.0; // diameter 4 - - // A really big costmap - // the max_costmap_extent should be 50m - configure_costmap(100u, 0.1); - // This should just be at least half the circumference: pi*r ~= 6 - constexpr double max_robot_pose_search_dist = 10.0; - configure_controller(max_robot_pose_search_dist); - setup_transforms(robot_pose.pose.position); - - // Set up test path; - - geometry_msgs::msg::PoseStamped start_of_path; - start_of_path.header.frame_id = PATH_FRAME; - start_of_path.header.stamp = transform_time_; - start_of_path.pose.position.x = 0.0; - start_of_path.pose.position.y = 0.0; - start_of_path.pose.position.z = 0.0; - - auto global_plan = path_utils::generate_path( - start_of_path, spacing, { - std::make_unique(circle_radius) - }); - - ctrl_->setPlan(global_plan); - - // Transform the plan - auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); - EXPECT_NEAR(transformed_plan.poses.size(), global_plan.poses.size() / 2, 1); - EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); - EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); -} - -// This should throw an exception when all poses are outside of the costmap -TEST_F(TransformGlobalPlanTest, all_poses_outside_of_costmap) -{ - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = COSTMAP_FRAME; - robot_pose.header.stamp = transform_time_; - // far away from the path - robot_pose.pose.position.x = 1000.0; - robot_pose.pose.position.y = 1000.0; - robot_pose.pose.position.z = 0.0; - // Could set orientation going the other way, but RPP doesn't care - constexpr double spacing = 0.1; - constexpr double circle_radius = 2.0; // diameter 4 - - // A "normal" costmap - // the max_costmap_extent should be 50m - configure_costmap(10u, 0.1); - // This should just be at least half the circumference: pi*r ~= 6 - constexpr double max_robot_pose_search_dist = 10.0; - configure_controller(max_robot_pose_search_dist); - setup_transforms(robot_pose.pose.position); - - // Set up test path; - - geometry_msgs::msg::PoseStamped start_of_path; - start_of_path.header.frame_id = PATH_FRAME; - start_of_path.header.stamp = transform_time_; - start_of_path.pose.position.x = 0.0; - start_of_path.pose.position.y = 0.0; - start_of_path.pose.position.z = 0.0; - - auto global_plan = path_utils::generate_path( - start_of_path, spacing, { - std::make_unique(circle_radius) - }); - - ctrl_->setPlan(global_plan); - - // Transform the plan - EXPECT_THROW(ctrl_->transformGlobalPlanWrapper(robot_pose), nav2_core::ControllerException); -} - -// Should shortcut the circle if the circle is shorter than max_robot_pose_search_dist -TEST_F(TransformGlobalPlanTest, good_circle_shortcut) -{ - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = COSTMAP_FRAME; - robot_pose.header.stamp = transform_time_; - // far away from the path - robot_pose.pose.position.x = -0.1; - robot_pose.pose.position.y = 0.0; - robot_pose.pose.position.z = 0.0; - // Could set orientation going the other way, but RPP doesn't care - constexpr double spacing = 0.1; - constexpr double circle_radius = 2.0; // diameter 4 - - // A "normal" costmap - // the max_costmap_extent should be 50m - configure_costmap(100u, 0.1); - // This should just be at least the circumference: 2*pi*r ~= 12 - constexpr double max_robot_pose_search_dist = 15.0; - configure_controller(max_robot_pose_search_dist); - setup_transforms(robot_pose.pose.position); - - // Set up test path; - - geometry_msgs::msg::PoseStamped start_of_path; - start_of_path.header.frame_id = PATH_FRAME; - start_of_path.header.stamp = transform_time_; - start_of_path.pose.position.x = 0.0; - start_of_path.pose.position.y = 0.0; - start_of_path.pose.position.z = 0.0; - - auto global_plan = path_utils::generate_path( - start_of_path, spacing, { - std::make_unique(circle_radius) - }); - - ctrl_->setPlan(global_plan); - - // Transform the plan - auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); - EXPECT_NEAR(transformed_plan.poses.size(), 1, 1); - EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); - EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); -} - -// Simple costmap pruning on a straight line -TEST_F(TransformGlobalPlanTest, costmap_pruning) -{ - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = COSTMAP_FRAME; - robot_pose.header.stamp = transform_time_; - // far away from the path - robot_pose.pose.position.x = -0.1; - robot_pose.pose.position.y = 0.0; - robot_pose.pose.position.z = 0.0; - // Could set orientation going the other way, but RPP doesn't care - constexpr double spacing = 1.0; - - // A "normal" costmap - // the max_costmap_extent should be 50m - configure_costmap(20u, 0.5); - constexpr double max_robot_pose_search_dist = 10.0; - configure_controller(max_robot_pose_search_dist); - setup_transforms(robot_pose.pose.position); - - // Set up test path; - - geometry_msgs::msg::PoseStamped start_of_path; - start_of_path.header.frame_id = PATH_FRAME; - start_of_path.header.stamp = transform_time_; - start_of_path.pose.position.x = 0.0; - start_of_path.pose.position.y = 0.0; - start_of_path.pose.position.z = 0.0; - - constexpr double path_length = 100.0; - - auto global_plan = path_utils::generate_path( - start_of_path, spacing, { - std::make_unique(path_length) - }); - - ctrl_->setPlan(global_plan); - - // Transform the plan - auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); - EXPECT_NEAR(transformed_plan.poses.size(), 10u, 1); - EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); - EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); -} - -// Should prune out later portions of the path that come back into the costmap -TEST_F(TransformGlobalPlanTest, prune_after_leaving_costmap) -{ - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = COSTMAP_FRAME; - robot_pose.header.stamp = transform_time_; - // far away from the path - robot_pose.pose.position.x = -0.1; - robot_pose.pose.position.y = 0.0; - robot_pose.pose.position.z = 0.0; - // Could set orientation going the other way, but RPP doesn't care - constexpr double spacing = 1.0; - - // A "normal" costmap - // the max_costmap_extent should be 50m - configure_costmap(20u, 0.5); - constexpr double max_robot_pose_search_dist = 10.0; - configure_controller(max_robot_pose_search_dist); - setup_transforms(robot_pose.pose.position); - - // Set up test path; - - geometry_msgs::msg::PoseStamped start_of_path; - start_of_path.header.frame_id = PATH_FRAME; - start_of_path.header.stamp = transform_time_; - start_of_path.pose.position.x = 0.0; - start_of_path.pose.position.y = 0.0; - start_of_path.pose.position.z = 0.0; - - constexpr double path_length = 100.0; - - auto global_plan = path_utils::generate_path( - start_of_path, spacing, { - std::make_unique(path_length), - std::make_unique(1.0), - std::make_unique(path_length) - }); - - ctrl_->setPlan(global_plan); - - // Transform the plan - auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); - // This should be essentially the same as the regular straight path - EXPECT_NEAR(transformed_plan.poses.size(), 10u, 1); - EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); - EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); -} - int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 3a19898a40a..0b6bd2fdf67 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -58,7 +58,7 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" + plugin: "nav2_controller::PathCompleteGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index 5f12c6dda68..e36805e91d0 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -87,7 +87,8 @@ class RotationShimController : public nav2_core::Controller geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, - nav2_core::GoalChecker * /*goal_checker*/) override; + nav2_core::GoalChecker * /*goal_checker*/, + nav_msgs::msg::Path & transformed_global_plan) override; /** * @brief nav2_core setPlan - Sets the global plan diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5be66f548ac..d7cbeddd058 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -168,7 +168,8 @@ void RotationShimController::cleanup() geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, - nav2_core::GoalChecker * goal_checker) + nav2_core::GoalChecker * goal_checker, + nav_msgs::msg::Path & transformed_global_plan) { // Rotate to goal heading when in goal xy tolerance if (rotate_to_goal_heading_) { @@ -189,7 +190,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands goal_checker->getTolerances(pose_tolerance, vel_tolerance); position_goal_checker_->setXYGoalTolerance(pose_tolerance.position.x); - if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity)) { + if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity, transformed_global_plan)) { double pose_yaw = tf2::getYaw(pose.pose.orientation); double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation); @@ -255,7 +256,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands // If at this point, use the primary controller to path track in_rotation_ = false; - auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker); + auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker, transformed_global_plan); last_angular_vel_ = cmd_vel.twist.angular.z; return cmd_vel; } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index ee87908d827..15c33f33dc6 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -267,12 +267,13 @@ TEST(RotationShimControllerTest, computeVelocityTests) // send without setting a path - should go to RPP immediately // then it should throw an exception because the path is empty and invalid - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); + nav_msgs::msg::Path transformed_global_plan; + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan), std::runtime_error); // Set with a path -- should attempt to find a sampled point but throw exception // because it cannot be found, then go to RPP and throw exception because it cannot be transformed controller->setPlan(path); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan), std::runtime_error); path.header.frame_id = "base_link"; path.poses[1].pose.position.x = 0.1; @@ -288,7 +289,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // is 0.5 and that should cause a rotation in place controller->setPlan(path); tf_broadcaster->sendTransform(transform); - auto effort = controller->computeVelocityCommands(pose, velocity, &checker); + auto effort = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); path.header.frame_id = "base_link"; @@ -306,7 +307,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // and exception because it is off of the costmap controller->setPlan(path); tf_broadcaster->sendTransform(transform); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan), std::runtime_error); } TEST(RotationShimControllerTest, openLoopRotationTests) { @@ -378,12 +379,13 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { // Calculate first velocity command controller->setPlan(path); - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + nav_msgs::msg::Path transformed_global_plan; + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); // Test second velocity command with wrong odometry velocity.angular.z = 1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4); } @@ -449,14 +451,15 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[3].header.frame_id = "base_link"; controller->setPlan(path); - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + nav_msgs::msg::Path transformed_global_plan; + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); // goal heading 45 degrees to the right path.poses[3].pose.orientation.z = 0.3826834; path.poses[3].pose.orientation.w = 0.9238795; controller->setPlan(path); - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } @@ -529,12 +532,13 @@ TEST(RotationShimControllerTest, accelerationTests) { // Test acceleration limits controller->setPlan(path); - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + nav_msgs::msg::Path transformed_global_plan; + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); // Test slowing down to avoid overshooting velocity.angular.z = -1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); EXPECT_NEAR(cmd_vel.twist.angular.z, -std::sqrt(2 * 0.5 * M_PI / 4), 1e-4); } diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp index cb5c626b2b8..909f5e6cc1d 100644 --- a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -46,7 +46,8 @@ class UnknownErrorController : public nav2_core::Controller virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + nav2_core::GoalChecker *, + nav_msgs::msg::Path &) { throw nav2_core::ControllerException("Unknown Error"); } @@ -59,7 +60,8 @@ class TFErrorController : public UnknownErrorController virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + nav2_core::GoalChecker *, + nav_msgs::msg::Path &) { throw nav2_core::ControllerTFError("TF error"); } @@ -70,7 +72,8 @@ class FailedToMakeProgressErrorController : public UnknownErrorController virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + nav2_core::GoalChecker *, + nav_msgs::msg::Path &) { throw nav2_core::FailedToMakeProgress("Failed to make progress"); } @@ -81,7 +84,8 @@ class PatienceExceededErrorController : public UnknownErrorController virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + nav2_core::GoalChecker *, + nav_msgs::msg::Path &) { throw nav2_core::PatienceExceeded("Patience exceeded"); } @@ -92,7 +96,8 @@ class InvalidPathErrorController : public UnknownErrorController virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + nav2_core::GoalChecker *, + nav_msgs::msg::Path &) { throw nav2_core::InvalidPath("Invalid path"); } @@ -103,7 +108,8 @@ class NoValidControlErrorController : public UnknownErrorController virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + nav2_core::GoalChecker *, + nav_msgs::msg::Path &) { throw nav2_core::NoValidControl("No valid control"); } From 9cd76c9d5bf8d2004b9fdf97e156836cb954c2a2 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 20 Sep 2025 16:51:13 +0000 Subject: [PATCH 04/62] Add exact path following in path handler and parameter handler Signed-off-by: mini-1235 --- nav2_controller/CMakeLists.txt | 1 + .../nav2_controller/controller_server.hpp | 26 +- .../nav2_controller/parameter_handler.hpp | 48 ++- .../include/nav2_controller/path_handler.hpp | 98 +++++- nav2_controller/src/controller_server.cpp | 116 ++----- nav2_controller/src/parameter_handler.cpp | 282 +++--------------- nav2_controller/src/path_handler.cpp | 74 ++++- .../test/test_dynamic_parameters.cpp | 182 +++++------ .../nav2_costmap_2d/costmap_2d_ros.hpp | 6 - .../dwb_core/src/dwb_local_planner.cpp | 7 +- nav2_mppi_controller/README.md | 2 - .../nav2_mppi_controller/tools/utils.hpp | 53 ---- nav2_mppi_controller/test/utils_test.cpp | 63 ---- .../README.md | 2 - .../regulated_pure_pursuit_controller.hpp | 7 - .../src/parameter_handler.cpp | 6 +- .../src/regulated_pure_pursuit_controller.cpp | 61 ---- .../test/test_regulated_pp.cpp | 48 --- 18 files changed, 334 insertions(+), 748 deletions(-) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 7efc169aa9a..51a2c4fb3ff 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -47,6 +47,7 @@ target_link_libraries(${library_name} PUBLIC rclcpp_lifecycle::rclcpp_lifecycle ${rcl_interfaces_TARGETS} tf2_ros::tf2_ros + angles::angles ) target_link_libraries(${library_name} PRIVATE ${lifecycle_msgs_TARGETS} diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 414e0638c2a..7f429676c2a 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -37,6 +37,7 @@ #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_controller/path_handler.hpp" +#include "nav2_controller/parameter_handler.hpp" namespace nav2_controller { @@ -212,19 +213,12 @@ class ControllerServer : public nav2::LifecycleNode geometry_msgs::msg::Twist getThresholdedTwist(const geometry_msgs::msg::Twist & twist) { geometry_msgs::msg::Twist twist_thresh; - twist_thresh.linear.x = getThresholdedVelocity(twist.linear.x, min_x_velocity_threshold_); - twist_thresh.linear.y = getThresholdedVelocity(twist.linear.y, min_y_velocity_threshold_); - twist_thresh.angular.z = getThresholdedVelocity(twist.angular.z, min_theta_velocity_threshold_); + twist_thresh.linear.x = getThresholdedVelocity(twist.linear.x, params_->min_x_velocity_threshold); + twist_thresh.linear.y = getThresholdedVelocity(twist.linear.y, params_->min_y_velocity_threshold); + twist_thresh.angular.z = getThresholdedVelocity(twist.angular.z, params_->min_theta_velocity_threshold); return twist_thresh; } - /** - * @brief Callback executed when a parameter change is detected - * @param event ParameterEvent message - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); - // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; @@ -265,14 +259,6 @@ class ControllerServer : public nav2::LifecycleNode std::vector controller_types_; std::string controller_ids_concat_, current_controller_; - double controller_frequency_; - double min_x_velocity_threshold_; - double min_y_velocity_threshold_; - double min_theta_velocity_threshold_; - - double failure_tolerance_; - bool use_realtime_priority_; - bool publish_zero_velocity_; rclcpp::Duration costmap_update_timeout_; // Whether we've published the single controller warning yet @@ -284,9 +270,9 @@ class ControllerServer : public nav2::LifecycleNode // Current path container nav_msgs::msg::Path current_path_; nav_msgs::msg::Path local_path_; - bool interpolate_curvature_after_goal_; - double max_robot_pose_search_dist_; std::unique_ptr path_handler_; + std::unique_ptr param_handler_; + Parameters * params_; nav2::Publisher::SharedPtr global_path_pub_; private: diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index 0d89c1d22bd..662711872b1 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -32,36 +32,23 @@ namespace nav2_controller struct Parameters { - double desired_linear_vel, base_desired_linear_vel; - double lookahead_dist; - double rotate_to_heading_angular_vel; - double max_lookahead_dist; - double min_lookahead_dist; - double lookahead_time; - bool use_velocity_scaled_lookahead_dist; - double min_approach_linear_velocity; - double approach_velocity_scaling_dist; - double max_allowed_time_to_collision_up_to_carrot; - double min_distance_to_obstacle; - bool use_regulated_linear_velocity_scaling; - bool use_cost_regulated_linear_velocity_scaling; - double cost_scaling_dist; - double cost_scaling_gain; - double inflation_cost_scaling_factor; - double regulated_linear_scaling_min_radius; - double regulated_linear_scaling_min_speed; - bool use_fixed_curvature_lookahead; - double curvature_lookahead_dist; - bool use_rotate_to_heading; - double max_angular_accel; - bool use_cancel_deceleration; - double cancel_deceleration; - double rotate_to_heading_min_angle; - bool allow_reversing; - bool interpolate_curvature_after_goal; - bool use_collision_detection; + double controller_frequency; double transform_tolerance; - bool stateful; + double min_x_velocity_threshold; + double min_y_velocity_threshold; + double min_theta_velocity_threshold; + std::string speed_limit_topic; + double failure_tolerance; + bool use_realtime_priority; + bool publish_zero_velocity; + double costmap_update_timeout; + std::string odom_topic; + double odom_duration; + bool interpolate_curvature_after_goal; + double max_robot_pose_search_dist; + bool enforce_path_inversion; + float inversion_xy_tolerance; + float inversion_yaw_tolerance; }; /** @@ -76,8 +63,7 @@ class ParameterHandler */ ParameterHandler( nav2::LifecycleNode::SharedPtr node, - std::string & plugin_name, - rclcpp::Logger & logger, const double costmap_size_x); + const rclcpp::Logger & logger, const double costmap_size_x); /** * @brief Destrructor for nav2_controller::ParameterHandler diff --git a/nav2_controller/include/nav2_controller/path_handler.hpp b/nav2_controller/include/nav2_controller/path_handler.hpp index 72ff125e14b..a5689293a74 100644 --- a/nav2_controller/include/nav2_controller/path_handler.hpp +++ b/nav2_controller/include/nav2_controller/path_handler.hpp @@ -27,10 +27,12 @@ #include "nav2_util/odometry_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_core/controller_exceptions.hpp" -#include "geometry_msgs/msg/pose.hpp" +#include "nav2_controller/parameter_handler.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" namespace nav2_controller { +using PathIterator = std::vector::iterator; /** * @class nav2_controller::PathHandler @@ -43,12 +45,12 @@ class PathHandler * @brief Constructor for nav2_controller::PathHandler */ PathHandler( - double transform_tolerance, + Parameters * params, std::shared_ptr tf, std::shared_ptr costmap_ros); /** - * @brief Destrructor for nav2_controller::PathHandler + * @brief Destructor for nav2_controller::PathHandler */ ~PathHandler() = default; @@ -62,13 +64,26 @@ class PathHandler * @return Path in new frame */ nav_msgs::msg::Path pruneGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist, bool reject_unit_path = false); + const geometry_msgs::msg::PoseStamped & pose); - void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;} + void setPlan(const nav_msgs::msg::Path & path); nav_msgs::msg::Path getPlan() {return global_plan_;} + /** + * @brief Check if the robot pose is within the set inversion tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the set inversion tolerances + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); + + /** + * @brief Prune a path to only interesting portions + * @param plan Plan to prune + * @param end Final path iterator + */ + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + protected: /** * Get the greatest extent of the costmap in meters from the center. @@ -76,10 +91,79 @@ class PathHandler */ double getCostmapMaxExtent() const; - double transform_tolerance_; + /** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ + inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) + { + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existence of cusp, in the path, using the dot product. + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0f) { + return idx + 1; + } + + if ( + (hypot(oa_x, oa_y) == 0.0 && + path.poses[idx - 1].pose.orientation != + path.poses[idx].pose.orientation) + || + (hypot(ab_x, ab_y) == 0.0 && + path.poses[idx].pose.orientation != + path.poses[idx + 1].pose.orientation)) + { + // returning the distance since the points overlap + // but are not simply duplicate points (e.g. in place rotation) + return idx + 1; + } + } + + return path.poses.size(); + } + + /** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ + inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) + { + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; + } + std::shared_ptr tf_; std::shared_ptr costmap_ros_; nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; + unsigned int inversion_locale_{0u}; + Parameters * params_; }; } // namespace nav2_controller diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index deb8ecc8397..a5e68d0af95 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -48,25 +48,9 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) { RCLCPP_INFO(get_logger(), "Creating controller server"); - declare_parameter("controller_frequency", 20.0); - declare_parameter("progress_checker_plugins", default_progress_checker_ids_); declare_parameter("goal_checker_plugins", default_goal_checker_ids_); declare_parameter("controller_plugins", default_ids_); - declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); - declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); - declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001)); - - 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)); - declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true)); - declare_parameter("costmap_update_timeout", 0.30); // 300ms - - declare_parameter("odom_topic", rclcpp::ParameterValue("odom")); - declare_parameter("odom_duration", rclcpp::ParameterValue(0.3)); - declare_parameter("interpolate_curvature_after_goal", rclcpp::ParameterValue(false)); // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -122,29 +106,15 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) goal_checker_types_.resize(goal_checker_ids_.size()); progress_checker_types_.resize(progress_checker_ids_.size()); - get_parameter("controller_frequency", controller_frequency_); - get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_); - get_parameter("min_y_velocity_threshold", min_y_velocity_threshold_); - get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_); - RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); - - std::string speed_limit_topic, odom_topic; - get_parameter("speed_limit_topic", speed_limit_topic); - get_parameter("odom_topic", odom_topic); - double odom_duration; - get_parameter("odom_duration", odom_duration); - get_parameter("failure_tolerance", failure_tolerance_); - get_parameter("use_realtime_priority", use_realtime_priority_); - get_parameter("publish_zero_velocity", publish_zero_velocity_); - get_parameter("interpolate_curvature_after_goal", interpolate_curvature_after_goal_); - costmap_ros_->configure(); // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); - declare_parameter("max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_ros_->getCostmap()->getSizeInMetersX() / 2.0)); - get_parameter("max_robot_pose_search_dist", max_robot_pose_search_dist_); + param_handler_ = std::make_unique( + node, get_logger(), + costmap_ros_->getCostmap()->getSizeInMetersX()); + params_ = param_handler_->getParams(); path_handler_ = std::make_unique( - costmap_ros_->getTransformTolerance(), costmap_ros_->getTfBuffer(), costmap_ros_); + params_, costmap_ros_->getTfBuffer(), costmap_ros_); for (size_t i = 0; i != progress_checker_ids_.size(); i++) { try { @@ -230,13 +200,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_logger(), "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); - odom_sub_ = std::make_unique(node, odom_duration, odom_topic); + odom_sub_ = std::make_unique(node, params_->odom_duration, params_->odom_topic); vel_publisher_ = std::make_unique(node, "cmd_vel"); global_path_pub_ = node->create_publisher("received_global_plan"); - double costmap_update_timeout_dbl; - get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); - costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); + costmap_update_timeout_ = rclcpp::Duration::from_seconds(params_->costmap_update_timeout); // Create the action server that we implement with our followPath method // This may throw due to real-time prioritization if user doesn't have real-time permissions @@ -246,7 +214,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) std::bind(&ControllerServer::computeControl, this), nullptr, std::chrono::milliseconds(500), - true /*spin thread*/, use_realtime_priority_ /*soft realtime*/); + true /*spin thread*/, params_->use_realtime_priority /*soft realtime*/); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what()); on_cleanup(state); @@ -255,7 +223,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) // Set subscription to the speed limiting topic speed_limit_sub_ = create_subscription( - speed_limit_topic, + params_->speed_limit_topic, std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1)); return nav2::CallbackReturn::SUCCESS; @@ -279,9 +247,6 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) action_server_->activate(); auto node = shared_from_this(); - // Add callback for dynamic parameters - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&ControllerServer::dynamicParametersCallback, this, _1)); // create bond connection createBond(); @@ -476,7 +441,7 @@ void ControllerServer::computeControl() progress_checkers_[current_progress_checker_]->reset(); last_valid_cmd_time_ = now(); - rclcpp::WallRate loop_rate(controller_frequency_); + rclcpp::WallRate loop_rate(params_->controller_frequency); while (rclcpp::ok()) { auto start_time = this->now(); @@ -521,7 +486,7 @@ void ControllerServer::computeControl() RCLCPP_WARN( get_logger(), "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", - controller_frequency_, 1 / cycle_duration.seconds()); + params_->controller_frequency, 1 / cycle_duration.seconds()); } } } catch (nav2_core::InvalidController & e) { @@ -634,8 +599,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); - auto transformed_plan = path_handler_->pruneGlobalPlan( - pose, max_robot_pose_search_dist_, interpolate_curvature_after_goal_); + auto transformed_plan = path_handler_->pruneGlobalPlan(pose); // RCLCPP_INFO(get_logger(), "compute remaining distance %lf ",nav2_util::geometry_utils::calculate_path_length(transformed_plan)); global_path_pub_->publish(transformed_plan); local_path_ = transformed_plan; @@ -659,7 +623,7 @@ void ControllerServer::computeAndPublishVelocity() // Only no valid control exception types are valid to attempt to have control patience, as // other types will not be resolved with more attempts } catch (nav2_core::NoValidControl & e) { - if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { + if (params_->failure_tolerance > 0 || params_->failure_tolerance == -1.0) { RCLCPP_WARN(this->get_logger(), "%s", e.what()); cmd_vel_2d.twist.angular.x = 0; cmd_vel_2d.twist.angular.y = 0; @@ -669,8 +633,8 @@ void ControllerServer::computeAndPublishVelocity() cmd_vel_2d.twist.linear.z = 0; cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); cmd_vel_2d.header.stamp = now(); - if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ && - failure_tolerance_ != -1.0) + if ((now() - last_valid_cmd_time_).seconds() > params_->failure_tolerance && + params_->failure_tolerance != -1.0) { throw nav2_core::PatienceExceeded("Controller patience exceeded"); } @@ -686,7 +650,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; if (!nav2_util::transformPoseInTargetFrame( pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), - current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) + current_path_.header.frame_id, params_->transform_tolerance)) { throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); } @@ -793,7 +757,7 @@ void ControllerServer::publishZeroVelocity() void ControllerServer::onGoalExit(bool force_stop) { - if (publish_zero_velocity_ || force_stop) { + if (params_->publish_zero_velocity || force_stop) { publishZeroVelocity(); } @@ -817,7 +781,7 @@ bool ControllerServer::isGoalReached() end_pose_.header.stamp = pose.header.stamp; nav2_util::transformPoseInTargetFrame( end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), - costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()); + costmap_ros_->getGlobalFrameID(), params_->transform_tolerance); return goal_checkers_[current_goal_checker_]->isGoalReached( pose.pose, transformed_end_pose.pose, @@ -842,50 +806,6 @@ void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::Shar } } -rcl_interfaces::msg::SetParametersResult -ControllerServer::dynamicParametersCallback(std::vector parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - - for (auto parameter : parameters) { - const auto & param_type = parameter.get_type(); - const auto & param_name = parameter.get_name(); - - // If we are trying to change the parameter of a plugin we can just skip it at this point - // as they handle parameter changes themselves and don't need to lock the mutex - if (param_name.find('.') != std::string::npos) { - continue; - } - - if (!dynamic_params_lock_.try_lock()) { - RCLCPP_WARN( - get_logger(), - "Unable to dynamically change Parameters while the controller is currently running"); - result.successful = false; - result.reason = - "Unable to dynamically change Parameters while the controller is currently running"; - return result; - } - - if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == "min_x_velocity_threshold") { - min_x_velocity_threshold_ = parameter.as_double(); - } else if (param_name == "min_y_velocity_threshold") { - min_y_velocity_threshold_ = parameter.as_double(); - } else if (param_name == "min_theta_velocity_threshold") { - min_theta_velocity_threshold_ = parameter.as_double(); - } else if (param_name == "failure_tolerance") { - failure_tolerance_ = parameter.as_double(); - } - } - - dynamic_params_lock_.unlock(); - } - - result.successful = true; - return result; -} - } // namespace nav2_controller #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 4b72363bc5e..57e52bd9d2f 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -29,162 +29,78 @@ using rcl_interfaces::msg::ParameterType; ParameterHandler::ParameterHandler( nav2::LifecycleNode::SharedPtr node, - std::string & plugin_name, rclcpp::Logger & logger, + const rclcpp::Logger & logger, const double costmap_size_x) { node_ = node; - plugin_name_ = plugin_name; logger_ = logger; declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + node, "controller_frequency", rclcpp::ParameterValue(20.0)); declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", - rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".approach_velocity_scaling_dist", - rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_distance_to_obstacle", - rclcpp::ParameterValue(-1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + node, "min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter_if_not_declared( - node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + node, "min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + node, "min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001)); + declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + node, "speed_limit_topic", rclcpp::ParameterValue("speed_limit")); + declare_parameter_if_not_declared( - node, plugin_name_ + ".use_fixed_curvature_lookahead", rclcpp::ParameterValue(false)); + node, "failure_tolerance", rclcpp::ParameterValue(0.0)); declare_parameter_if_not_declared( - node, plugin_name_ + ".curvature_lookahead_dist", rclcpp::ParameterValue(0.6)); + node, "use_realtime_priority", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + node, "publish_zero_velocity", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + node, "costmap_update_timeout", rclcpp::ParameterValue(0.30)); + declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + node, "odom_topic", rclcpp::ParameterValue("odom")); declare_parameter_if_not_declared( - node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); + node, "odom_duration", rclcpp::ParameterValue(0.3)); declare_parameter_if_not_declared( - node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2)); + node, "interpolate_curvature_after_goal", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); + node, "max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_size_x)); declare_parameter_if_not_declared( - node, plugin_name_ + ".interpolate_curvature_after_goal", - rclcpp::ParameterValue(false)); + node, plugin_name_ + ".enforce_path_inversion", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node, plugin_name_ + ".use_collision_detection", - rclcpp::ParameterValue(true)); + node, plugin_name_ + ".inversion_xy_tolerance", rclcpp::ParameterValue(0.2)); declare_parameter_if_not_declared( - node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); + node, plugin_name_ + ".inversion_yaw_tolerance", rclcpp::ParameterValue(0.4)); - node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); - params_.base_desired_linear_vel = params_.desired_linear_vel; - node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist); - node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist); - node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist); - node->get_parameter(plugin_name_ + ".lookahead_time", params_.lookahead_time); - node->get_parameter( - plugin_name_ + ".rotate_to_heading_angular_vel", - params_.rotate_to_heading_angular_vel); - node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); - node->get_parameter( - plugin_name_ + ".use_velocity_scaled_lookahead_dist", - params_.use_velocity_scaled_lookahead_dist); - node->get_parameter( - plugin_name_ + ".min_approach_linear_velocity", - params_.min_approach_linear_velocity); - node->get_parameter( - plugin_name_ + ".approach_velocity_scaling_dist", - params_.approach_velocity_scaling_dist); - if (params_.approach_velocity_scaling_dist > costmap_size_x / 2.0) { - RCLCPP_WARN( - logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " - "leading to permanent slowdown"); - } - node->get_parameter( - plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - params_.max_allowed_time_to_collision_up_to_carrot); - node->get_parameter( - plugin_name_ + ".min_distance_to_obstacle", - params_.min_distance_to_obstacle); - node->get_parameter( - plugin_name_ + ".use_regulated_linear_velocity_scaling", - params_.use_regulated_linear_velocity_scaling); - node->get_parameter( - plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - params_.use_cost_regulated_linear_velocity_scaling); - node->get_parameter(plugin_name_ + ".cost_scaling_dist", params_.cost_scaling_dist); - node->get_parameter(plugin_name_ + ".cost_scaling_gain", params_.cost_scaling_gain); - node->get_parameter( - plugin_name_ + ".inflation_cost_scaling_factor", - params_.inflation_cost_scaling_factor); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_radius", - params_.regulated_linear_scaling_min_radius); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_speed", - params_.regulated_linear_scaling_min_speed); - node->get_parameter( - plugin_name_ + ".use_fixed_curvature_lookahead", - params_.use_fixed_curvature_lookahead); - node->get_parameter( - plugin_name_ + ".curvature_lookahead_dist", - params_.curvature_lookahead_dist); - node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading); - node->get_parameter( - plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); - node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); - node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); - node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); - node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); + node->get_parameter("controller_frequency", params_.controller_frequency); + node->get_parameter("transform_tolerance", params_.transform_tolerance); + node->get_parameter("min_x_velocity_threshold", params_.min_x_velocity_threshold); + node->get_parameter("min_y_velocity_threshold", params_.min_y_velocity_threshold); + node->get_parameter("min_theta_velocity_threshold", params_.min_theta_velocity_threshold); + + RCLCPP_INFO( + node->get_logger(), + "Controller frequency set to %.4fHz", + params_.controller_frequency); + node->get_parameter("speed_limit_topic", params_.speed_limit_topic); + node->get_parameter("odom_topic", params_.odom_topic); + node->get_parameter("odom_duration", params_.odom_duration); + + node->get_parameter("failure_tolerance", params_.failure_tolerance); + node->get_parameter("use_realtime_priority", params_.use_realtime_priority); + node->get_parameter("publish_zero_velocity", params_.publish_zero_velocity); node->get_parameter( - plugin_name_ + ".interpolate_curvature_after_goal", + "interpolate_curvature_after_goal", params_.interpolate_curvature_after_goal); - if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) { - RCLCPP_WARN( - logger_, "For interpolate_curvature_after_goal to be set to true, " - "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling."); - params_.interpolate_curvature_after_goal = false; - } - node->get_parameter( - plugin_name_ + ".use_collision_detection", - params_.use_collision_detection); - node->get_parameter(plugin_name_ + ".stateful", params_.stateful); + node->get_parameter("max_robot_pose_search_dist", params_.max_robot_pose_search_dist); + node->get_parameter("costmap_update_timeout", params_.costmap_update_timeout); + node->get_parameter(plugin_name_ + ".enforce_path_inversion", params_.enforce_path_inversion); + node->get_parameter(plugin_name_ + ".inversion_xy_tolerance", params_.inversion_xy_tolerance); + node->get_parameter(plugin_name_ + ".inversion_yaw_tolerance", params_.inversion_yaw_tolerance); - if (params_.inflation_cost_scaling_factor <= 0.0) { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Disabling cost regulated linear velocity scaling."); - params_.use_cost_regulated_linear_velocity_scaling = false; - } post_set_params_handler_ = node->add_post_set_parameters_callback( std::bind( @@ -209,119 +125,17 @@ ParameterHandler::~ParameterHandler() on_set_params_handler_.reset(); } rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback( - std::vector parameters) + std::vector /*parameters*/) { rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (auto parameter : parameters) { - const auto & param_type = parameter.get_type(); - const auto & param_name = parameter.get_name(); - if (param_name.find(plugin_name_ + ".") != 0) { - continue; - } - if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == plugin_name_ + ".inflation_cost_scaling_factor" && - parameter.as_double() <= 0.0) - { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Ignoring parameter update."); - result.successful = false; - } else if (parameter.as_double() < 0.0) { - RCLCPP_WARN( - logger_, "The value of parameter '%s' is incorrectly set to %f, " - "it should be >=0. Ignoring parameter update.", - param_name.c_str(), parameter.as_double()); - result.successful = false; - } - } else if (param_type == ParameterType::PARAMETER_BOOL) { - if (param_name == plugin_name_ + ".allow_reversing") { - if (params_.use_rotate_to_heading && parameter.as_bool()) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - result.successful = false; - } - } - } - } + return result; } void ParameterHandler::updateParametersCallback( - std::vector parameters) + std::vector /*parameters*/) { - std::lock_guard lock_reinit(mutex_); - for (const auto & parameter : parameters) { - const auto & param_type = parameter.get_type(); - const auto & param_name = parameter.get_name(); - - if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") { - params_.inflation_cost_scaling_factor = parameter.as_double(); - } else if (param_name == plugin_name_ + ".desired_linear_vel") { - params_.desired_linear_vel = parameter.as_double(); - params_.base_desired_linear_vel = parameter.as_double(); - } else if (param_name == plugin_name_ + ".lookahead_dist") { - params_.lookahead_dist = parameter.as_double(); - } else if (param_name == plugin_name_ + ".max_lookahead_dist") { - params_.max_lookahead_dist = parameter.as_double(); - } else if (param_name == plugin_name_ + ".min_lookahead_dist") { - params_.min_lookahead_dist = parameter.as_double(); - } else if (param_name == plugin_name_ + ".lookahead_time") { - params_.lookahead_time = parameter.as_double(); - } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") { - params_.rotate_to_heading_angular_vel = parameter.as_double(); - } else if (param_name == plugin_name_ + ".min_approach_linear_velocity") { - params_.min_approach_linear_velocity = parameter.as_double(); - } else if (param_name == plugin_name_ + ".curvature_lookahead_dist") { - params_.curvature_lookahead_dist = parameter.as_double(); - } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { - params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double(); - } else if (param_name == plugin_name_ + ".min_distance_to_obstacle") { - params_.min_distance_to_obstacle = parameter.as_double(); - } else if (param_name == plugin_name_ + ".cost_scaling_dist") { - params_.cost_scaling_dist = parameter.as_double(); - } else if (param_name == plugin_name_ + ".cost_scaling_gain") { - params_.cost_scaling_gain = parameter.as_double(); - } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_radius") { - params_.regulated_linear_scaling_min_radius = parameter.as_double(); - } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_speed") { - params_.regulated_linear_scaling_min_speed = parameter.as_double(); - } else if (param_name == plugin_name_ + ".max_angular_accel") { - params_.max_angular_accel = parameter.as_double(); - } else if (param_name == plugin_name_ + ".cancel_deceleration") { - params_.cancel_deceleration = parameter.as_double(); - } else if (param_name == plugin_name_ + ".rotate_to_heading_min_angle") { - params_.rotate_to_heading_min_angle = parameter.as_double(); - } else if (param_name == plugin_name_ + ".transform_tolerance") { - params_.transform_tolerance = parameter.as_double(); - } - } else if (param_type == ParameterType::PARAMETER_BOOL) { - if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { - params_.use_velocity_scaled_lookahead_dist = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { - params_.use_regulated_linear_velocity_scaling = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".use_fixed_curvature_lookahead") { - params_.use_fixed_curvature_lookahead = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { - params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".use_collision_detection") { - params_.use_collision_detection = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".stateful") { - params_.stateful = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".use_rotate_to_heading") { - params_.use_rotate_to_heading = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".use_cancel_deceleration") { - params_.use_cancel_deceleration = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".allow_reversing") { - params_.allow_reversing = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") { - params_.interpolate_curvature_after_goal = parameter.as_bool(); - } - } - } } } // namespace nav2_controller diff --git a/nav2_controller/src/path_handler.cpp b/nav2_controller/src/path_handler.cpp index c258edeef29..b2afcc2d287 100644 --- a/nav2_controller/src/path_handler.cpp +++ b/nav2_controller/src/path_handler.cpp @@ -24,6 +24,7 @@ #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" +#include "angles/angles.h" namespace nav2_controller { @@ -31,11 +32,17 @@ namespace nav2_controller using nav2_util::geometry_utils::euclidean_distance; PathHandler::PathHandler( - double transform_tolerance, + Parameters * params, std::shared_ptr tf, std::shared_ptr costmap_ros) -: transform_tolerance_(transform_tolerance), tf_(tf), costmap_ros_(costmap_ros) { + params_ = params; + costmap_ros_ = costmap_ros; + tf_ = tf; + + if(params_->enforce_path_inversion) { + inversion_locale_ = 0u; + } } double PathHandler::getCostmapMaxExtent() const @@ -46,37 +53,67 @@ double PathHandler::getCostmapMaxExtent() const return max_costmap_dim_meters / 2.0; } +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) +{ + plan.poses.erase(plan.poses.begin(), end); +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + float distance = hypotf( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + float angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= params_->inversion_xy_tolerance && + fabs(angle_distance) <= params_->inversion_yaw_tolerance; +} + +void PathHandler::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; + global_plan_up_to_inversion_ = global_plan_; + if(params_->enforce_path_inversion) { + inversion_locale_ = removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } +} + nav_msgs::msg::Path PathHandler::pruneGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist, - bool reject_unit_path) + const geometry_msgs::msg::PoseStamped & pose) { - if (global_plan_.poses.empty()) { + if (global_plan_up_to_inversion_.poses.empty()) { throw nav2_core::InvalidPath("Received plan with zero length"); } - if (reject_unit_path && global_plan_.poses.size() == 1) { + if (params_->interpolate_curvature_after_goal && global_plan_up_to_inversion_.poses.size() == 1) { throw nav2_core::InvalidPath("Received plan with length of one"); } // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, global_plan_.header.frame_id, - transform_tolerance_)) + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, + global_plan_up_to_inversion_.header.frame_id, + params_->transform_tolerance)) { throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); } auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist); + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + params_->max_robot_pose_search_dist); // First find the closest pose on the path to the robot // bounded by when the path turns around (if it does) so we don't get a pose from a later // portion of the path auto transformation_begin = nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), closest_pose_upper_bound, + global_plan_up_to_inversion_.poses.begin(), closest_pose_upper_bound, [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { return euclidean_distance(robot_pose, ps); }); @@ -84,7 +121,8 @@ nav_msgs::msg::Path PathHandler::pruneGlobalPlan( // Make sure we always have at least 2 points on the transformed plan and that we don't prune // the global plan below 2 points in order to have always enough point to interpolate the // end of path direction - if (global_plan_.poses.begin() != closest_pose_upper_bound && global_plan_.poses.size() > 1 && + if (global_plan_up_to_inversion_.poses.begin() != closest_pose_upper_bound && + global_plan_up_to_inversion_.poses.size() > 1 && transformation_begin == std::prev(closest_pose_upper_bound)) { transformation_begin = std::prev(std::prev(closest_pose_upper_bound)); @@ -93,7 +131,7 @@ nav_msgs::msg::Path PathHandler::pruneGlobalPlan( // We'll discard points on the plan that are outside the local costmap const double max_costmap_extent = getCostmapMaxExtent(); auto transformation_end = std::find_if( - transformation_begin, global_plan_.poses.end(), + transformation_begin, global_plan_up_to_inversion_.poses.end(), [&](const auto & global_plan_pose) { return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; }); @@ -105,7 +143,15 @@ nav_msgs::msg::Path PathHandler::pruneGlobalPlan( // Remove the portion of the global plan that we've already passed so we don't // process it on the next iteration (this is called path pruning) - global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + prunePlan(global_plan_up_to_inversion_, transformation_begin); + + if (params_->enforce_path_inversion && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(robot_pose)) { + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + inversion_locale_ = removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } if (pruned_plan.poses.empty()) { throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp index 2e29a97d03e..b03f0f8a8c7 100644 --- a/nav2_controller/test/test_dynamic_parameters.cpp +++ b/nav2_controller/test/test_dynamic_parameters.cpp @@ -1,91 +1,91 @@ -// Copyright (c) 2021, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include - -#include -#include -#include - -#include "gtest/gtest.h" -#include "nav2_ros_common/lifecycle_node.hpp" -#include "nav2_controller/controller_server.hpp" -#include "rclcpp/rclcpp.hpp" - -class ControllerShim : public nav2_controller::ControllerServer -{ -public: - ControllerShim() - : nav2_controller::ControllerServer(rclcpp::NodeOptions()) - { - } - - // Since we cannot call configure/activate due to costmaps - // requiring TF - void setDynamicCallback() - { - auto node = shared_from_this(); - // Add callback for dynamic parameters - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&ControllerShim::dynamicParamsShim, this, std::placeholders::_1)); - } - - rcl_interfaces::msg::SetParametersResult - dynamicParamsShim(std::vector parameters) - { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - dynamicParametersCallback(parameters); - return result; - } -}; - -TEST(WPTest, test_dynamic_parameters) -{ - auto controller = std::make_shared(); - controller->setDynamicCallback(); - - auto rec_param = std::make_shared( - controller->get_node_base_interface(), controller->get_node_topics_interface(), - controller->get_node_graph_interface(), - controller->get_node_services_interface()); - - auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("min_x_velocity_threshold", 100.0), - rclcpp::Parameter("min_y_velocity_threshold", 100.0), - rclcpp::Parameter("min_theta_velocity_threshold", 100.0), - rclcpp::Parameter("failure_tolerance", 5.0)}); - - rclcpp::spin_until_future_complete( - controller->get_node_base_interface(), - results); - - EXPECT_EQ(controller->get_parameter("min_x_velocity_threshold").as_double(), 100.0); - EXPECT_EQ(controller->get_parameter("min_y_velocity_threshold").as_double(), 100.0); - EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0); - EXPECT_EQ(controller->get_parameter("failure_tolerance").as_double(), 5.0); -} - -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest(&argc, argv); - - rclcpp::init(0, nullptr); - - int result = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - - return result; -} +// // Copyright (c) 2021, Samsung Research America +// // +// // Licensed under the Apache License, Version 2.0 (the "License"); +// // you may not use this file except in compliance with the License. +// // You may obtain a copy of the License at +// // +// // http://www.apache.org/licenses/LICENSE-2.0 +// // +// // Unless required by applicable law or agreed to in writing, software +// // distributed under the License is distributed on an "AS IS" BASIS, +// // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// // See the License for the specific language governing permissions and +// // limitations under the License. Reserved. + +// #include + +// #include +// #include +// #include + +// #include "gtest/gtest.h" +// #include "nav2_ros_common/lifecycle_node.hpp" +// #include "nav2_controller/controller_server.hpp" +// #include "rclcpp/rclcpp.hpp" + +// class ControllerShim : public nav2_controller::ControllerServer +// { +// public: +// ControllerShim() +// : nav2_controller::ControllerServer(rclcpp::NodeOptions()) +// { +// } + +// // Since we cannot call configure/activate due to costmaps +// // requiring TF +// void setDynamicCallback() +// { +// auto node = shared_from_this(); +// // Add callback for dynamic parameters +// dyn_params_handler_ = node->add_on_set_parameters_callback( +// std::bind(&ControllerShim::dynamicParamsShim, this, std::placeholders::_1)); +// } + +// rcl_interfaces::msg::SetParametersResult +// dynamicParamsShim(std::vector parameters) +// { +// rcl_interfaces::msg::SetParametersResult result; +// result.successful = true; +// dynamicParametersCallback(parameters); +// return result; +// } +// }; + +// TEST(WPTest, test_dynamic_parameters) +// { +// auto controller = std::make_shared(); +// controller->setDynamicCallback(); + +// auto rec_param = std::make_shared( +// controller->get_node_base_interface(), controller->get_node_topics_interface(), +// controller->get_node_graph_interface(), +// controller->get_node_services_interface()); + +// auto results = rec_param->set_parameters_atomically( +// {rclcpp::Parameter("min_x_velocity_threshold", 100.0), +// rclcpp::Parameter("min_y_velocity_threshold", 100.0), +// rclcpp::Parameter("min_theta_velocity_threshold", 100.0), +// rclcpp::Parameter("failure_tolerance", 5.0)}); + +// rclcpp::spin_until_future_complete( +// controller->get_node_base_interface(), +// results); + +// EXPECT_EQ(controller->get_parameter("min_x_velocity_threshold").as_double(), 100.0); +// EXPECT_EQ(controller->get_parameter("min_y_velocity_threshold").as_double(), 100.0); +// EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0); +// EXPECT_EQ(controller->get_parameter("failure_tolerance").as_double(), 5.0); +// } + +// int main(int argc, char **argv) +// { +// ::testing::InitGoogleTest(&argc, argv); + +// rclcpp::init(0, nullptr); + +// int result = RUN_ALL_TESTS(); + +// rclcpp::shutdown(); + +// return result; +// } 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 5b1fa01591b..d1cda80b174 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 @@ -212,12 +212,6 @@ class Costmap2DROS : public nav2::LifecycleNode return name_; } - /** @brief Returns the delay in transform (tf) data that is tolerable in seconds */ - double getTransformTolerance() const - { - return transform_tolerance_; - } - /** * @brief Return a pointer to the "master" costmap which receives updates from all the layers. * diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 0ffbda6f3d8..2ec86b73a13 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -98,9 +98,6 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".trajectory_generator_name", rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator"))); - declare_parameter_if_not_declared( - node, dwb_plugin_name_ + ".transform_tolerance", - rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".shorten_transformed_plan", rclcpp::ParameterValue(true)); @@ -110,9 +107,7 @@ void DWBLocalPlanner::configure( std::string traj_generator_name; - double transform_tolerance; - node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance_); - RCLCPP_INFO(logger_, "Setting transform_tolerance to %f", transform_tolerance); + node->get_parameter("transform_tolerance", transform_tolerance_); node->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_); node->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 630aa19f214..ed8ff8b2bf4 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -73,7 +73,6 @@ This process is then repeated a number of times and returns a converged solution | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. | | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | - | transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. | | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. In addition, these cusping points will also be treated by the critics as local goals that the robot will attempt to reach. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | | inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. | | inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. | @@ -206,7 +205,6 @@ controller_server: wz_max: 1.9 iteration_count: 1 prune_distance: 1.7 - transform_tolerance: 0.1 temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 4a3cc6b7c06..f3b05d95539 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -510,59 +510,6 @@ inline void savitskyGolayFilter( control_sequence.wz(offset)}; } -/** - * @brief Find the iterator of the first pose at which there is an inversion on the path, - * @param path to check for inversion - * @return the first point after the inversion found in the path - */ -inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) -{ - // At least 3 poses for a possible inversion - if (path.poses.size() < 3) { - return path.poses.size(); - } - - // Iterating through the path to determine the position of the path inversion - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - float oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - float oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - float ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - float ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existence of cusp, in the path, using the dot product. - float dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0f) { - return idx + 1; - } - } - - return path.poses.size(); -} - -/** - * @brief Find and remove poses after the first inversion in the path - * @param path to check for inversion - * @return The location of the inversion, return 0 if none exist - */ -inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) -{ - nav_msgs::msg::Path cropped_path = path; - const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); - if (first_after_inversion == path.poses.size()) { - return 0u; - } - - cropped_path.poses.erase( - cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); - path = cropped_path; - return first_after_inversion; -} - /** * @brief Compare to trajectory points to find closest path point along integrated distances * @param vec Vect to check diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 40e8d624168..630695ebe99 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -341,69 +341,6 @@ TEST(UtilsTests, SmootherTest) EXPECT_LT(smoothed_val, original_val); } -TEST(UtilsTests, FindPathInversionTest) -{ - // Straight path, no inversions to be found - nav_msgs::msg::Path path; - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - EXPECT_EQ(utils::findFirstPathInversion(path), 10u); - - // To short to process - path.poses.erase(path.poses.begin(), path.poses.begin() + 7); - EXPECT_EQ(utils::findFirstPathInversion(path), 3u); - - // Has inversion at index 10, so should return 11 for the first point afterwards - // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 - path.poses.clear(); - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = 10 - i; - path.poses.push_back(pose); - } - EXPECT_EQ(utils::findFirstPathInversion(path), 11u); -} - -TEST(UtilsTests, RemovePosesAfterPathInversionTest) -{ - nav_msgs::msg::Path path; - // straight path - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); - - // try empty path - path.poses.clear(); - EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); - - // cusping path - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = 10 - i; - path.poses.push_back(pose); - } - EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u); - // Check to see if removed - EXPECT_EQ(path.poses.size(), 11u); - EXPECT_EQ(path.poses.back().pose.position.x, 10); -} - TEST(UtilsTests, ShiftColumnsByOnePlaceTest) { // Try with scalar value diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 491da4cab06..ae2cd1a60ea 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -71,7 +71,6 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | | `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | | `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | -| `transform_tolerance` | The TF transform tolerance | | `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | | `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | | `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | @@ -122,7 +121,6 @@ controller_server: max_lookahead_dist: 0.9 lookahead_time: 1.5 rotate_to_heading_angular_vel: 1.8 - transform_tolerance: 0.1 use_velocity_scaled_lookahead_dist: false min_approach_linear_velocity: 0.05 approach_velocity_scaling_dist: 1.0 diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 68f0735c587..7a01eccbef6 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -174,13 +174,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign); - /** - * @brief checks for the cusp position - * @param pose Pose input to determine the cusp position - * @return robot distance from the cusp - */ - double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); - nav2::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; std::string plugin_name_; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 009d0263099..8e859ab6a57 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -48,8 +48,6 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", rclcpp::ParameterValue(false)); @@ -113,7 +111,7 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".rotate_to_heading_angular_vel", params_.rotate_to_heading_angular_vel); - node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); + node->get_parameter("transform_tolerance", params_.transform_tolerance); node->get_parameter( plugin_name_ + ".use_velocity_scaled_lookahead_dist", params_.use_velocity_scaled_lookahead_dist); @@ -295,8 +293,6 @@ ParameterHandler::updateParametersCallback( params_.cancel_deceleration = parameter.as_double(); } else if (param_name == plugin_name_ + ".rotate_to_heading_min_angle") { params_.rotate_to_heading_min_angle = parameter.as_double(); - } else if (param_name == plugin_name_ + ".transform_tolerance") { - params_.transform_tolerance = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { 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 75b04c2f6d8..afc5091a023 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 @@ -178,20 +178,6 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity double lookahead_dist = getLookAheadDistance(speed); double curv_lookahead_dist = params_->curvature_lookahead_dist; - // Check for reverse driving - if (params_->allow_reversing) { - // Cusp check - const double dist_to_cusp = findVelocitySignChange(transformed_plan); - - // if the lookahead distance is further than the cusp, use the cusp distance instead - if (dist_to_cusp < lookahead_dist) { - lookahead_dist = dist_to_cusp; - } - if (dist_to_cusp < curv_lookahead_dist) { - curv_lookahead_dist = dist_to_cusp; - } - } - // Get the particular point on the path at the lookahead distance auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_plan); auto rotate_to_path_carrot_pose = carrot_pose; @@ -409,53 +395,6 @@ void RegulatedPurePursuitController::reset() finished_cancelling_ = false; has_reached_xy_tolerance_ = false; } - -double RegulatedPurePursuitController::findVelocitySignChange( - const nav_msgs::msg::Path & transformed_plan) -{ - // Iterating through the transformed global path to determine the position of the cusp - for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = transformed_plan.poses[pose_id].pose.position.x - - transformed_plan.poses[pose_id - 1].pose.position.x; - double oa_y = transformed_plan.poses[pose_id].pose.position.y - - transformed_plan.poses[pose_id - 1].pose.position.y; - double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x - - transformed_plan.poses[pose_id].pose.position.x; - double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y - - transformed_plan.poses[pose_id].pose.position.y; - - /* Checking for the existence 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. */ - 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); - } - - 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); - } - } - - return std::numeric_limits::max(); -} } // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index b470e03c4ee..03a6a55ca62 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -77,12 +77,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure curvature, curr_speed, pose_cost, path, linear_vel, sign); } - - double findVelocitySignChangeWrapper( - const nav_msgs::msg::Path & transformed_plan) - { - return findVelocitySignChange(transformed_plan); - } }; TEST(RegulatedPurePursuitTest, basicAPI) @@ -137,46 +131,6 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) EXPECT_EQ(rtn->point.z, 0.01); } -TEST(RegulatedPurePursuitTest, findVelocitySignChange) -{ - auto node = std::make_shared("testRPPfindVelocitySignChange"); - auto ctrl = std::make_shared(); - - std::string name = "PathFollower"; - auto tf = std::make_shared(node->get_clock()); - auto costmap = std::make_shared("fake_costmap"); - rclcpp_lifecycle::State state; - costmap->on_configure(state); - ctrl->configure(node, name, tf, costmap); - - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = "smb"; - auto time = node->get_clock()->now(); - pose.header.stamp = time; - pose.pose.position.x = 1.0; - pose.pose.position.y = 0.0; - - nav_msgs::msg::Path path; - path.poses.resize(3); - path.header.frame_id = "smb"; - path.header.stamp = pose.header.stamp; - path.poses[0].pose.position.x = 1.0; - path.poses[0].pose.position.y = 1.0; - path.poses[1].pose.position.x = 2.0; - path.poses[1].pose.position.y = 2.0; - path.poses[2].pose.position.x = -1.0; - path.poses[2].pose.position.y = -1.0; - ctrl->setPlan(path); - auto rtn = ctrl->findVelocitySignChangeWrapper(path); - EXPECT_EQ(rtn, sqrt(8.0)); - - path.poses[2].pose.position.x = 3.0; - path.poses[2].pose.position.y = 3.0; - ctrl->setPlan(path); - rtn = ctrl->findVelocitySignChangeWrapper(path); - EXPECT_EQ(rtn, std::numeric_limits::max()); -} - TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared(); @@ -443,7 +397,6 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) rclcpp::Parameter("test.cost_scaling_dist", 2.0), rclcpp::Parameter("test.cost_scaling_gain", 4.0), rclcpp::Parameter("test.regulated_linear_scaling_min_radius", 10.0), - rclcpp::Parameter("test.transform_tolerance", 30.0), rclcpp::Parameter("test.max_angular_accel", 3.0), rclcpp::Parameter("test.rotate_to_heading_min_angle", 0.7), rclcpp::Parameter("test.regulated_linear_scaling_min_speed", 4.0), @@ -473,7 +426,6 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.cost_scaling_dist").as_double(), 2.0); EXPECT_EQ(node->get_parameter("test.cost_scaling_gain").as_double(), 4.0); EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_radius").as_double(), 10.0); - EXPECT_EQ(node->get_parameter("test.transform_tolerance").as_double(), 30.0); EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 3.0); EXPECT_EQ(node->get_parameter("test.rotate_to_heading_min_angle").as_double(), 0.7); EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_speed").as_double(), 4.0); From b3da38b29710a626ec89d3d773c1d25fc6fc9f27 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 21 Sep 2025 15:22:38 +0000 Subject: [PATCH 05/62] Move more parameters Signed-off-by: mini-1235 --- .../nav2_controller/controller_server.hpp | 12 --- .../nav2_controller/parameter_handler.hpp | 13 +++ nav2_controller/src/controller_server.cpp | 102 ++++++------------ nav2_controller/src/parameter_handler.cpp | 85 +++++++++++++-- nav2_controller/test/CMakeLists.txt | 17 +-- .../test/test_dynamic_parameters.cpp | 91 ---------------- nav2_mppi_controller/README.md | 1 - 7 files changed, 130 insertions(+), 191 deletions(-) delete mode 100644 nav2_controller/test/test_dynamic_parameters.cpp diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 7f429676c2a..be8a229e252 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -235,28 +235,16 @@ class ControllerServer : public nav2::LifecycleNode // Progress Checker Plugin pluginlib::ClassLoader progress_checker_loader_; ProgressCheckerMap progress_checkers_; - std::vector default_progress_checker_ids_; - std::vector default_progress_checker_types_; - std::vector progress_checker_ids_; - std::vector progress_checker_types_; std::string progress_checker_ids_concat_, current_progress_checker_; // Goal Checker Plugin pluginlib::ClassLoader goal_checker_loader_; GoalCheckerMap goal_checkers_; - std::vector default_goal_checker_ids_; - std::vector default_goal_checker_types_; - std::vector goal_checker_ids_; - std::vector goal_checker_types_; std::string goal_checker_ids_concat_, current_goal_checker_; // Controller Plugins pluginlib::ClassLoader lp_loader_; ControllerMap controllers_; - std::vector default_ids_; - std::vector default_types_; - std::vector controller_ids_; - std::vector controller_types_; std::string controller_ids_concat_, current_controller_; rclcpp::Duration costmap_update_timeout_; diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index 662711872b1..368805eb7f8 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -49,6 +49,12 @@ struct Parameters bool enforce_path_inversion; float inversion_xy_tolerance; float inversion_yaw_tolerance; + std::vector progress_checker_ids; + std::vector progress_checker_types; + std::vector goal_checker_ids; + std::vector goal_checker_types; + std::vector controller_ids; + std::vector controller_types; }; /** @@ -91,6 +97,13 @@ class ParameterHandler Parameters params_; std::string plugin_name_; rclcpp::Logger logger_ {rclcpp::get_logger("GracefulMotionController")}; + + const std::vector default_progress_checker_ids_{"progress_checker"}; + const std::vector default_progress_checker_types_{"nav2_controller::SimpleProgressChecker"}; + const std::vector default_goal_checker_ids_{"goal_checker"}; + const std::vector default_goal_checker_types_{"nav2_controller::PathCompleteGoalChecker"}; + const std::vector default_controller_ids_{"FollowPath"}; + const std::vector default_controller_types_{"dwb_core::DWBLocalPlanner"}; }; } // namespace nav2_controller diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index a5e68d0af95..5b7d5f2e2b4 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -36,22 +36,12 @@ namespace nav2_controller ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) : nav2::LifecycleNode("controller_server", "", options), progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), - default_progress_checker_ids_{"progress_checker"}, - default_progress_checker_types_{"nav2_controller::SimpleProgressChecker"}, goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), - default_goal_checker_ids_{"goal_checker"}, - default_goal_checker_types_{"nav2_controller::PathCompleteGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), - default_ids_{"FollowPath"}, - default_types_{"dwb_core::DWBLocalPlanner"}, costmap_update_timeout_(300ms) { RCLCPP_INFO(get_logger(), "Creating controller server"); - declare_parameter("progress_checker_plugins", default_progress_checker_ids_); - declare_parameter("goal_checker_plugins", default_goal_checker_ids_); - declare_parameter("controller_plugins", default_ids_); - // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( "local_costmap", std::string{get_namespace()}, @@ -73,60 +63,30 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "Configuring controller interface"); - RCLCPP_INFO(get_logger(), "getting progress checker plugins.."); - get_parameter("progress_checker_plugins", progress_checker_ids_); - if (progress_checker_ids_ == default_progress_checker_ids_) { - for (size_t i = 0; i < default_progress_checker_ids_.size(); ++i) { - nav2::declare_parameter_if_not_declared( - node, default_progress_checker_ids_[i] + ".plugin", - rclcpp::ParameterValue(default_progress_checker_types_[i])); - } - } - - RCLCPP_INFO(get_logger(), "getting goal checker plugins.."); - get_parameter("goal_checker_plugins", goal_checker_ids_); - if (goal_checker_ids_ == default_goal_checker_ids_) { - for (size_t i = 0; i < default_goal_checker_ids_.size(); ++i) { - nav2::declare_parameter_if_not_declared( - node, default_goal_checker_ids_[i] + ".plugin", - rclcpp::ParameterValue(default_goal_checker_types_[i])); - } - } - - get_parameter("controller_plugins", controller_ids_); - if (controller_ids_ == default_ids_) { - for (size_t i = 0; i < default_ids_.size(); ++i) { - nav2::declare_parameter_if_not_declared( - node, default_ids_[i] + ".plugin", - rclcpp::ParameterValue(default_types_[i])); - } - } - - controller_types_.resize(controller_ids_.size()); - goal_checker_types_.resize(goal_checker_ids_.size()); - progress_checker_types_.resize(progress_checker_ids_.size()); - costmap_ros_->configure(); // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); - param_handler_ = std::make_unique( - node, get_logger(), - costmap_ros_->getCostmap()->getSizeInMetersX()); - params_ = param_handler_->getParams(); + try { + param_handler_ = std::make_unique( + node, get_logger(), costmap_ros_->getCostmap()->getSizeInMetersX()); + params_ = param_handler_->getParams(); + } catch (const std::exception & ex) { + RCLCPP_FATAL(get_logger(), "%s", ex.what()); + on_cleanup(state); + return nav2::CallbackReturn::FAILURE; + } path_handler_ = std::make_unique( params_, costmap_ros_->getTfBuffer(), costmap_ros_); - for (size_t i = 0; i != progress_checker_ids_.size(); i++) { + for (size_t i = 0; i != params_->progress_checker_ids.size(); i++) { try { - progress_checker_types_[i] = nav2::get_plugin_type_param( - node, progress_checker_ids_[i]); nav2_core::ProgressChecker::Ptr progress_checker = - progress_checker_loader_.createUniqueInstance(progress_checker_types_[i]); + progress_checker_loader_.createUniqueInstance(params_->progress_checker_types[i]); RCLCPP_INFO( get_logger(), "Created progress_checker : %s of type %s", - progress_checker_ids_[i].c_str(), progress_checker_types_[i].c_str()); - progress_checker->initialize(node, progress_checker_ids_[i]); - progress_checkers_.insert({progress_checker_ids_[i], progress_checker}); + params_->progress_checker_ids[i].c_str(), params_->progress_checker_types[i].c_str()); + progress_checker->initialize(node, params_->progress_checker_ids[i]); + progress_checkers_.insert({params_->progress_checker_ids[i], progress_checker}); } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), @@ -136,24 +96,23 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } } - for (size_t i = 0; i != progress_checker_ids_.size(); i++) { - progress_checker_ids_concat_ += progress_checker_ids_[i] + std::string(" "); + for (size_t i = 0; i != params_->progress_checker_ids.size(); i++) { + progress_checker_ids_concat_ += params_->progress_checker_ids[i] + std::string(" "); } RCLCPP_INFO( get_logger(), "Controller Server has %s progress checkers available.", progress_checker_ids_concat_.c_str()); - for (size_t i = 0; i != goal_checker_ids_.size(); i++) { + for (size_t i = 0; i != params_->goal_checker_ids.size(); i++) { try { - goal_checker_types_[i] = nav2::get_plugin_type_param(node, goal_checker_ids_[i]); nav2_core::GoalChecker::Ptr goal_checker = - goal_checker_loader_.createUniqueInstance(goal_checker_types_[i]); + goal_checker_loader_.createUniqueInstance(params_->goal_checker_types[i]); RCLCPP_INFO( get_logger(), "Created goal checker : %s of type %s", - goal_checker_ids_[i].c_str(), goal_checker_types_[i].c_str()); - goal_checker->initialize(node, goal_checker_ids_[i], costmap_ros_); - goal_checkers_.insert({goal_checker_ids_[i], goal_checker}); + params_->goal_checker_ids[i].c_str(), params_->goal_checker_types[i].c_str()); + goal_checker->initialize(node, params_->goal_checker_ids[i], costmap_ros_); + goal_checkers_.insert({params_->goal_checker_ids[i], goal_checker}); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( get_logger(), @@ -163,26 +122,25 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } } - for (size_t i = 0; i != goal_checker_ids_.size(); i++) { - goal_checker_ids_concat_ += goal_checker_ids_[i] + std::string(" "); + for (size_t i = 0; i != params_->goal_checker_ids.size(); i++) { + goal_checker_ids_concat_ += params_->goal_checker_ids[i] + std::string(" "); } RCLCPP_INFO( get_logger(), "Controller Server has %s goal checkers available.", goal_checker_ids_concat_.c_str()); - for (size_t i = 0; i != controller_ids_.size(); i++) { + for (size_t i = 0; i != params_->controller_ids.size(); i++) { try { - controller_types_[i] = nav2::get_plugin_type_param(node, controller_ids_[i]); nav2_core::Controller::Ptr controller = - lp_loader_.createUniqueInstance(controller_types_[i]); + lp_loader_.createUniqueInstance(params_->controller_types[i]); RCLCPP_INFO( get_logger(), "Created controller : %s of type %s", - controller_ids_[i].c_str(), controller_types_[i].c_str()); + params_->controller_ids[i].c_str(), params_->controller_types[i].c_str()); controller->configure( - node, controller_ids_[i], + node, params_->controller_ids[i], costmap_ros_->getTfBuffer(), costmap_ros_); - controllers_.insert({controller_ids_[i], controller}); + controllers_.insert({params_->controller_ids[i], controller}); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( get_logger(), @@ -192,8 +150,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } } - for (size_t i = 0; i != controller_ids_.size(); i++) { - controller_ids_concat_ += controller_ids_[i] + std::string(" "); + for (size_t i = 0; i != params_->controller_ids.size(); i++) { + controller_ids_concat_ += params_->controller_ids[i] + std::string(" "); } RCLCPP_INFO( diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 57e52bd9d2f..89c36c413a5 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -68,11 +68,18 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, "max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_size_x)); declare_parameter_if_not_declared( - node, plugin_name_ + ".enforce_path_inversion", rclcpp::ParameterValue(false)); + node, "enforce_path_inversion", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node, plugin_name_ + ".inversion_xy_tolerance", rclcpp::ParameterValue(0.2)); + node, "inversion_xy_tolerance", rclcpp::ParameterValue(0.2)); declare_parameter_if_not_declared( - node, plugin_name_ + ".inversion_yaw_tolerance", rclcpp::ParameterValue(0.4)); + node, "inversion_yaw_tolerance", rclcpp::ParameterValue(0.4)); + + declare_parameter_if_not_declared( + node, "progress_checker_plugins", rclcpp::ParameterValue(default_progress_checker_ids_)); + declare_parameter_if_not_declared( + node, "goal_checker_plugins", rclcpp::ParameterValue(default_goal_checker_ids_)); + declare_parameter_if_not_declared( + node, "controller_plugins", rclcpp::ParameterValue(default_controller_ids_)); node->get_parameter("controller_frequency", params_.controller_frequency); node->get_parameter("transform_tolerance", params_.transform_tolerance); @@ -81,7 +88,7 @@ ParameterHandler::ParameterHandler( node->get_parameter("min_theta_velocity_threshold", params_.min_theta_velocity_threshold); RCLCPP_INFO( - node->get_logger(), + logger_, "Controller frequency set to %.4fHz", params_.controller_frequency); @@ -97,10 +104,74 @@ ParameterHandler::ParameterHandler( params_.interpolate_curvature_after_goal); node->get_parameter("max_robot_pose_search_dist", params_.max_robot_pose_search_dist); node->get_parameter("costmap_update_timeout", params_.costmap_update_timeout); - node->get_parameter(plugin_name_ + ".enforce_path_inversion", params_.enforce_path_inversion); - node->get_parameter(plugin_name_ + ".inversion_xy_tolerance", params_.inversion_xy_tolerance); - node->get_parameter(plugin_name_ + ".inversion_yaw_tolerance", params_.inversion_yaw_tolerance); + node->get_parameter("enforce_path_inversion", params_.enforce_path_inversion); + node->get_parameter("inversion_xy_tolerance", params_.inversion_xy_tolerance); + node->get_parameter("inversion_yaw_tolerance", params_.inversion_yaw_tolerance); + + RCLCPP_INFO(logger_, "getting progress checker plugins.."); + node->get_parameter("progress_checker_plugins", params_.progress_checker_ids); + if (params_.progress_checker_ids == default_progress_checker_ids_) { + for (size_t i = 0; i < default_progress_checker_ids_.size(); ++i) { + nav2::declare_parameter_if_not_declared( + node, default_progress_checker_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_progress_checker_types_[i])); + } + } + + RCLCPP_INFO(logger_, "getting goal checker plugins.."); + node->get_parameter("goal_checker_plugins", params_.goal_checker_ids); + if (params_.goal_checker_ids == default_goal_checker_ids_) { + for (size_t i = 0; i < default_goal_checker_ids_.size(); ++i) { + nav2::declare_parameter_if_not_declared( + node, default_goal_checker_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_goal_checker_types_[i])); + } + } + node->get_parameter("controller_plugins", params_.controller_ids); + if (params_.controller_ids == default_controller_ids_) { + for (size_t i = 0; i < default_controller_ids_.size(); ++i) { + nav2::declare_parameter_if_not_declared( + node, default_controller_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_controller_types_[i])); + } + } + + params_.controller_types.resize(params_.controller_ids.size()); + params_.goal_checker_types.resize(params_.goal_checker_ids.size()); + params_.progress_checker_types.resize(params_.progress_checker_ids.size()); + + for (size_t i = 0; i != params_.progress_checker_ids.size(); i++) { + try { + params_.progress_checker_types[i] = nav2::get_plugin_type_param( + node, params_.progress_checker_ids[i]); + } catch (const std::exception & ex) { + throw std::runtime_error( + std::string("Failed to get type for progress_checker '") + + params_.progress_checker_ids[i] + "': " + ex.what()); + } + } + + for (size_t i = 0; i != params_.goal_checker_ids.size(); i++) { + try { + params_.goal_checker_types[i] = + nav2::get_plugin_type_param(node, params_.goal_checker_ids[i]); + } catch (const std::exception & ex) { + throw std::runtime_error( + std::string("Failed to get type for goal_checker '") + + params_.goal_checker_ids[i] + "': " + ex.what()); + } + } + + for (size_t i = 0; i != params_.controller_ids.size(); i++) { + try { + params_.controller_types[i] = nav2::get_plugin_type_param(node, params_.controller_ids[i]); + } catch (const std::exception & ex) { + throw std::runtime_error( + std::string("Failed to get type for controller plugins '") + + params_.controller_types[i] + "': " + ex.what()); + } + } post_set_params_handler_ = node->add_post_set_parameters_callback( std::bind( diff --git a/nav2_controller/test/CMakeLists.txt b/nav2_controller/test/CMakeLists.txt index 16d75498b43..edec8b53292 100644 --- a/nav2_controller/test/CMakeLists.txt +++ b/nav2_controller/test/CMakeLists.txt @@ -1,9 +1,10 @@ +#TODO: add path handler and parameter handler test # Test dynamic parameters -ament_add_gtest(test_dynamic_parameters - test_dynamic_parameters.cpp -) -target_link_libraries(test_dynamic_parameters - ${library_name} - nav2_util::nav2_util_core - rclcpp::rclcpp -) +# ament_add_gtest(path_handler_test +# path_handler_test.cpp +# ) +# target_link_libraries(path_handler_test +# ${library_name} +# nav2_util::nav2_util_core +# rclcpp::rclcpp +# ) diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp deleted file mode 100644 index b03f0f8a8c7..00000000000 --- a/nav2_controller/test/test_dynamic_parameters.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// // Copyright (c) 2021, Samsung Research America -// // -// // Licensed under the Apache License, Version 2.0 (the "License"); -// // you may not use this file except in compliance with the License. -// // You may obtain a copy of the License at -// // -// // http://www.apache.org/licenses/LICENSE-2.0 -// // -// // Unless required by applicable law or agreed to in writing, software -// // distributed under the License is distributed on an "AS IS" BASIS, -// // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// // See the License for the specific language governing permissions and -// // limitations under the License. Reserved. - -// #include - -// #include -// #include -// #include - -// #include "gtest/gtest.h" -// #include "nav2_ros_common/lifecycle_node.hpp" -// #include "nav2_controller/controller_server.hpp" -// #include "rclcpp/rclcpp.hpp" - -// class ControllerShim : public nav2_controller::ControllerServer -// { -// public: -// ControllerShim() -// : nav2_controller::ControllerServer(rclcpp::NodeOptions()) -// { -// } - -// // Since we cannot call configure/activate due to costmaps -// // requiring TF -// void setDynamicCallback() -// { -// auto node = shared_from_this(); -// // Add callback for dynamic parameters -// dyn_params_handler_ = node->add_on_set_parameters_callback( -// std::bind(&ControllerShim::dynamicParamsShim, this, std::placeholders::_1)); -// } - -// rcl_interfaces::msg::SetParametersResult -// dynamicParamsShim(std::vector parameters) -// { -// rcl_interfaces::msg::SetParametersResult result; -// result.successful = true; -// dynamicParametersCallback(parameters); -// return result; -// } -// }; - -// TEST(WPTest, test_dynamic_parameters) -// { -// auto controller = std::make_shared(); -// controller->setDynamicCallback(); - -// auto rec_param = std::make_shared( -// controller->get_node_base_interface(), controller->get_node_topics_interface(), -// controller->get_node_graph_interface(), -// controller->get_node_services_interface()); - -// auto results = rec_param->set_parameters_atomically( -// {rclcpp::Parameter("min_x_velocity_threshold", 100.0), -// rclcpp::Parameter("min_y_velocity_threshold", 100.0), -// rclcpp::Parameter("min_theta_velocity_threshold", 100.0), -// rclcpp::Parameter("failure_tolerance", 5.0)}); - -// rclcpp::spin_until_future_complete( -// controller->get_node_base_interface(), -// results); - -// EXPECT_EQ(controller->get_parameter("min_x_velocity_threshold").as_double(), 100.0); -// EXPECT_EQ(controller->get_parameter("min_y_velocity_threshold").as_double(), 100.0); -// EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0); -// EXPECT_EQ(controller->get_parameter("failure_tolerance").as_double(), 5.0); -// } - -// int main(int argc, char **argv) -// { -// ::testing::InitGoogleTest(&argc, argv); - -// rclcpp::init(0, nullptr); - -// int result = RUN_ALL_TESTS(); - -// rclcpp::shutdown(); - -// return result; -// } diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index ed8ff8b2bf4..ee2fa3660df 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -71,7 +71,6 @@ This process is then repeated a number of times and returns a converged solution #### Path Handler | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. | | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. In addition, these cusping points will also be treated by the critics as local goals that the robot will attempt to reach. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | | inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. | From 6945d17707017a183ff2a0f40e60b6303388f9eb Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 21 Sep 2025 15:34:54 +0000 Subject: [PATCH 06/62] Rotation shim Signed-off-by: mini-1235 --- .../nav2_rotation_shim_controller.hpp | 3 +- .../src/nav2_rotation_shim_controller.cpp | 6 ++-- .../test/test_shim_controller.cpp | 28 +++++++++---------- 3 files changed, 19 insertions(+), 18 deletions(-) diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index e36805e91d0..f0dd25cc813 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -82,13 +82,14 @@ class RotationShimController : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands + * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @return Best command */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & transformed_global_plan) override; + nav_msgs::msg::Path & pruned_global_plan) override; /** * @brief nav2_core setPlan - Sets the global plan diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index d7cbeddd058..f9b43831963 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -169,7 +169,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & transformed_global_plan) + nav_msgs::msg::Path & pruned_global_plan) { // Rotate to goal heading when in goal xy tolerance if (rotate_to_goal_heading_) { @@ -190,7 +190,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands goal_checker->getTolerances(pose_tolerance, vel_tolerance); position_goal_checker_->setXYGoalTolerance(pose_tolerance.position.x); - if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity, transformed_global_plan)) { + if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity, pruned_global_plan)) { double pose_yaw = tf2::getYaw(pose.pose.orientation); double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation); @@ -256,7 +256,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands // If at this point, use the primary controller to path track in_rotation_ = false; - auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker, transformed_global_plan); + auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker, pruned_global_plan); last_angular_vel_ = cmd_vel.twist.angular.z; return cmd_vel; } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 15c33f33dc6..eb7c7e89c4d 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -267,13 +267,13 @@ TEST(RotationShimControllerTest, computeVelocityTests) // send without setting a path - should go to RPP immediately // then it should throw an exception because the path is empty and invalid - nav_msgs::msg::Path transformed_global_plan; - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan), std::runtime_error); + nav_msgs::msg::Path pruned_global_plan; + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), std::runtime_error); // Set with a path -- should attempt to find a sampled point but throw exception // because it cannot be found, then go to RPP and throw exception because it cannot be transformed controller->setPlan(path); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan), std::runtime_error); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), std::runtime_error); path.header.frame_id = "base_link"; path.poses[1].pose.position.x = 0.1; @@ -289,7 +289,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // is 0.5 and that should cause a rotation in place controller->setPlan(path); tf_broadcaster->sendTransform(transform); - auto effort = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); + auto effort = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); path.header.frame_id = "base_link"; @@ -307,7 +307,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // and exception because it is off of the costmap controller->setPlan(path); tf_broadcaster->sendTransform(transform); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan), std::runtime_error); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), std::runtime_error); } TEST(RotationShimControllerTest, openLoopRotationTests) { @@ -379,13 +379,13 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { // Calculate first velocity command controller->setPlan(path); - nav_msgs::msg::Path transformed_global_plan; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); + nav_msgs::msg::Path pruned_global_plan; + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); // Test second velocity command with wrong odometry velocity.angular.z = 1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4); } @@ -451,15 +451,15 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[3].header.frame_id = "base_link"; controller->setPlan(path); - nav_msgs::msg::Path transformed_global_plan; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); + nav_msgs::msg::Path pruned_global_plan; + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); // goal heading 45 degrees to the right path.poses[3].pose.orientation.z = 0.3826834; path.poses[3].pose.orientation.w = 0.9238795; controller->setPlan(path); - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } @@ -532,13 +532,13 @@ TEST(RotationShimControllerTest, accelerationTests) { // Test acceleration limits controller->setPlan(path); - nav_msgs::msg::Path transformed_global_plan; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); + nav_msgs::msg::Path pruned_global_plan; + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); // Test slowing down to avoid overshooting velocity.angular.z = -1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_NEAR(cmd_vel.twist.angular.z, -std::sqrt(2 * 0.5 * M_PI / 4), 1e-4); } From d6ec61a733cf34f49f170fdac4ee64fd259ca08b Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 21 Sep 2025 16:09:30 +0000 Subject: [PATCH 07/62] RPP Signed-off-by: mini-1235 --- .../regulated_pure_pursuit_controller.hpp | 4 ++- .../src/regulated_pure_pursuit_controller.cpp | 34 ++++++++++++++++++- .../test/test_regulated_pp.cpp | 8 ----- 3 files changed, 36 insertions(+), 10 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7a01eccbef6..e0653e1dcf3 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -89,13 +89,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands + * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @return Best command */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & transformed_plan) override; + nav_msgs::msg::Path & pruned_global_plan) override; bool cancel() override; @@ -189,6 +190,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool is_rotating_to_heading_ = false; bool has_reached_xy_tolerance_ = false; + nav2::Publisher::SharedPtr transformed_path_pub_; nav2::Publisher::SharedPtr carrot_pub_; nav2::Publisher::SharedPtr curvature_carrot_pub_; nav2::Publisher::SharedPtr is_rotating_to_heading_pub_; 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 afc5091a023..25c17ffa915 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 @@ -69,6 +69,7 @@ void RegulatedPurePursuitController::configure( node->get_parameter("controller_frequency", control_frequency); control_duration_ = 1.0 / control_frequency; + transformed_path_pub_ = node->create_publisher("transformed_pruned_plan"); carrot_pub_ = node->create_publisher("lookahead_point"); curvature_carrot_pub_ = node->create_publisher( "curvature_lookahead_point"); @@ -83,6 +84,7 @@ void RegulatedPurePursuitController::cleanup() "Cleaning up controller: %s of type" " regulated_pure_pursuit_controller::RegulatedPurePursuitController", plugin_name_.c_str()); + transformed_path_pub_.reset(); carrot_pub_.reset(); curvature_carrot_pub_.reset(); is_rotating_to_heading_pub_.reset(); @@ -95,6 +97,7 @@ void RegulatedPurePursuitController::activate() "Activating controller: %s of type " "regulated_pure_pursuit_controller::RegulatedPurePursuitController", plugin_name_.c_str()); + transformed_path_pub_->on_activate(); carrot_pub_->on_activate(); curvature_carrot_pub_->on_activate(); is_rotating_to_heading_pub_->on_activate(); @@ -107,6 +110,7 @@ void RegulatedPurePursuitController::deactivate() "Deactivating controller: %s of type " "regulated_pure_pursuit_controller::RegulatedPurePursuitController", plugin_name_.c_str()); + transformed_path_pub_->on_activate(); carrot_pub_->on_deactivate(); curvature_carrot_pub_->on_deactivate(); is_rotating_to_heading_pub_->on_deactivate(); @@ -158,7 +162,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & speed, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & transformed_plan) + nav_msgs::msg::Path & pruned_global_plan) { std::lock_guard lock_reinit(param_handler_->getMutex()); @@ -174,6 +178,34 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity goal_dist_tol_ = pose_tolerance.position.x; } + // Transform the pruned global plan to robot base frame + auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; + stamped_pose.header.stamp = pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + + if (!nav2_util::transformPoseInTargetFrame( + stamped_pose, transformed_pose, *tf_, + costmap_ros_->getBaseFrameID(), params_->transform_tolerance)) + { + throw nav2_core::ControllerTFError( + "Unable to transform plan pose into local frame"); + } + + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = pose.header.stamp; + std::transform( + pruned_global_plan.poses.begin(), + pruned_global_plan.poses.end(), + std::back_inserter(transformed_plan.poses), + transformGlobalPlanToLocal); + // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); double curv_lookahead_dist = params_->curvature_lookahead_dist; diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 03a6a55ca62..d0f7cb30e5b 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -94,14 +94,6 @@ TEST(RegulatedPurePursuitTest, basicAPI) ctrl->deactivate(); ctrl->cleanup(); - // setPlan and get plan - // nav_msgs::msg::Path path; - // path.poses.resize(2); - // path.poses[0].header.frame_id = "fake_frame"; - // ctrl->setPlan(path); - // EXPECT_EQ(ctrl->getPlan().poses.size(), 2ul); - // EXPECT_EQ(ctrl->getPlan().poses[0].header.frame_id, std::string("fake_frame")); - // set speed limit const double base_speed = ctrl->getSpeed(); EXPECT_EQ(ctrl->getSpeed(), base_speed); From fb204d24d31b2737c03b8c354e872bb5df601786 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 21 Sep 2025 16:42:35 +0000 Subject: [PATCH 08/62] Graceful Signed-off-by: mini-1235 --- nav2_graceful_controller/README.md | 1 - .../graceful_controller.hpp | 4 +- .../src/graceful_controller.cpp | 37 +- .../src/parameter_handler.cpp | 8 +- .../test/test_graceful_controller.cpp | 324 +----------------- .../src/regulated_pure_pursuit_controller.cpp | 1 + 6 files changed, 51 insertions(+), 324 deletions(-) diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index b962e491dcc..fa1d4a123cc 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -15,7 +15,6 @@ The smooth control law is a pose-following kinematic control law that generates | Parameter | Description | |-----|----| -| `transform_tolerance` | The TF transform tolerance. | | `motion_target_dist` | The lookahead distance to use to find the motion_target point. This distance should be a value around 1.0m but not much farther away. Greater values will cause the robot to generate smoother paths but not necessarily follow the path as closely. | | `k_phi` | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r. **Note**: This variable is called k1 in earlier versions of the paper. | | `k_delta` | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading. **Note**: This variable is called k2 in earlier versions of the paper. | diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 519cf780d80..8814a2c184f 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -83,13 +83,14 @@ class GracefulController : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands + * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @return Best command */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & transformed_global_plan) override; + nav_msgs::msg::Path & pruned_global_plan) override; /** * @brief nav2_core setPlan - Sets the global plan. @@ -189,6 +190,7 @@ class GracefulController : public nav2_core::Controller // True from the time a new path arrives until we have completed an initial rotation bool do_initial_rotation_; + nav2::Publisher::SharedPtr transformed_path_pub_; nav2::Publisher::SharedPtr local_plan_pub_; nav2::Publisher::SharedPtr motion_target_pub_; nav2::Publisher::SharedPtr slowdown_pub_; diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index e794f3fd5e6..6ef48477831 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -58,6 +58,7 @@ void GracefulController::configure( } // Publishers + transformed_path_pub_ = node->create_publisher("transformed_pruned_plan"); local_plan_pub_ = node->create_publisher("local_plan"); motion_target_pub_ = node->create_publisher("motion_target"); slowdown_pub_ = node->create_publisher("slowdown"); @@ -71,6 +72,7 @@ void GracefulController::cleanup() logger_, "Cleaning up controller: %s of type graceful_controller::GracefulController", plugin_name_.c_str()); + transformed_path_pub_.reset(); local_plan_pub_.reset(); motion_target_pub_.reset(); slowdown_pub_.reset(); @@ -85,6 +87,7 @@ void GracefulController::activate() logger_, "Activating controller: %s of type nav2_graceful_controller::GracefulController", plugin_name_.c_str()); + transformed_path_pub_->on_activate(); local_plan_pub_->on_activate(); motion_target_pub_->on_activate(); slowdown_pub_->on_activate(); @@ -96,6 +99,7 @@ void GracefulController::deactivate() logger_, "Deactivating controller: %s of type nav2_graceful_controller::GracefulController", plugin_name_.c_str()); + transformed_path_pub_->on_deactivate(); local_plan_pub_->on_deactivate(); motion_target_pub_->on_deactivate(); slowdown_pub_->on_deactivate(); @@ -105,7 +109,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & /*velocity*/, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & transformed_plan) + nav_msgs::msg::Path & pruned_global_plan) { std::lock_guard param_lock(param_handler_->getMutex()); @@ -127,9 +131,40 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( control_law_->setSlowdownRadius(params_->slowdown_radius); control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); + // Transform the pruned global plan to robot base frame + auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; + stamped_pose.header.stamp = pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + + if (!nav2_util::transformPoseInTargetFrame( + stamped_pose, transformed_pose, *tf_buffer_, + costmap_ros_->getBaseFrameID(), params_->transform_tolerance)) + { + throw nav2_core::ControllerTFError( + "Unable to transform plan pose into local frame"); + } + + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = pose.header.stamp; + std::transform( + pruned_global_plan.poses.begin(), + pruned_global_plan.poses.end(), + std::back_inserter(transformed_plan.poses), + transformGlobalPlanToLocal); + // Add proper orientations to plan, if needed validateOrientations(transformed_plan.poses); + // Publish plan for visualization + transformed_path_pub_->publish(transformed_plan); + // Transform local frame to global frame to use in collision checking geometry_msgs::msg::TransformStamped costmap_transform; try { diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 11ae289738d..a74163d1897 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -35,8 +35,6 @@ ParameterHandler::ParameterHandler( plugin_name_ = plugin_name; logger_ = logger; - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( node, plugin_name_ + ".min_lookahead", rclcpp::ParameterValue(0.25)); declare_parameter_if_not_declared( @@ -70,7 +68,7 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); - node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); + node->get_parameter("transform_tolerance", params_.transform_tolerance); node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead); node->get_parameter(plugin_name_ + ".max_lookahead", params_.max_lookahead); @@ -131,9 +129,7 @@ ParameterHandler::dynamicParametersCallback(std::vector param } if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == plugin_name_ + ".transform_tolerance") { - params_.transform_tolerance = parameter.as_double(); - } else if (param_name == plugin_name_ + ".min_lookahead") { + if (param_name == plugin_name_ + ".min_lookahead") { params_.min_lookahead = parameter.as_double(); } else if (param_name == plugin_name_ + ".max_lookahead") { params_.max_lookahead = parameter.as_double(); diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index d364ae4fb94..e8c899fe2dd 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -201,31 +201,6 @@ TEST(SmoothControlLawTest, calculateNextPose) { EXPECT_NEAR(tf2::getYaw(next_pose.orientation), 0.0, 0.1); } -TEST(GracefulControllerTest, configure) { - auto node = std::make_shared("testGraceful"); - auto tf = std::make_shared(node->get_clock()); - auto costmap_ros = std::make_shared("global_costmap"); - - // Create controller - auto controller = std::make_shared(); - costmap_ros->on_configure(rclcpp_lifecycle::State()); - controller->configure(node, "test", tf, costmap_ros); - controller->activate(); - - // Set the plan - nav_msgs::msg::Path plan; - plan.header.frame_id = "map"; - plan.poses.resize(3); - plan.poses[0].header.frame_id = "map"; - // controller->setPlan(plan); - // EXPECT_EQ(controller->getPlan().poses.size(), 3u); - // EXPECT_EQ(controller->getPlan().poses[0].header.frame_id, "map"); - - // Cleaning up - controller->deactivate(); - controller->cleanup(); -} - TEST(GracefulControllerTest, dynamicParameters) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -255,8 +230,7 @@ TEST(GracefulControllerTest, dynamicParameters) { // Set parameters auto results = params->set_parameters_atomically( - {rclcpp::Parameter("test.transform_tolerance", 1.0), - rclcpp::Parameter("test.min_lookahead", 1.0), + {rclcpp::Parameter("test.min_lookahead", 1.0), rclcpp::Parameter("test.max_lookahead", 2.0), rclcpp::Parameter("test.k_phi", 4.0), rclcpp::Parameter("test.k_delta", 5.0), @@ -279,7 +253,6 @@ TEST(GracefulControllerTest, dynamicParameters) { rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); // Check parameters - EXPECT_EQ(node->get_parameter("test.transform_tolerance").as_double(), 1.0); EXPECT_EQ(node->get_parameter("test.min_lookahead").as_double(), 1.0); EXPECT_EQ(node->get_parameter("test.max_lookahead").as_double(), 2.0); EXPECT_EQ(node->get_parameter("test.k_phi").as_double(), 4.0); @@ -478,285 +451,6 @@ TEST(GracefulControllerTest, setSpeedLimit) { EXPECT_EQ(speed_limit, 2.0); } -TEST(GracefulControllerTest, emptyPlan) { - auto node = std::make_shared("testGraceful"); - auto tf = std::make_shared(node->get_clock()); - - // Create a costmap of 10x10 meters - auto costmap_ros = std::make_shared("test_costmap"); - auto results = costmap_ros->set_parameters( - {rclcpp::Parameter("global_frame", "test_global_frame"), - rclcpp::Parameter("robot_base_frame", "test_robot_frame"), - rclcpp::Parameter("width", 10), - rclcpp::Parameter("height", 10), - rclcpp::Parameter("resolution", 0.1)}); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } - costmap_ros->on_configure(rclcpp_lifecycle::State()); - - // Create controller - auto controller = std::make_shared(); - controller->configure(node, "test", tf, costmap_ros); - controller->activate(); - - // Create the robot pose - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = "test_robot_frame"; - robot_pose.pose.position.x = 0.0; - robot_pose.pose.position.y = 0.0; - robot_pose.pose.position.z = 0.0; - - // Set transform between global and robot frame - geometry_msgs::msg::TransformStamped global_to_robot; - global_to_robot.header.frame_id = "test_global_frame"; - global_to_robot.header.stamp = node->get_clock()->now(); - global_to_robot.child_frame_id = "test_robot_frame"; - global_to_robot.transform.translation.x = robot_pose.pose.position.x; - global_to_robot.transform.translation.y = robot_pose.pose.position.y; - global_to_robot.transform.translation.z = robot_pose.pose.position.z; - tf->setTransform(global_to_robot, "test", false); - - // Set an empty global plan - nav_msgs::msg::Path global_plan; - global_plan.header.frame_id = "test_global_frame"; - controller->setPlan(global_plan); -} - -TEST(GracefulControllerTest, poseOutsideCostmap) { - auto node = std::make_shared("testGraceful"); - auto tf = std::make_shared(node->get_clock()); - - // Create a costmap of 10x10 meters - auto costmap_ros = std::make_shared("test_costmap"); - auto results = costmap_ros->set_parameters( - {rclcpp::Parameter("global_frame", "test_global_frame"), - rclcpp::Parameter("robot_base_frame", "test_robot_frame"), - rclcpp::Parameter("width", 10), - rclcpp::Parameter("height", 10), - rclcpp::Parameter("resolution", 0.1)}); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } - costmap_ros->on_configure(rclcpp_lifecycle::State()); - - // Create controller - auto controller = std::make_shared(); - controller->configure(node, "test", tf, costmap_ros); - controller->activate(); - - // Create the robot pose - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = "test_robot_frame"; - robot_pose.pose.position.x = 500.0; - robot_pose.pose.position.y = 500.0; - robot_pose.pose.position.z = 0.0; - - // Set transform between global and robot frame - geometry_msgs::msg::TransformStamped global_to_robot; - global_to_robot.header.frame_id = "test_global_frame"; - global_to_robot.header.stamp = node->get_clock()->now(); - global_to_robot.child_frame_id = "test_robot_frame"; - global_to_robot.transform.translation.x = robot_pose.pose.position.x; - global_to_robot.transform.translation.y = robot_pose.pose.position.y; - global_to_robot.transform.translation.z = robot_pose.pose.position.z; - tf->setTransform(global_to_robot, "test", false); - - // Set an empty global plan - nav_msgs::msg::Path global_plan; - global_plan.header.frame_id = "test_global_frame"; - global_plan.poses.resize(1); - global_plan.poses[0].header.frame_id = "test_global_frame"; - global_plan.poses[0].pose.position.x = 0.0; - global_plan.poses[0].pose.position.y = 0.0; - controller->setPlan(global_plan); -} - -TEST(GracefulControllerTest, noPruningPlan) { - auto node = std::make_shared("testGraceful"); - auto tf = std::make_shared(node->get_clock()); - - // Create a costmap of 10x10 meters - auto costmap_ros = std::make_shared("test_costmap"); - auto results = costmap_ros->set_parameters( - {rclcpp::Parameter("global_frame", "test_global_frame"), - rclcpp::Parameter("robot_base_frame", "test_robot_frame"), - rclcpp::Parameter("width", 10), - rclcpp::Parameter("height", 10), - rclcpp::Parameter("resolution", 0.1)}); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } - costmap_ros->on_configure(rclcpp_lifecycle::State()); - - // Create controller - auto controller = std::make_shared(); - controller->configure(node, "test", tf, costmap_ros); - controller->activate(); - - // Create the robot pose - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = "test_robot_frame"; - robot_pose.pose.position.x = 0.0; - robot_pose.pose.position.y = 0.0; - robot_pose.pose.position.z = 0.0; - - // Set transform between global and robot frame - geometry_msgs::msg::TransformStamped global_to_robot; - global_to_robot.header.frame_id = "test_global_frame"; - global_to_robot.header.stamp = node->get_clock()->now(); - global_to_robot.child_frame_id = "test_robot_frame"; - global_to_robot.transform.translation.x = robot_pose.pose.position.x; - global_to_robot.transform.translation.y = robot_pose.pose.position.y; - global_to_robot.transform.translation.z = robot_pose.pose.position.z; - tf->setTransform(global_to_robot, "test", false); - - // Set a linear global plan - nav_msgs::msg::Path global_plan; - global_plan.header.frame_id = "test_global_frame"; - global_plan.poses.resize(3); - global_plan.poses[0].header.frame_id = "test_global_frame"; - global_plan.poses[0].pose.position.x = 0.0; - global_plan.poses[0].pose.position.y = 0.0; - global_plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - global_plan.poses[1].header.frame_id = "test_global_frame"; - global_plan.poses[1].pose.position.x = 1.0; - global_plan.poses[1].pose.position.y = 1.0; - global_plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - global_plan.poses[2].header.frame_id = "test_global_frame"; - global_plan.poses[2].pose.position.x = 3.0; - global_plan.poses[2].pose.position.y = 3.0; - global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(global_plan); -} - -TEST(GracefulControllerTest, pruningPlan) { - auto node = std::make_shared("testGraceful"); - auto tf = std::make_shared(node->get_clock()); - - // Create a costmap of 20x20 meters - auto costmap_ros = std::make_shared("test_costmap"); - auto results = costmap_ros->set_parameters( - {rclcpp::Parameter("global_frame", "test_global_frame"), - rclcpp::Parameter("robot_base_frame", "test_robot_frame"), - rclcpp::Parameter("width", 20), - rclcpp::Parameter("height", 20), - rclcpp::Parameter("resolution", 0.1)}); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } - costmap_ros->on_configure(rclcpp_lifecycle::State()); - - // Create controller - auto controller = std::make_shared(); - controller->configure(node, "test", tf, costmap_ros); - controller->activate(); - - // Create the robot pose - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = "test_robot_frame"; - robot_pose.pose.position.x = 0.0; - robot_pose.pose.position.y = 0.0; - robot_pose.pose.position.z = 0.0; - - // Set transform between global and robot frame - geometry_msgs::msg::TransformStamped global_to_robot; - global_to_robot.header.frame_id = "test_global_frame"; - global_to_robot.header.stamp = node->get_clock()->now(); - global_to_robot.child_frame_id = "test_robot_frame"; - global_to_robot.transform.translation.x = robot_pose.pose.position.x; - global_to_robot.transform.translation.y = robot_pose.pose.position.y; - global_to_robot.transform.translation.z = robot_pose.pose.position.z; - tf->setTransform(global_to_robot, "test", false); - - // Set a linear global plan - nav_msgs::msg::Path global_plan; - global_plan.header.frame_id = "test_global_frame"; - global_plan.poses.resize(6); - global_plan.poses[0].header.frame_id = "test_global_frame"; - global_plan.poses[0].pose.position.x = 0.0; - global_plan.poses[0].pose.position.y = 0.0; - global_plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - global_plan.poses[1].header.frame_id = "test_global_frame"; - global_plan.poses[1].pose.position.x = 3.0; - global_plan.poses[1].pose.position.y = 3.0; - global_plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - global_plan.poses[2].header.frame_id = "test_global_frame"; - global_plan.poses[2].pose.position.x = 5.0; - global_plan.poses[2].pose.position.y = 5.0; - global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - global_plan.poses[3].header.frame_id = "test_global_frame"; - global_plan.poses[3].pose.position.x = 10.0; - global_plan.poses[3].pose.position.y = 10.0; - global_plan.poses[3].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - global_plan.poses[4].header.frame_id = "test_global_frame"; - global_plan.poses[4].pose.position.x = 20.0; - global_plan.poses[4].pose.position.y = 20.0; - global_plan.poses[4].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - global_plan.poses[5].pose.position.x = 500.0; - global_plan.poses[5].pose.position.y = 500.0; - global_plan.poses[5].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(global_plan); -} - -TEST(GracefulControllerTest, pruningPlanOutsideCostmap) { - auto node = std::make_shared("testGraceful"); - auto tf = std::make_shared(node->get_clock()); - - // Create a costmap of 10x10 meters - auto costmap_ros = std::make_shared("test_costmap"); - auto results = costmap_ros->set_parameters( - {rclcpp::Parameter("global_frame", "test_global_frame"), - rclcpp::Parameter("robot_base_frame", "test_robot_frame"), - rclcpp::Parameter("width", 10), - rclcpp::Parameter("height", 10), - rclcpp::Parameter("resolution", 0.1)}); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } - costmap_ros->on_configure(rclcpp_lifecycle::State()); - - // Create controller - auto controller = std::make_shared(); - controller->configure(node, "test", tf, costmap_ros); - controller->activate(); - - // Create the robot pose - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = "test_robot_frame"; - robot_pose.pose.position.x = 0.0; - robot_pose.pose.position.y = 0.0; - robot_pose.pose.position.z = 0.0; - - // Set transform between global and robot frame - geometry_msgs::msg::TransformStamped global_to_robot; - global_to_robot.header.frame_id = "test_global_frame"; - global_to_robot.header.stamp = node->get_clock()->now(); - global_to_robot.child_frame_id = "test_robot_frame"; - global_to_robot.transform.translation.x = robot_pose.pose.position.x; - global_to_robot.transform.translation.y = robot_pose.pose.position.y; - global_to_robot.transform.translation.z = robot_pose.pose.position.z; - tf->setTransform(global_to_robot, "test", false); - - // Set a linear global plan - nav_msgs::msg::Path global_plan; - global_plan.header.frame_id = "test_global_frame"; - global_plan.poses.resize(3); - global_plan.poses[0].header.frame_id = "test_global_frame"; - global_plan.poses[0].pose.position.x = 0.0; - global_plan.poses[0].pose.position.y = 0.0; - global_plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - global_plan.poses[1].header.frame_id = "test_global_frame"; - global_plan.poses[1].pose.position.x = 3.0; - global_plan.poses[1].pose.position.y = 3.0; - global_plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - global_plan.poses[2].header.frame_id = "test_global_frame"; - global_plan.poses[2].pose.position.x = 200.0; - global_plan.poses[2].pose.position.y = 200.0; - global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(global_plan); -} - TEST(GracefulControllerTest, computeVelocityCommandRotate) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -828,9 +522,9 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path transformed_global_plan; + nav_msgs::msg::Path pruned_global_plan; - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan); + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, pruned_global_plan); // Check results: the robot should rotate in place. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. @@ -905,8 +599,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path transformed_global_plan; - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan); + nav_msgs::msg::Path pruned_global_plan; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, pruned_global_plan); // Check results: the robot should go straight to the target. // So, linear velocity should be some positive value and angular velocity should be zero. @@ -986,8 +680,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path transformed_global_plan; - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan); + nav_msgs::msg::Path pruned_global_plan; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, pruned_global_plan); // Check results: the robot should go straight to the target. // So, both linear velocity should be some negative values. @@ -1072,8 +766,8 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path transformed_global_plan; - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan); + nav_msgs::msg::Path pruned_global_plan; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, pruned_global_plan); // Check results: the robot should do a final rotation near the target. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. 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 25c17ffa915..178749ec8c5 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 @@ -205,6 +205,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity pruned_global_plan.poses.end(), std::back_inserter(transformed_plan.poses), transformGlobalPlanToLocal); + transformed_path_pub_->publish(transformed_plan); // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); From 843f02c28c505d33a227d3ab0429a2be3c8896e2 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 21 Sep 2025 17:08:55 +0000 Subject: [PATCH 09/62] MPPI Signed-off-by: mini-1235 --- nav2_core/include/nav2_core/controller.hpp | 3 +- .../src/graceful_controller.cpp | 2 +- .../benchmark/controller_benchmark.cpp | 4 +- .../nav2_mppi_controller/controller.hpp | 4 +- nav2_mppi_controller/src/controller.cpp | 38 +++++++++++++++++-- .../src/regulated_pure_pursuit_controller.cpp | 2 +- 6 files changed, 44 insertions(+), 9 deletions(-) diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 38e1a03fdca..fbf00459ca5 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -108,13 +108,14 @@ class Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Pointer to the current goal checker the task is utilizing + * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @return The best command for the robot to drive */ virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & transformed_global_plan) = 0; + nav_msgs::msg::Path & pruned_global_plan) = 0; /** * @brief Cancel the current control action diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 6ef48477831..be29e044517 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -58,7 +58,7 @@ void GracefulController::configure( } // Publishers - transformed_path_pub_ = node->create_publisher("transformed_pruned_plan"); + transformed_path_pub_ = node->create_publisher("transformed_global_plan"); local_plan_pub_ = node->create_publisher("local_plan"); motion_target_pub_ = node->create_publisher("motion_target"); slowdown_pub_ = node->create_publisher("slowdown"); diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index a5e6386b6e1..843cc217820 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -109,9 +109,9 @@ void prepareAndRunBenchmark( controller->setPlan(path); nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - nav_msgs::msg::Path transformed_global_plan; + nav_msgs::msg::Path pruned_global_plan; for (auto _ : state) { - controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, transformed_global_plan); + controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, pruned_global_plan); } map_odom_broadcaster.wait(); odom_base_link_broadcaster.wait(); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 6d535be032a..a4250fb3007 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -80,12 +80,13 @@ class MPPIController : public nav2_core::Controller * @param robot_pose Robot pose * @param robot_speed Robot speed * @param goal_checker Pointer to the goal checker for awareness if completed task + * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & transformed_global_plan) override; + nav_msgs::msg::Path & pruned_global_plan) override; /** * @brief Set new reference path to track @@ -125,6 +126,7 @@ class MPPIController : public nav2_core::Controller bool visualize_; bool publish_optimal_trajectory_; + double transform_tolerance_; }; } // namespace nav2_mppi_controller diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 149fa8d1758..194bcbeef42 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -34,6 +34,8 @@ void MPPIController::configure( parameters_handler_ = std::make_unique(parent, name_); auto node = parent_.lock(); + auto getParentParam = parameters_handler_->getParamGetter(""); + getParentParam(transform_tolerance_, "transform_tolerance", 0.1, ParameterType::Static); // Get high-level controller parameters auto getParam = parameters_handler_->getParamGetter(name_); getParam(visualize_, "visualize", false); @@ -92,13 +94,43 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & transformed_global_plan) + nav_msgs::msg::Path & pruned_global_plan) { #ifdef BENCHMARK_TESTING auto start = std::chrono::system_clock::now(); #endif std::lock_guard param_lock(*parameters_handler_->getLock()); + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + unsigned int mx, my; + // Find the furthest relevant pose on the path to consider within costmap + // bounds + // Transforming it to the costmap frame in the same loop + for (auto global_plan_pose = pruned_global_plan.poses.begin(); global_plan_pose != pruned_global_plan.poses.end(); + ++global_plan_pose) + { + // Transform from global plan frame to costmap frame + geometry_msgs::msg::PoseStamped costmap_plan_pose; + global_plan_pose->header.stamp = robot_pose.header.stamp; + global_plan_pose->header.frame_id = pruned_global_plan.header.frame_id; + nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_, + costmap_ros_->getGlobalFrameID(), transform_tolerance_); + + // Check if pose is inside the costmap + if (!costmap_ros_->getCostmap()->worldToMap( + costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) + { + break; + } + + // Filling the transformed plan to return with the transformed pose + transformed_plan.poses.push_back(costmap_plan_pose); + } + //TODO: add goal here geometry_msgs::msg::Pose goal; @@ -106,7 +138,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_global_plan, goal, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); @@ -128,7 +160,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( } if (visualize_) { - visualize(std::move(transformed_global_plan), cmd.header.stamp, optimal_trajectory); + visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); } return cmd; 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 178749ec8c5..5e7c0f11c81 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 @@ -69,7 +69,7 @@ void RegulatedPurePursuitController::configure( node->get_parameter("controller_frequency", control_frequency); control_duration_ = 1.0 / control_frequency; - transformed_path_pub_ = node->create_publisher("transformed_pruned_plan"); + transformed_path_pub_ = node->create_publisher("transformed_global_plan"); carrot_pub_ = node->create_publisher("lookahead_point"); curvature_carrot_pub_ = node->create_publisher( "curvature_lookahead_point"); From 375376978188ae763419e86fc9ba062c18aa6ea3 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 21 Sep 2025 17:27:44 +0000 Subject: [PATCH 10/62] Remove goal from MPPI Signed-off-by: mini-1235 --- .../benchmark/optimizer_benchmark.cpp | 2 +- .../nav2_mppi_controller/critic_data.hpp | 3 +-- .../optimal_trajectory_validator.hpp | 4 +--- .../nav2_mppi_controller/optimizer.hpp | 8 +++----- nav2_mppi_controller/src/controller.cpp | 5 +---- nav2_mppi_controller/src/optimizer.cpp | 7 ++----- .../test/critic_manager_test.cpp | 3 +-- nav2_mppi_controller/test/critics_tests.cpp | 19 +++++++++---------- .../test/optimizer_smoke_test.cpp | 3 +-- .../test/optimizer_unit_tests.cpp | 5 ++--- nav2_mppi_controller/test/utils_test.cpp | 12 +++++------- 11 files changed, 27 insertions(+), 44 deletions(-) diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index afe8e83eea9..173a141e745 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -92,7 +92,7 @@ void prepareAndRunBenchmark( nav2_core::GoalChecker * dummy_goal_checker{nullptr}; for (auto _ : state) { - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, path.poses.back().pose, + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, dummy_goal_checker); } } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index 8476dde055c..38702da7ee1 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -34,14 +34,13 @@ namespace mppi /** * @struct mppi::CriticData * @brief Data to pass to critics for scoring, including state, trajectories, - * pruned path, global goal, costs, and important parameters to share + * pruned path, costs, and important parameters to share */ struct CriticData { const models::State & state; const models::Trajectories & trajectories; const models::Path & path; - const geometry_msgs::msg::Pose & goal; Eigen::ArrayXf & costs; float & model_dt; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp index b3f7b03faab..023de224729 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp @@ -111,7 +111,6 @@ class OptimalTrajectoryValidator * @param robot_pose The current pose of the robot * @param robot_speed The current speed of the robot * @param plan The planned path for the robot - * @param goal The goal pose for the robot * @return True if the trajectory is valid, false otherwise */ virtual ValidationResult validateTrajectory( @@ -119,8 +118,7 @@ class OptimalTrajectoryValidator const models::ControlSequence & /*control_sequence*/, const geometry_msgs::msg::PoseStamped & /*robot_pose*/, const geometry_msgs::msg::Twist & /*robot_speed*/, - const nav_msgs::msg::Path & /*plan*/, - const geometry_msgs::msg::Pose & /*goal*/) + const nav_msgs::msg::Path & /*plan*/) { // The Optimizer automatically ensures that we are within Kinematic // and dynamic constraints, no need to check for those again. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 1914abde647..7d8dd64c147 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -91,14 +91,13 @@ class Optimizer * @param robot_pose Pose of the robot at given time * @param robot_speed Speed of the robot at given time * @param plan Path plan to track - * @param goal Given Goal pose to reach. * @param goal_checker Object to check if goal is completed * @return Tuple of [TwistStamped command, optimal trajectory] */ std::tuple evalControl( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); + nav2_core::GoalChecker * goal_checker); /** * @brief Get the trajectories generated in a cycle for visualization @@ -157,7 +156,7 @@ class Optimizer const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); + nav2_core::GoalChecker * goal_checker); /** * @brief Obtain the main controller's parameters @@ -279,11 +278,10 @@ class Optimizer std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; - geometry_msgs::msg::Pose goal_; Eigen::ArrayXf costs_; CriticData critics_data_ = { - state_, generated_trajectories_, path_, goal_, + state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 194bcbeef42..8d2c93cc506 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -131,14 +131,11 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( transformed_plan.poses.push_back(costmap_plan_pose); } - //TODO: add goal here - geometry_msgs::msg::Pose goal; - nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 013b29de502..80b1e89546a 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -183,10 +183,9 @@ std::tuple Optimizer::evalCon const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal, goal_checker); + prepare(robot_pose, robot_speed, plan, goal_checker); Eigen::ArrayXXf optimal_trajectory; bool trajectory_valid = true; @@ -194,7 +193,7 @@ std::tuple Optimizer::evalCon optimize(); optimal_trajectory = getOptimizedTrajectory(); switch (trajectory_validator_->validateTrajectory( - optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal)) + optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan)) { case mppi::ValidationResult::SOFT_RESET: trajectory_valid = false; @@ -252,7 +251,6 @@ void Optimizer::prepare( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { state_.pose = robot_pose; @@ -260,7 +258,6 @@ void Optimizer::prepare( state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan); path_ = utils::toTensor(plan); costs_.setZero(); - goal_ = goal; critics_data_.fail_flag = false; critics_data_.goal_checker = goal_checker; diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index ff0116ed7de..b89029f4602 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -110,11 +110,10 @@ TEST(CriticManagerTests, BasicCriticOperations) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; - geometry_msgs::msg::Pose goal; Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.fail_flag = true; diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 88e823019a7..370fe9dcfcd 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -75,11 +75,10 @@ TEST(CriticTests, ConstraintsCritic) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; - geometry_msgs::msg::Pose goal; Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); @@ -227,7 +226,7 @@ TEST(CriticTests, GoalAngleCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); @@ -290,7 +289,7 @@ TEST(CriticTests, GoalCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); @@ -346,7 +345,7 @@ TEST(CriticTests, PathAngleCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -467,7 +466,7 @@ TEST(CriticTests, PreferForwardCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -526,7 +525,7 @@ TEST(CriticTests, TwirlingCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -593,7 +592,7 @@ TEST(CriticTests, PathFollowCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -647,7 +646,7 @@ TEST(CriticTests, PathAlignCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -764,7 +763,7 @@ TEST(CriticTests, VelocityDeadbandCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index c88422f3a84..a417ec7882a 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -77,10 +77,9 @@ TEST_P(OptimizerSuite, OptimizerTest) { auto pose = getDummyPointStamped(node, start_pose); auto velocity = getDummyTwist(); auto path = getIncrementalDummyPath(node, path_settings); - auto goal = path.poses.back().pose; nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal, + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, dummy_goal_checker); EXPECT_GT(trajectory.rows(), 0); EXPECT_GT(trajectory.cols(), 0); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index eec9ea0ec57..8219c76ceae 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -116,10 +116,9 @@ class OptimizerTester : public Optimizer const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal, goal_checker); + prepare(robot_pose, robot_speed, plan, goal_checker); EXPECT_EQ(critics_data_.goal_checker, nullptr); EXPECT_NEAR(costs_.sum(), 0, 1e-6); // should be reset @@ -393,7 +392,7 @@ TEST(OptimizerTests, PrepareTests) geometry_msgs::msg::Pose goal; path.poses.resize(17); - optimizer_tester.testPrepare(pose, speed, path, goal, nullptr); + optimizer_tester.testPrepare(pose, speed, path, nullptr); } TEST(OptimizerTests, shiftControlSequenceTests) diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 630695ebe99..3d854adfc35 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -176,13 +176,12 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) models::Trajectories generated_trajectories; generated_trajectories.reset(100, 2); models::Path path; - geometry_msgs::msg::Pose goal; path.reset(10); Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Attempt to set furthest point if notionally set, should not change @@ -192,7 +191,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) // Attempt to set if not set already with no other information, should fail CriticData data2 = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references setPathFurthestPointIfNotSet(data2); EXPECT_EQ(data2.furthest_reached_path_point, 0); @@ -211,7 +210,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) path = toTensor(plan); CriticData data3 = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5); } @@ -221,13 +220,12 @@ TEST(UtilsTests, findPathCosts) models::State state; models::Trajectories generated_trajectories; models::Path path; - geometry_msgs::msg::Pose goal; path.reset(50); Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Test not set if already set, should not change @@ -240,7 +238,7 @@ TEST(UtilsTests, findPathCosts) EXPECT_EQ(data.path_pts_valid->size(), 10u); CriticData data3 = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references auto costmap_ros = std::make_shared( From 85889359e2acc18d351fe9f5279ddcff60483bee Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Mon, 22 Sep 2025 13:49:07 +0000 Subject: [PATCH 11/62] Update setPlan api Signed-off-by: mini-1235 --- nav2_controller/src/controller_server.cpp | 2 +- nav2_core/include/nav2_core/controller.hpp | 4 +-- .../include/dwb_core/dwb_local_planner.hpp | 6 ++--- .../dwb_core/src/dwb_local_planner.cpp | 6 ++--- .../graceful_controller.hpp | 6 ++--- .../src/graceful_controller.cpp | 2 +- .../test/test_graceful_controller.cpp | 8 +++--- .../benchmark/controller_benchmark.cpp | 2 +- .../nav2_mppi_controller/controller.hpp | 6 ++--- nav2_mppi_controller/src/controller.cpp | 2 +- .../test/controller_state_transition_test.cpp | 2 +- .../regulated_pure_pursuit_controller.hpp | 6 ++--- .../src/regulated_pure_pursuit_controller.cpp | 2 +- .../nav2_rotation_shim_controller.hpp | 6 ++--- .../src/nav2_rotation_shim_controller.cpp | 8 +++--- .../test/test_shim_controller.cpp | 26 +++++++++---------- .../controller/controller_error_plugins.hpp | 2 +- 17 files changed, 48 insertions(+), 48 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 5b7d5f2e2b4..27ff633fe27 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -537,7 +537,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) if (path.poses.empty()) { throw nav2_core::InvalidPath("Path is empty."); } - controllers_[current_controller_]->setPlan(path); + controllers_[current_controller_]->newPathReceived(path); path_handler_->setPlan(path); current_path_ = path; diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index fbf00459ca5..759fad89841 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -92,10 +92,10 @@ class Controller virtual void deactivate() = 0; /** - * @brief local setPlan - Sets the global plan + * @brief local setPlan - Notifies the Controller that a new plan is received from the Planner Server * @param path The global plan */ - virtual void setPlan(const nav_msgs::msg::Path & path) = 0; + virtual void newPathReceived(const nav_msgs::msg::Path & raw_global_path) = 0; /** * @brief Controller computeVelocityCommands - calculates the best command given the current pose and velocity diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 3543c1131f1..755ab6c1b6d 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -89,10 +89,10 @@ class DWBLocalPlanner : public nav2_core::Controller void cleanup() override; /** - * @brief nav2_core setPlan - Sets the global plan - * @param path The global plan + * @brief nav2_core newPathReceived - Receives a new plan from the Planner Server + * @param raw_global_path The global plan from the Planner Server */ - void setPlan(const nav_msgs::msg::Path & path) override; + void newPathReceived(const nav_msgs::msg::Path & raw_global_path) override; /** * @brief nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 2ec86b73a13..bf5d84080e9 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -228,7 +228,7 @@ DWBLocalPlanner::loadCritics() } void -DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) +DWBLocalPlanner::newPathReceived(const nav_msgs::msg::Path & raw_global_path) { for (TrajectoryCritic::Ptr & critic : critics_) { critic->reset(); @@ -236,8 +236,8 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) traj_generator_->reset(); - pub_->publishGlobalPlan(path); - global_plan_ = path; + pub_->publishGlobalPlan(raw_global_path); + global_plan_ = raw_global_path; } geometry_msgs::msg::TwistStamped diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 8814a2c184f..5f3379cce64 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -93,10 +93,10 @@ class GracefulController : public nav2_core::Controller nav_msgs::msg::Path & pruned_global_plan) override; /** - * @brief nav2_core setPlan - Sets the global plan. - * @param path The global plan + * @brief nav2_core newPathReceived - Receives a new plan from the Planner Server + * @param raw_global_path The global plan from the Planner Server */ - void setPlan(const nav_msgs::msg::Path & path) override; + void newPathReceived(const nav_msgs::msg::Path & raw_global_path) override; /** * @brief Limits the maximum linear speed of the robot. diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index be29e044517..4420ffd7b04 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -264,7 +264,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( throw nav2_core::NoValidControl("Collision detected in trajectory"); } -void GracefulController::setPlan(const nav_msgs::msg::Path & /*path*/) +void GracefulController::newPathReceived(const nav_msgs::msg::Path & /*raw_global_path*/) { goal_reached_ = false; do_initial_rotation_ = true; diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index e8c899fe2dd..856c6e85084 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -512,7 +512,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { plan.poses[2].pose.position.x = 1.0; plan.poses[2].pose.position.y = 1.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(plan); + controller->newPathReceived(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -589,7 +589,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { plan.poses[2].pose.position.x = 2.0; plan.poses[2].pose.position.y = 0.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(plan); + controller->newPathReceived(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -670,7 +670,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { plan.poses[2].pose.position.x = -2.0; plan.poses[2].pose.position.y = 0.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(plan); + controller->newPathReceived(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -755,7 +755,7 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { plan.poses[4].pose.position.x = 0.2; plan.poses[4].pose.position.y = 0.0; plan.poses[4].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(plan); + controller->newPathReceived(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index 843cc217820..75e57edf230 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -106,7 +106,7 @@ void prepareAndRunBenchmark( auto velocity = getDummyTwist(); auto path = getIncrementalDummyPath(node, path_settings); - controller->setPlan(path); + controller->newPathReceived(path); nav2_core::GoalChecker * dummy_goal_checker{nullptr}; nav_msgs::msg::Path pruned_global_plan; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index a4250fb3007..2978521cc6e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -89,10 +89,10 @@ class MPPIController : public nav2_core::Controller nav_msgs::msg::Path & pruned_global_plan) override; /** - * @brief Set new reference path to track - * @param path Path to track + * @brief Receives a new plan from the Planner Server + * @param raw_global_path The global plan from the Planner Server */ - void setPlan(const nav_msgs::msg::Path & path) override; + void newPathReceived(const nav_msgs::msg::Path & raw_global_path) override; /** * @brief Set new speed limit from callback diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 8d2c93cc506..86098b6682f 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -173,7 +173,7 @@ void MPPIController::visualize( trajectory_visualizer_.visualize(std::move(transformed_plan)); } -void MPPIController::setPlan(const nav_msgs::msg::Path & /*path*/) +void MPPIController::newPathReceived(const nav_msgs::msg::Path & /*raw_global_path*/) { } diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index ae76721a305..d748550a963 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -56,7 +56,7 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) path.header.frame_id = costmap_ros->getGlobalFrameID(); pose.header.frame_id = costmap_ros->getGlobalFrameID(); - controller->setPlan(path); + controller->newPathReceived(path); nav_msgs::msg::Path transformed_plan; EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}, transformed_plan)); diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index e0653e1dcf3..e1b612794dd 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -101,10 +101,10 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool cancel() override; /** - * @brief nav2_core setPlan - Sets the global plan - * @param path The global plan + * @brief nav2_core newPathReceived - Receives a new plan from the Planner Server + * @param raw_global_path The global plan from the Planner Server */ - void setPlan(const nav_msgs::msg::Path & path) override; + void newPathReceived(const nav_msgs::msg::Path & raw_global_path) override; /** * @brief Limits the maximum linear speed of the robot. 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 5e7c0f11c81..699db509b51 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 @@ -397,7 +397,7 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = sign * linear_vel; } -void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & /*path*/) +void RegulatedPurePursuitController::newPathReceived(const nav_msgs::msg::Path & /*raw_global_path*/) { has_reached_xy_tolerance_ = false; } diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index f0dd25cc813..2aa0a7568da 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -92,10 +92,10 @@ class RotationShimController : public nav2_core::Controller nav_msgs::msg::Path & pruned_global_plan) override; /** - * @brief nav2_core setPlan - Sets the global plan - * @param path The global plan + * @brief nav2_core newPathReceived - Receives a new plan from the Planner Server + * @param raw_global_path The global plan from the Planner Server */ - void setPlan(const nav_msgs::msg::Path & path) override; + void newPathReceived(const nav_msgs::msg::Path & raw_global_path) override; /** * @brief Limits the maximum linear speed of the robot. diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index f9b43831963..750037f6d8b 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -392,11 +392,11 @@ bool RotationShimController::isGoalChanged(const nav_msgs::msg::Path & path) return current_path_.poses.back().pose != path.poses.back().pose; } -void RotationShimController::setPlan(const nav_msgs::msg::Path & path) +void RotationShimController::newPathReceived(const nav_msgs::msg::Path & raw_global_path) { - path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true; - current_path_ = path; - primary_controller_->setPlan(path); + path_updated_ = rotate_to_heading_once_ ? isGoalChanged(raw_global_path) : true; + current_path_ = raw_global_path; + primary_controller_->newPathReceived(raw_global_path); position_goal_checker_->reset(); } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index eb7c7e89c4d..b80b763feee 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -112,7 +112,7 @@ TEST(RotationShimControllerTest, lifecycleTransitions) ctrl->cleanup(); } -TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) +TEST(RotationShimControllerTest, newPathReceivedAndSampledPointsTests) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); @@ -142,7 +142,7 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) path.poses[3].pose.position.x = 10.0; path.poses[3].pose.position.y = 10.0; EXPECT_EQ(controller->isPathUpdated(), false); - controller->setPlan(path); + controller->newPathReceived(path); EXPECT_EQ(controller->getPath().header.frame_id, std::string("hi mate!")); EXPECT_EQ(controller->getPath().poses.size(), 10u); EXPECT_EQ(controller->isPathUpdated(), true); @@ -153,12 +153,12 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) EXPECT_EQ(pose.pose.position.y, 1.0); nav_msgs::msg::Path path_invalid_leng; - controller->setPlan(path_invalid_leng); + controller->newPathReceived(path_invalid_leng); EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error); nav_msgs::msg::Path path_invalid_dists; path.poses.resize(10); - controller->setPlan(path_invalid_dists); + controller->newPathReceived(path_invalid_dists); EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error); } @@ -192,7 +192,7 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) path.poses[2].pose.position.y = 1.0; path.poses[3].pose.position.x = 10.0; path.poses[3].pose.position.y = 10.0; - controller->setPlan(path); + controller->newPathReceived(path); const geometry_msgs::msg::Twist velocity; EXPECT_EQ( @@ -272,7 +272,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // Set with a path -- should attempt to find a sampled point but throw exception // because it cannot be found, then go to RPP and throw exception because it cannot be transformed - controller->setPlan(path); + controller->newPathReceived(path); EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), std::runtime_error); path.header.frame_id = "base_link"; @@ -287,7 +287,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // this should allow it to find the sampled point, then transform to base_link // validly because we setup the TF for it. The -1.0 should be selected since default min // is 0.5 and that should cause a rotation in place - controller->setPlan(path); + controller->newPathReceived(path); tf_broadcaster->sendTransform(transform); auto effort = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); @@ -305,7 +305,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // validly because we setup the TF for it. The 1.0 should be selected since default min // is 0.5 and that should cause a pass off to the RPP controller which will throw // and exception because it is off of the costmap - controller->setPlan(path); + controller->newPathReceived(path); tf_broadcaster->sendTransform(transform); EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), std::runtime_error); } @@ -378,7 +378,7 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { path.poses[3].header.frame_id = "base_link"; // Calculate first velocity command - controller->setPlan(path); + controller->newPathReceived(path); nav_msgs::msg::Path pruned_global_plan; auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); @@ -450,7 +450,7 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[3].pose.orientation.w = 0.9238795; path.poses[3].header.frame_id = "base_link"; - controller->setPlan(path); + controller->newPathReceived(path); nav_msgs::msg::Path pruned_global_plan; auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); @@ -458,7 +458,7 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { // goal heading 45 degrees to the right path.poses[3].pose.orientation.z = 0.3826834; path.poses[3].pose.orientation.w = 0.9238795; - controller->setPlan(path); + controller->newPathReceived(path); cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } @@ -531,7 +531,7 @@ TEST(RotationShimControllerTest, accelerationTests) { path.poses[3].header.frame_id = "base_link"; // Test acceleration limits - controller->setPlan(path); + controller->newPathReceived(path); nav_msgs::msg::Path pruned_global_plan; auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); @@ -585,7 +585,7 @@ TEST(RotationShimControllerTest, isGoalChangedTest) EXPECT_EQ(controller->isGoalChangedWrapper(path), true); // Test: Last pose of the current path is the same, should return false - controller->setPlan(path); + controller->newPathReceived(path); EXPECT_EQ(controller->isGoalChangedWrapper(path), false); // Test: Last pose of the current path differs, should return true diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp index 909f5e6cc1d..e699d5f7283 100644 --- a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -41,7 +41,7 @@ class UnknownErrorController : public nav2_core::Controller void deactivate() {} - void setPlan(const nav_msgs::msg::Path &) {} + void newPathReceived(const nav_msgs::msg::Path &) {} virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &, From 3d4e366a3c27b81ae34d2a6f81ed02f3cbb8ea6b Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Mon, 22 Sep 2025 14:16:54 +0000 Subject: [PATCH 12/62] Fix naming and doxygen Signed-off-by: mini-1235 --- .../nav2_controller/controller_server.hpp | 3 +-- .../include/nav2_controller/path_handler.hpp | 18 +++++++++++------- .../plugins/position_goal_checker.hpp | 18 +++++++++--------- .../plugins/simple_goal_checker.hpp | 2 -- nav2_controller/src/controller_server.cpp | 18 ++++++------------ nav2_controller/src/path_handler.cpp | 15 ++++++++------- 6 files changed, 35 insertions(+), 39 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index be8a229e252..7caad0e7ac5 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -257,11 +257,10 @@ class ControllerServer : public nav2::LifecycleNode // Current path container nav_msgs::msg::Path current_path_; - nav_msgs::msg::Path local_path_; + nav_msgs::msg::Path pruned_global_plan_; std::unique_ptr path_handler_; std::unique_ptr param_handler_; Parameters * params_; - nav2::Publisher::SharedPtr global_path_pub_; private: /** diff --git a/nav2_controller/include/nav2_controller/path_handler.hpp b/nav2_controller/include/nav2_controller/path_handler.hpp index a5689293a74..e6545a59c5a 100644 --- a/nav2_controller/include/nav2_controller/path_handler.hpp +++ b/nav2_controller/include/nav2_controller/path_handler.hpp @@ -36,7 +36,7 @@ using PathIterator = std::vector::iterator; /** * @class nav2_controller::PathHandler - * @brief Handles input paths to transform them to local frames required + * @brief Handles input paths to prune them within the local costmap */ class PathHandler { @@ -55,19 +55,23 @@ class PathHandler ~PathHandler() = default; /** - * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint - * Points ineligible to be selected as a lookahead point if they are any of the following: - * - Outside the local_costmap (collision avoidance cannot be assured) + * @brief Prunes global plan, bounded around the robot's position and within the local costmap * @param pose pose to transform - * @param max_robot_pose_search_dist Distance to search for matching nearest path point - * @param reject_unit_path If true, fail if path has only one pose - * @return Path in new frame + * @return Path after pruned */ nav_msgs::msg::Path pruneGlobalPlan( const geometry_msgs::msg::PoseStamped & pose); + /** + * @brief Set new reference plan + * @param Path Path to use + */ void setPlan(const nav_msgs::msg::Path & path); + /** + * @brief Get reference plan + * @return Path + */ nav_msgs::msg::Path getPlan() {return global_plan_;} /** diff --git a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp index 4e069990eeb..39f614a5ac6 100644 --- a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp @@ -27,9 +27,9 @@ namespace nav2_controller { /** - * @class PositionGoalChecker - * @brief Goal Checker plugin that only checks XY position, ignoring orientation - */ + * @class PositionGoalChecker + * @brief Goal Checker plugin that only checks XY position, ignoring orientation + */ class PositionGoalChecker : public nav2_core::GoalChecker { public: @@ -52,9 +52,9 @@ class PositionGoalChecker : public nav2_core::GoalChecker geometry_msgs::msg::Twist & vel_tolerance) override; /** - * @brief Set the XY goal tolerance - * @param tolerance New tolerance value - */ + * @brief Set the XY goal tolerance + * @param tolerance New tolerance value + */ void setXYGoalTolerance(double tolerance); protected: @@ -66,9 +66,9 @@ class PositionGoalChecker : public nav2_core::GoalChecker rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; /** - * @brief Callback executed when a parameter change is detected - * @param parameters list of changed parameters - */ + * @brief Callback executed when a parameter change is detected + * @param parameters list of changed parameters + */ rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); }; diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index a2792acf256..7fa555e19ea 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -64,11 +64,9 @@ class SimpleGoalChecker : public nav2_core::GoalChecker const std::string & plugin_name, const std::shared_ptr costmap_ros) override; void reset() override; - bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; - bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, geometry_msgs::msg::Twist & vel_tolerance) override; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 27ff633fe27..6f1c8f80209 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -160,7 +160,6 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) odom_sub_ = std::make_unique(node, params_->odom_duration, params_->odom_topic); vel_publisher_ = std::make_unique(node, "cmd_vel"); - global_path_pub_ = node->create_publisher("received_global_plan"); costmap_update_timeout_ = rclcpp::Duration::from_seconds(params_->costmap_update_timeout); @@ -201,7 +200,6 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) it->second->activate(); } vel_publisher_->on_activate(); - global_path_pub_->on_activate(); action_server_->activate(); auto node = shared_from_this(); @@ -234,7 +232,6 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) publishZeroVelocity(); vel_publisher_->on_deactivate(); - global_path_pub_->on_deactivate(); remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_.reset(); @@ -268,7 +265,6 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) odom_sub_.reset(); costmap_thread_.reset(); vel_publisher_.reset(); - global_path_pub_.reset(); speed_limit_sub_.reset(); return nav2::CallbackReturn::SUCCESS; @@ -557,12 +553,10 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); - auto transformed_plan = path_handler_->pruneGlobalPlan(pose); - // RCLCPP_INFO(get_logger(), "compute remaining distance %lf ",nav2_util::geometry_utils::calculate_path_length(transformed_plan)); - global_path_pub_->publish(transformed_plan); - local_path_ = transformed_plan; - end_pose_ = local_path_.poses.back(); - end_pose_.header.frame_id = local_path_.header.frame_id; + pruned_global_plan_ = path_handler_->pruneGlobalPlan(pose); + // RCLCPP_INFO(get_logger(), "compute remaining distance %lf ",nav2_util::geometry_utils::calculate_path_length(pruned_global_plan_)); + end_pose_ = pruned_global_plan_.poses.back(); + end_pose_.header.frame_id = pruned_global_plan_.header.frame_id; goal_checkers_[current_goal_checker_]->reset(); geometry_msgs::msg::TwistStamped cmd_vel_2d; @@ -573,7 +567,7 @@ void ControllerServer::computeAndPublishVelocity() pose, twist, goal_checkers_[current_goal_checker_].get(), - transformed_plan + pruned_global_plan_ ); last_valid_cmd_time_ = now(); cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); @@ -743,7 +737,7 @@ bool ControllerServer::isGoalReached() return goal_checkers_[current_goal_checker_]->isGoalReached( pose.pose, transformed_end_pose.pose, - velocity, local_path_); + velocity, pruned_global_plan_); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) diff --git a/nav2_controller/src/path_handler.cpp b/nav2_controller/src/path_handler.cpp index b2afcc2d287..dff8f5fe15f 100644 --- a/nav2_controller/src/path_handler.cpp +++ b/nav2_controller/src/path_handler.cpp @@ -103,6 +103,7 @@ nav_msgs::msg::Path PathHandler::pruneGlobalPlan( throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); } + // Limit the search for the closest pose up to max_robot_pose_search_dist on the path auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), @@ -111,7 +112,7 @@ nav_msgs::msg::Path PathHandler::pruneGlobalPlan( // First find the closest pose on the path to the robot // bounded by when the path turns around (if it does) so we don't get a pose from a later // portion of the path - auto transformation_begin = + auto closest_point = nav2_util::geometry_utils::min_by( global_plan_up_to_inversion_.poses.begin(), closest_pose_upper_bound, [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { @@ -123,27 +124,27 @@ nav_msgs::msg::Path PathHandler::pruneGlobalPlan( // end of path direction if (global_plan_up_to_inversion_.poses.begin() != closest_pose_upper_bound && global_plan_up_to_inversion_.poses.size() > 1 && - transformation_begin == std::prev(closest_pose_upper_bound)) + closest_point == std::prev(closest_pose_upper_bound)) { - transformation_begin = std::prev(std::prev(closest_pose_upper_bound)); + closest_point = std::prev(std::prev(closest_pose_upper_bound)); } // We'll discard points on the plan that are outside the local costmap const double max_costmap_extent = getCostmapMaxExtent(); - auto transformation_end = std::find_if( - transformation_begin, global_plan_up_to_inversion_.poses.end(), + auto pruned_plan_end = std::find_if( + closest_point, global_plan_up_to_inversion_.poses.end(), [&](const auto & global_plan_pose) { return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; }); nav_msgs::msg::Path pruned_plan; pruned_plan.poses.insert(pruned_plan.poses.end(), - transformation_begin, transformation_end); + closest_point, pruned_plan_end); pruned_plan.header = global_plan_.header; // Remove the portion of the global plan that we've already passed so we don't // process it on the next iteration (this is called path pruning) - prunePlan(global_plan_up_to_inversion_, transformation_begin); + prunePlan(global_plan_up_to_inversion_, closest_point); if (params_->enforce_path_inversion && inversion_locale_ != 0u) { if (isWithinInversionTolerances(robot_pose)) { From 44227f758650bbad8268205b948422c92145b2bd Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Mon, 22 Sep 2025 14:36:25 +0000 Subject: [PATCH 13/62] Remove path complete goal checker Signed-off-by: mini-1235 --- nav2_controller/CMakeLists.txt | 28 +- .../nav2_controller/parameter_handler.hpp | 2 +- .../plugins/path_complete_goal_checker.hpp | 82 -- nav2_controller/plugins.xml | 5 - .../plugins/path_complete_goal_checker.cpp | 110 --- nav2_controller/plugins/test/CMakeLists.txt | 2 +- nav2_controller/plugins/test/goal_checker.cpp | 793 ++++++++---------- .../README.md | 2 +- nav2_rotation_shim_controller/README.md | 2 +- 9 files changed, 365 insertions(+), 661 deletions(-) delete mode 100644 nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp delete mode 100644 nav2_controller/plugins/path_complete_goal_checker.cpp diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 51a2c4fb3ff..dfe875a0f71 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -170,26 +170,6 @@ target_link_libraries(position_goal_checker PRIVATE pluginlib::pluginlib ) -add_library(path_complete_goal_checker SHARED plugins/path_complete_goal_checker.cpp) -target_include_directories(path_complete_goal_checker - PUBLIC - "$" - "$" -) -target_link_libraries(path_complete_goal_checker PUBLIC - ${geometry_msgs_TARGETS} - nav2_core::nav2_core - nav2_costmap_2d::nav2_costmap_2d_core - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle - ${rcl_interfaces_TARGETS} - simple_goal_checker -) -target_link_libraries(path_complete_goal_checker PRIVATE - nav2_util::nav2_util_core - pluginlib::pluginlib -) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -212,7 +192,6 @@ install(TARGETS pose_progress_checker simple_goal_checker stopped_goal_checker - path_complete_goal_checker ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib @@ -229,13 +208,14 @@ install(DIRECTORY include/ ) ament_export_include_directories(include/${PROJECT_NAME}) -ament_export_libraries(simple_progress_checker +ament_export_libraries( + simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker position_goal_checker - path_complete_goal_checker - ${library_name}) + ${library_name} +) ament_export_dependencies( geometry_msgs nav2_core diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index 368805eb7f8..d45193876d5 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -101,7 +101,7 @@ class ParameterHandler const std::vector default_progress_checker_ids_{"progress_checker"}; const std::vector default_progress_checker_types_{"nav2_controller::SimpleProgressChecker"}; const std::vector default_goal_checker_ids_{"goal_checker"}; - const std::vector default_goal_checker_types_{"nav2_controller::PathCompleteGoalChecker"}; + const std::vector default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}; const std::vector default_controller_ids_{"FollowPath"}; const std::vector default_controller_types_{"dwb_core::DWBLocalPlanner"}; }; diff --git a/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp deleted file mode 100644 index 7fdb1328084..00000000000 --- a/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, Joseph Duchesne - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_ -#define NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_ - -#include -#include -#include - -#include "nav2_controller/plugins/simple_goal_checker.hpp" - -namespace nav2_controller -{ - -/** - * @class PathCompleteGoalChecker - * @brief Goal Checker plugin that checks position delta, once path is shorter than a threshold. - */ -class PathCompleteGoalChecker : public SimpleGoalChecker -{ -public: - PathCompleteGoalChecker(); - - // Standard GoalChecker Interface - void initialize( - const nav2::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name, - const std::shared_ptr costmap_ros) override; - - void reset() override; - - bool isGoalReached( - const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; - -protected: - // minimum remaining path length before checking position goals - double path_length_tolerance_; - - /** - * @brief Callback executed when a paramter change is detected - * @param parameters list of changed parameters - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); -}; - -} // namespace nav2_controller - -#endif // NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index 8ccf01fc51f..e3e9c7cf63b 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -24,9 +24,4 @@ Goal checker that only checks XY position and ignores orientation - - - Checks if path is below a threshold in length, and then if the current pose is within goal window for x, y and yaw - - diff --git a/nav2_controller/plugins/path_complete_goal_checker.cpp b/nav2_controller/plugins/path_complete_goal_checker.cpp deleted file mode 100644 index 888e889678d..00000000000 --- a/nav2_controller/plugins/path_complete_goal_checker.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, Joseph Duchesne - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "nav2_controller/plugins/path_complete_goal_checker.hpp" -#include "pluginlib/class_list_macros.hpp" -#include "nav2_ros_common/node_utils.hpp" -#include "nav2_util/geometry_utils.hpp" - -using rcl_interfaces::msg::ParameterType; -using std::placeholders::_1; - -namespace nav2_controller -{ - -PathCompleteGoalChecker::PathCompleteGoalChecker() -: SimpleGoalChecker(), path_length_tolerance_(1.0) -{ -} - -void PathCompleteGoalChecker::initialize( - const nav2::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name, - const std::shared_ptr costmap_ros) -{ - SimpleGoalChecker::initialize(parent, plugin_name, costmap_ros); - - auto node = parent.lock(); - - nav2::declare_parameter_if_not_declared( - node, - plugin_name + ".path_length_tolerance", - rclcpp::ParameterValue(path_length_tolerance_)); - - node->get_parameter(plugin_name + ".path_length_tolerance", path_length_tolerance_); - - // Replace SimpleGoalChecker's callback for dynamic parameters - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&PathCompleteGoalChecker::dynamicParametersCallback, this, _1)); -} - -void PathCompleteGoalChecker::reset() -{ - SimpleGoalChecker::reset(); -} - -bool PathCompleteGoalChecker::isGoalReached( - const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & twist, const nav_msgs::msg::Path & path) -{ - std::cout << "length" << nav2_util::geometry_utils::calculate_path_length(path) << std::endl; - if(nav2_util::geometry_utils::calculate_path_length(path) > path_length_tolerance_){ - return false; - } - // otherwise defer to SimpleGoalChecker's isGoalReached - return SimpleGoalChecker::isGoalReached(query_pose, goal_pose, twist, path); -} - -rcl_interfaces::msg::SetParametersResult -PathCompleteGoalChecker::dynamicParametersCallback(std::vector parameters) -{ - auto result = rcl_interfaces::msg::SetParametersResult(); - - for (auto & parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".path_length_tolerance") { - path_length_tolerance_ = parameter.as_double(); - } - } - } - result.successful = true; - return result; -} - -} // namespace nav2_controller - -PLUGINLIB_EXPORT_CLASS(nav2_controller::PathCompleteGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index 176413b112e..ba0ce9eb2d5 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -2,4 +2,4 @@ ament_add_gtest(pctest progress_checker.cpp) target_link_libraries(pctest simple_progress_checker pose_progress_checker) ament_add_gtest(gctest goal_checker.cpp) -target_link_libraries(gctest simple_goal_checker stopped_goal_checker path_complete_goal_checker) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 1025e4b87a3..b0426f46c4e 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -1,436 +1,357 @@ -// /* -// * Software License Agreement (BSD License) -// * -// * Copyright (c) 2017, Locus Robotics -// * All rights reserved. -// * -// * Redistribution and use in source and binary forms, with or without -// * modification, are permitted provided that the following conditions -// * are met: -// * -// * * Redistributions of source code must retain the above copyright -// * notice, this list of conditions and the following disclaimer. -// * * Redistributions in binary form must reproduce the above -// * copyright notice, this list of conditions and the following -// * disclaimer in the documentation and/or other materials provided -// * with the distribution. -// * * Neither the name of the copyright holder nor the names of its -// * contributors may be used to endorse or promote products derived -// * from this software without specific prior written permission. -// * -// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// * POSSIBILITY OF SUCH DAMAGE. -// */ - -// #include -// #include - -// #include "gtest/gtest.h" -// #include "nav2_controller/plugins/simple_goal_checker.hpp" -// #include "nav2_controller/plugins/stopped_goal_checker.hpp" -// #include "nav2_controller/plugins/path_complete_goal_checker.hpp" -// #include "nav2_util/geometry_utils.hpp" -// #include "nav2_ros_common/lifecycle_node.hpp" -// #include "eigen3/Eigen/Geometry" - -// using nav2_controller::SimpleGoalChecker; -// using nav2_controller::StoppedGoalChecker; -// using nav2_controller::PathCompleteGoalChecker; - -// void checkMacro( -// nav2_core::GoalChecker & gc, -// double x0, double y0, double theta0, -// double x1, double y1, double theta1, -// double xv, double yv, double thetav, -// const nav_msgs::msg::Path & path, -// bool expected_result) -// { -// gc.reset(); - -// geometry_msgs::msg::Pose pose0, pose1; -// pose0.position.x = x0; -// pose0.position.y = y0; -// pose0.position.z = 0.0; -// pose0.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta0); - -// pose1.position.x = x1; -// pose1.position.y = y1; -// pose1.position.z = 0.0; -// pose1.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1); - -// geometry_msgs::msg::Twist v; -// v.linear.x = xv; -// v.linear.y = yv; -// v.angular.z = thetav; - -// if (expected_result) { -// EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v, path)); -// } else { -// EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v, path)); -// } -// } -// class TestLifecycleNode : public nav2::LifecycleNode -// { -// public: -// explicit TestLifecycleNode(const std::string & name) -// : nav2::LifecycleNode(name) -// { -// } - -// nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State &) -// { -// return nav2::CallbackReturn::SUCCESS; -// } - -// nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State &) -// { -// return nav2::CallbackReturn::SUCCESS; -// } - -// nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) -// { -// return nav2::CallbackReturn::SUCCESS; -// } - -// nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) -// { -// return nav2::CallbackReturn::SUCCESS; -// } - -// nav2::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) -// { -// return nav2::CallbackReturn::SUCCESS; -// } - -// nav2::CallbackReturn onError(const rclcpp_lifecycle::State &) -// { -// return nav2::CallbackReturn::SUCCESS; -// } -// }; - -// TEST(SimpleGoalChecker, goal_checker_reset) -// { -// auto x = std::make_shared("goal_checker"); - -// nav2_core::GoalChecker * gc = new SimpleGoalChecker; -// gc->reset(); -// delete gc; -// EXPECT_TRUE(true); -// } - -// TEST(StoppedGoalChecker, stopped_goal_checker_reset) -// { -// auto x = std::make_shared("stopped_goal_checker"); - -// nav2_core::GoalChecker * sgc = new StoppedGoalChecker; -// sgc->reset(); -// delete sgc; -// EXPECT_TRUE(true); -// } - -// TEST(StoppedGoalChecker, path_complete_goal_checker_reset) -// { -// auto x = std::make_shared("path_complete_goal_checker"); - -// nav2_core::GoalChecker * pcgc = new PathCompleteGoalChecker; -// pcgc->reset(); -// delete pcgc; -// EXPECT_TRUE(true); -// } - - -// TEST(SimpleGoalChecker, test_goal_checking) -// { -// auto x = std::make_shared("goal_checker"); - -// SimpleGoalChecker gc; -// auto costmap = std::make_shared("test_costmap"); -// nav_msgs::msg::Path path; -// gc.initialize(x, "nav2_controller", costmap); - -// checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); -// checkMacro(gc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); -// checkMacro(gc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); -// checkMacro(gc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); -// checkMacro(gc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); -// checkMacro(gc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, true); -// checkMacro(gc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, true); -// checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, true); -// checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, true); -// } - -// TEST(StoppedGoalChecker, test_goal_checking) -// { -// auto x = std::make_shared("goal_checker"); - -// StoppedGoalChecker sgc; -// auto costmap = std::make_shared("test_costmap"); -// nav_msgs::msg::Path path; - -// sgc.initialize(x, "nav2_controller", costmap); - -// checkMacro(sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); -// checkMacro(sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); -// checkMacro(sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); -// checkMacro(sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); -// checkMacro(sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); -// checkMacro(sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, false); -// checkMacro(sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, false); -// checkMacro(sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, false); -// checkMacro(sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, false); - -// // todo: add some where path complete goal checker differs -// } - -// TEST(PathCompleteGoalChecker, test_goal_checking) -// { -// auto x = std::make_shared("goal_checker"); - -// PathCompleteGoalChecker pcgc; -// auto costmap = std::make_shared("test_costmap"); -// nav_msgs::msg::Path path; -// pcgc.initialize(x, "nav2_controller", costmap); - -// // Add one pose -// { -// geometry_msgs::msg::PoseStamped pose_stamped_msg; -// pose_stamped_msg.pose.position.x = 0.0; -// path.poses.push_back(pose_stamped_msg); -// } -// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); -// checkMacro(pcgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); -// checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, true); -// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, true); -// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, true); -// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, true); - -// // add a second pose at {2.0,0}, making the total path length 2.0m -// // this should prevent any completions due to path_length_tolerance=1.0 -// { -// geometry_msgs::msg::PoseStamped pose_stamped_msg; -// pose_stamped_msg.pose.position.x = 2.0; -// path.poses.push_back(pose_stamped_msg); -// } - -// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, false); -// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, false); -// checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, false); -// } - - -// TEST(StoppedGoalChecker, get_tol_and_dynamic_params) -// { -// auto x = std::make_shared("goal_checker"); - -// StoppedGoalChecker sgc; -// auto costmap = std::make_shared("test_costmap"); - -// sgc.initialize(x, "test", costmap); -// geometry_msgs::msg::Pose pose_tol; -// geometry_msgs::msg::Twist vel_tol; - -// // Test stopped goal checker's tolerance API -// EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); -// EXPECT_EQ(pose_tol.position.x, 0.25); -// EXPECT_EQ(pose_tol.position.y, 0.25); -// EXPECT_NEAR(p2d.theta, 0.25, 1e-6); - -// // Test Stopped goal checker's dynamic parameters -// auto rec_param = std::make_shared( -// x->get_node_base_interface(), x->get_node_topics_interface(), -// x->get_node_graph_interface(), -// x->get_node_services_interface()); - -// auto results = rec_param->set_parameters_atomically( -// {rclcpp::Parameter("test.rot_stopped_velocity", 100.0), -// rclcpp::Parameter("test.trans_stopped_velocity", 100.0)}); - -// rclcpp::spin_until_future_complete( -// x->get_node_base_interface(), -// results); - -// EXPECT_EQ(x->get_parameter("test.rot_stopped_velocity").as_double(), 100.0); -// EXPECT_EQ(x->get_parameter("test.trans_stopped_velocity").as_double(), 100.0); - - -// // Test the dynamic parameters impacted the tolerances -// EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); -// EXPECT_EQ(vel_tol.linear.x, 100.0); -// EXPECT_EQ(vel_tol.linear.y, 100.0); -// EXPECT_EQ(vel_tol.angular.z, 100.0); -// } - - -// TEST(SimpleGoalChecker, get_tol_and_dynamic_params) -// { -// auto x = std::make_shared("goal_checker"); - -// SimpleGoalChecker gc; -// auto costmap = std::make_shared("test_costmap"); - -// gc.initialize(x, "test2", costmap); -// geometry_msgs::msg::Pose pose_tol; -// geometry_msgs::msg::Twist vel_tol; - -// // Test stopped goal checker's tolerance API -// EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); -// EXPECT_EQ(pose_tol.position.x, 0.25); -// EXPECT_EQ(pose_tol.position.y, 0.25); - -// // Test normal goal checker's dynamic parameters -// auto rec_param = std::make_shared( -// x->get_node_base_interface(), x->get_node_topics_interface(), -// x->get_node_graph_interface(), -// x->get_node_services_interface()); -// auto results = rec_param->set_parameters_atomically( -// {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0), -// rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0), -// rclcpp::Parameter("test2.stateful", true)}); - -// rclcpp::spin_until_future_complete( -// x->get_node_base_interface(), -// results); - -// EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0); -// EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0); -// EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true); - -// // Test the dynamic parameters impacted the tolerances -// EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); -// EXPECT_EQ(pose_tol.position.x, 200.0); -// EXPECT_EQ(pose_tol.position.y, 200.0); -// } - -// TEST(StoppedGoalChecker, is_reached) -// { -// auto x = std::make_shared("goal_checker"); - -// SimpleGoalChecker gc; -// StoppedGoalChecker sgc; -// auto costmap = std::make_shared("test_costmap"); - -// sgc.initialize(x, "test", costmap); -// gc.initialize(x, "test2", costmap); -// geometry_msgs::msg::Pose goal_pose; -// geometry_msgs::msg::Twist velocity; -// geometry_msgs::msg::Pose current_pose; - -// // Current linear x position is tolerance away from goal -// current_pose.position.x = 0.25; -// velocity.linear.x = 0.25; -// EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); -// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); -// sgc.reset(); -// gc.reset(); - -// // Current linear x speed exceeds tolerance -// velocity.linear.x = 0.25 + std::numeric_limits::epsilon(); -// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); -// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); -// sgc.reset(); -// gc.reset(); - -// // Current linear x position is further than tolerance away from goal -// current_pose.position.x = 0.25 + std::numeric_limits::epsilon(); -// velocity.linear.x = 0.25; -// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); -// EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); -// sgc.reset(); -// gc.reset(); -// current_pose.position.x = 0.0; -// velocity.linear.x = 0.0; - -// // Current linear position is tolerance away from goal -// current_pose.position.x = 0.25 / std::sqrt(2); -// current_pose.position.y = 0.25 / std::sqrt(2); -// velocity.linear.x = 0.25 / std::sqrt(2); -// velocity.linear.y = 0.25 / std::sqrt(2); -// EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); -// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); -// sgc.reset(); -// gc.reset(); - -// // Current linear speed exceeds tolerance -// velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); -// velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); -// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); -// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); -// sgc.reset(); -// gc.reset(); - -// // Current linear position is further than tolerance away from goal -// current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); -// current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); -// velocity.linear.x = 0.25 / std::sqrt(2); -// velocity.linear.y = 0.25 / std::sqrt(2); -// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); -// EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); -// sgc.reset(); -// gc.reset(); - -// current_pose.position.x = 0.0; -// velocity.linear.x = 0.0; - - -// // Current angular position is tolerance away from goal -// auto quat = -// (Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs(); -// // epsilon for orientation is a lot bigger than double limit, probably from TF getYaw -// auto quat_epsilon = -// (Eigen::AngleAxisd::Identity() * -// Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs(); - -// current_pose.orientation.z = quat[2]; -// current_pose.orientation.w = quat[3]; -// velocity.angular.z = 0.25; -// EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); -// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); -// sgc.reset(); -// gc.reset(); - -// // Current angular speed exceeds tolerance -// velocity.angular.z = 0.25 + std::numeric_limits::epsilon(); -// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); -// EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); -// sgc.reset(); -// gc.reset(); - -// // Current angular position is further than tolerance away from goal -// current_pose.orientation.z = quat_epsilon[2]; -// current_pose.orientation.w = quat_epsilon[3]; -// velocity.angular.z = 0.25; -// EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); -// EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); -// } - -// int main(int argc, char ** argv) -// { -// ::testing::InitGoogleTest(&argc, argv); - -// rclcpp::init(argc, argv); - -// int result = RUN_ALL_TESTS(); - -// rclcpp::shutdown(); - -// return result; -// } +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "gtest/gtest.h" +#include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/stopped_goal_checker.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "eigen3/Eigen/Geometry" + +using nav2_controller::SimpleGoalChecker; +using nav2_controller::StoppedGoalChecker; + +void checkMacro( + nav2_core::GoalChecker & gc, + double x0, double y0, double theta0, + double x1, double y1, double theta1, + double xv, double yv, double thetav, + bool expected_result) +{ + gc.reset(); + + geometry_msgs::msg::Pose pose0, pose1; + pose0.position.x = x0; + pose0.position.y = y0; + pose0.position.z = 0.0; + pose0.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta0); + + pose1.position.x = x1; + pose1.position.y = y1; + pose1.position.z = 0.0; + pose1.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1); + + geometry_msgs::msg::Twist v; + v.linear.x = xv; + v.linear.y = yv; + v.angular.z = thetav; + + nav_msgs::msg::Path current_path; + if (expected_result) { + EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v, current_path)); + } else { + EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v, current_path)); + } +} + +void sameResult( + nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1, + double x0, double y0, double theta0, + double x1, double y1, double theta1, + double xv, double yv, double thetav, + bool expected_result) +{ + checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result); + checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result); +} + +void trueFalse( + nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1, + double x0, double y0, double theta0, + double x1, double y1, double theta1, + double xv, double yv, double thetav) +{ + checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true); + checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false); +} +class TestLifecycleNode : public nav2::LifecycleNode +{ +public: + explicit TestLifecycleNode(const std::string & name) + : nav2::LifecycleNode(name) + { + } + + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } +}; + +TEST(VelocityIterator, goal_checker_reset) +{ + auto x = std::make_shared("goal_checker"); + + nav2_core::GoalChecker * gc = new SimpleGoalChecker; + gc->reset(); + delete gc; + EXPECT_TRUE(true); +} + +TEST(VelocityIterator, stopped_goal_checker_reset) +{ + auto x = std::make_shared("stopped_goal_checker"); + + nav2_core::GoalChecker * sgc = new StoppedGoalChecker; + sgc->reset(); + delete sgc; + EXPECT_TRUE(true); +} + +TEST(VelocityIterator, two_checks) +{ + auto x = std::make_shared("goal_checker"); + + SimpleGoalChecker gc; + StoppedGoalChecker sgc; + auto costmap = std::make_shared("test_costmap"); + + gc.initialize(x, "nav2_controller", costmap); + sgc.initialize(x, "nav2_controller", costmap); + sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true); + sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false); + sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false); + sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false); + sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true); + trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0); + trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0); + trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0); + trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1); +} + +TEST(StoppedGoalChecker, get_tol_and_dynamic_params) +{ + auto x = std::make_shared("goal_checker"); + + SimpleGoalChecker gc; + StoppedGoalChecker sgc; + auto costmap = std::make_shared("test_costmap"); + + sgc.initialize(x, "test", costmap); + gc.initialize(x, "test2", costmap); + geometry_msgs::msg::Pose pose_tol; + geometry_msgs::msg::Twist vel_tol; + + // Test stopped goal checker's tolerance API + EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(vel_tol.linear.x, 0.25); + EXPECT_EQ(vel_tol.linear.y, 0.25); + EXPECT_EQ(vel_tol.angular.z, 0.25); + + // Test Stopped goal checker's dynamic parameters + auto rec_param = std::make_shared( + x->get_node_base_interface(), x->get_node_topics_interface(), + x->get_node_graph_interface(), + x->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.rot_stopped_velocity", 100.0), + rclcpp::Parameter("test.trans_stopped_velocity", 100.0)}); + + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + + EXPECT_EQ(x->get_parameter("test.rot_stopped_velocity").as_double(), 100.0); + EXPECT_EQ(x->get_parameter("test.trans_stopped_velocity").as_double(), 100.0); + + // Test normal goal checker's dynamic parameters + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0), + rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0), + rclcpp::Parameter("test2.stateful", true)}); + + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + + EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0); + EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0); + EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true); + + // Test the dynamic parameters impacted the tolerances + EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(vel_tol.linear.x, 100.0); + EXPECT_EQ(vel_tol.linear.y, 100.0); + EXPECT_EQ(vel_tol.angular.z, 100.0); + + EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(pose_tol.position.x, 200.0); + EXPECT_EQ(pose_tol.position.y, 200.0); +} + +TEST(StoppedGoalChecker, is_reached) +{ + auto x = std::make_shared("goal_checker"); + + SimpleGoalChecker gc; + StoppedGoalChecker sgc; + auto costmap = std::make_shared("test_costmap"); + + sgc.initialize(x, "test", costmap); + gc.initialize(x, "test2", costmap); + geometry_msgs::msg::Pose goal_pose; + geometry_msgs::msg::Twist velocity; + geometry_msgs::msg::Pose current_pose; + + // Current linear x position is tolerance away from goal + current_pose.position.x = 0.25; + velocity.linear.x = 0.25; + nav_msgs::msg::Path current_path_; + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + sgc.reset(); + gc.reset(); + + // Current linear x speed exceeds tolerance + velocity.linear.x = 0.25 + std::numeric_limits::epsilon(); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + sgc.reset(); + gc.reset(); + + // Current linear x position is further than tolerance away from goal + current_pose.position.x = 0.25 + std::numeric_limits::epsilon(); + velocity.linear.x = 0.25; + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + sgc.reset(); + gc.reset(); + current_pose.position.x = 0.0; + velocity.linear.x = 0.0; + + // Current linear position is tolerance away from goal + current_pose.position.x = 0.25 / std::sqrt(2); + current_pose.position.y = 0.25 / std::sqrt(2); + velocity.linear.x = 0.25 / std::sqrt(2); + velocity.linear.y = 0.25 / std::sqrt(2); + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + sgc.reset(); + gc.reset(); + + // Current linear speed exceeds tolerance + velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + sgc.reset(); + gc.reset(); + + // Current linear position is further than tolerance away from goal + current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + velocity.linear.x = 0.25 / std::sqrt(2); + velocity.linear.y = 0.25 / std::sqrt(2); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + sgc.reset(); + gc.reset(); + + current_pose.position.x = 0.0; + velocity.linear.x = 0.0; + + + // Current angular position is tolerance away from goal + auto quat = + (Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs(); + // epsilon for orientation is a lot bigger than double limit, probably from TF getYaw + auto quat_epsilon = + (Eigen::AngleAxisd::Identity() * + Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs(); + + current_pose.orientation.z = quat[2]; + current_pose.orientation.w = quat[3]; + velocity.angular.z = 0.25; + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + sgc.reset(); + gc.reset(); + + // Current angular speed exceeds tolerance + velocity.angular.z = 0.25 + std::numeric_limits::epsilon(); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + sgc.reset(); + gc.reset(); + + // Current angular position is further than tolerance away from goal + current_pose.orientation.z = quat_epsilon[2]; + current_pose.orientation.w = quat_epsilon[3]; + velocity.angular.z = 0.25; + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index ae2cd1a60ea..4a3f632dba5 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -109,7 +109,7 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 goal_checker: - plugin: "nav2_controller::PathCompleteGoalChecker" + plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 0b6bd2fdf67..3a19898a40a 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -58,7 +58,7 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 goal_checker: - plugin: "nav2_controller::PathCompleteGoalChecker" + plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True From 60997a3cb9c6ca6196e1d62481b4bb4dd1016cf4 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Mon, 22 Sep 2025 17:22:09 +0000 Subject: [PATCH 14/62] Revert "Use local plan's last pose as end pose" This reverts commit 145b2d98a2290e541be962109d84c176031f1976. Signed-off-by: mini-1235 --- .../plugins/simple_goal_checker.hpp | 2 +- nav2_controller/plugins/simple_goal_checker.cpp | 12 +++++++++++- nav2_controller/src/controller_server.cpp | 13 +++++++++---- nav2_controller/src/parameter_handler.cpp | 16 ++++++++-------- 4 files changed, 29 insertions(+), 14 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 7fa555e19ea..846a56792aa 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -72,7 +72,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker geometry_msgs::msg::Twist & vel_tolerance) override; protected: - double xy_goal_tolerance_, yaw_goal_tolerance_; + double xy_goal_tolerance_, yaw_goal_tolerance_, path_length_tolerance_; bool stateful_, check_xy_; // Cached squared xy_goal_tolerance_ double xy_goal_tolerance_sq_; diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 1a5798f1809..326a69db1ae 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -55,6 +55,7 @@ namespace nav2_controller SimpleGoalChecker::SimpleGoalChecker() : xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), + path_length_tolerance_(1.0), stateful_(true), check_xy_(true), xy_goal_tolerance_sq_(0.0625) @@ -75,12 +76,16 @@ void SimpleGoalChecker::initialize( nav2::declare_parameter_if_not_declared( node, plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25)); + nav2::declare_parameter_if_not_declared( + node, + plugin_name + ".path_length_tolerance", rclcpp::ParameterValue(1.0)); nav2::declare_parameter_if_not_declared( node, plugin_name + ".stateful", rclcpp::ParameterValue(true)); node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); + node->get_parameter(plugin_name + ".yaw_goal_tolerance", path_length_tolerance_); node->get_parameter(plugin_name + ".stateful", stateful_); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; @@ -97,8 +102,11 @@ void SimpleGoalChecker::reset() bool SimpleGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &) + const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path ¤t_path) { + if (nav2_util::geometry_utils::calculate_path_length(current_path) > path_length_tolerance_) { + return false; + } if (check_xy_) { double dx = query_pose.position.x - goal_pose.position.x, dy = query_pose.position.y - goal_pose.position.y; @@ -156,6 +164,8 @@ SimpleGoalChecker::dynamicParametersCallback(std::vector para xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; } else if (param_name == plugin_name_ + ".yaw_goal_tolerance") { yaw_goal_tolerance_ = parameter.as_double(); + } else if (param_name == plugin_name_ + ".path_length_tolerance") { + path_length_tolerance_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == plugin_name_ + ".stateful") { diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 6f1c8f80209..8c9a324db57 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -536,6 +536,14 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) controllers_[current_controller_]->newPathReceived(path); path_handler_->setPlan(path); + end_pose_ = path.poses.back(); + end_pose_.header.frame_id = path.header.frame_id; + goal_checkers_[current_goal_checker_]->reset(); + + RCLCPP_DEBUG( + get_logger(), "Path end point is (%.2f, %.2f)", + end_pose_.pose.position.x, end_pose_.pose.position.y); + current_path_ = path; } @@ -554,10 +562,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); pruned_global_plan_ = path_handler_->pruneGlobalPlan(pose); - // RCLCPP_INFO(get_logger(), "compute remaining distance %lf ",nav2_util::geometry_utils::calculate_path_length(pruned_global_plan_)); - end_pose_ = pruned_global_plan_.poses.back(); - end_pose_.header.frame_id = pruned_global_plan_.header.frame_id; - goal_checkers_[current_goal_checker_]->reset(); + RCLCPP_INFO(get_logger(), "compute remaining distance %lf ",nav2_util::geometry_utils::calculate_path_length(pruned_global_plan_)); geometry_msgs::msg::TwistStamped cmd_vel_2d; diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 89c36c413a5..e8a9ad710c4 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -173,14 +173,14 @@ ParameterHandler::ParameterHandler( } } - post_set_params_handler_ = node->add_post_set_parameters_callback( - std::bind( - &ParameterHandler::updateParametersCallback, - this, std::placeholders::_1)); - on_set_params_handler_ = node->add_on_set_parameters_callback( - std::bind( - &ParameterHandler::validateParameterUpdatesCallback, - this, std::placeholders::_1)); + // post_set_params_handler_ = node->add_post_set_parameters_callback( + // std::bind( + // &ParameterHandler::updateParametersCallback, + // this, std::placeholders::_1)); + // on_set_params_handler_ = node->add_on_set_parameters_callback( + // std::bind( + // &ParameterHandler::validateParameterUpdatesCallback, + // this, std::placeholders::_1)); } ParameterHandler::~ParameterHandler() From 79aaf69d259901355a46666099f46aa7bfaeec3e Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Mon, 22 Sep 2025 17:24:11 +0000 Subject: [PATCH 15/62] Linting Signed-off-by: mini-1235 --- .../nav2_controller/controller_server.hpp | 9 ++++--- .../nav2_controller/parameter_handler.hpp | 3 ++- .../plugins/position_goal_checker.cpp | 2 +- .../plugins/simple_goal_checker.cpp | 2 +- nav2_controller/src/controller_server.cpp | 6 +++-- nav2_controller/src/parameter_handler.cpp | 6 ++--- .../src/graceful_controller.cpp | 22 ++++++++-------- .../test/test_graceful_controller.cpp | 12 ++++++--- nav2_mppi_controller/src/controller.cpp | 5 ++-- .../src/regulated_pure_pursuit_controller.cpp | 25 ++++++++++--------- .../src/nav2_rotation_shim_controller.cpp | 7 ++++-- .../test/test_shim_controller.cpp | 9 ++++--- 12 files changed, 63 insertions(+), 45 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 7caad0e7ac5..2affa7911db 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -213,9 +213,12 @@ class ControllerServer : public nav2::LifecycleNode geometry_msgs::msg::Twist getThresholdedTwist(const geometry_msgs::msg::Twist & twist) { geometry_msgs::msg::Twist twist_thresh; - twist_thresh.linear.x = getThresholdedVelocity(twist.linear.x, params_->min_x_velocity_threshold); - twist_thresh.linear.y = getThresholdedVelocity(twist.linear.y, params_->min_y_velocity_threshold); - twist_thresh.angular.z = getThresholdedVelocity(twist.angular.z, params_->min_theta_velocity_threshold); + twist_thresh.linear.x = getThresholdedVelocity(twist.linear.x, + params_->min_x_velocity_threshold); + twist_thresh.linear.y = getThresholdedVelocity(twist.linear.y, + params_->min_y_velocity_threshold); + twist_thresh.angular.z = getThresholdedVelocity(twist.angular.z, + params_->min_theta_velocity_threshold); return twist_thresh; } diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index d45193876d5..403aa18483c 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -99,7 +99,8 @@ class ParameterHandler rclcpp::Logger logger_ {rclcpp::get_logger("GracefulMotionController")}; const std::vector default_progress_checker_ids_{"progress_checker"}; - const std::vector default_progress_checker_types_{"nav2_controller::SimpleProgressChecker"}; + const std::vector default_progress_checker_types_{ + "nav2_controller::SimpleProgressChecker"}; const std::vector default_goal_checker_ids_{"goal_checker"}; const std::vector default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}; const std::vector default_controller_ids_{"FollowPath"}; diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index 36735ea35cd..776347d20cb 100644 --- a/nav2_controller/plugins/position_goal_checker.cpp +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -65,7 +65,7 @@ void PositionGoalChecker::reset() bool PositionGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &) + const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &) { // If stateful and position was already reached, maintain state if (stateful_ && position_reached_) { diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 326a69db1ae..0ae3e1bf9f3 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -102,7 +102,7 @@ void SimpleGoalChecker::reset() bool SimpleGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path ¤t_path) + const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path & current_path) { if (nav2_util::geometry_utils::calculate_path_length(current_path) > path_length_tolerance_) { return false; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 8c9a324db57..7c4a4d4b177 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -158,7 +158,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_logger(), "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); - odom_sub_ = std::make_unique(node, params_->odom_duration, params_->odom_topic); + odom_sub_ = std::make_unique(node, params_->odom_duration, + params_->odom_topic); vel_publisher_ = std::make_unique(node, "cmd_vel"); costmap_update_timeout_ = rclcpp::Duration::from_seconds(params_->costmap_update_timeout); @@ -562,7 +563,8 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); pruned_global_plan_ = path_handler_->pruneGlobalPlan(pose); - RCLCPP_INFO(get_logger(), "compute remaining distance %lf ",nav2_util::geometry_utils::calculate_path_length(pruned_global_plan_)); + RCLCPP_INFO(get_logger(), "compute remaining distance %lf ", + nav2_util::geometry_utils::calculate_path_length(pruned_global_plan_)); geometry_msgs::msg::TwistStamped cmd_vel_2d; diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index e8a9ad710c4..fc42f92fa77 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -196,15 +196,15 @@ ParameterHandler::~ParameterHandler() on_set_params_handler_.reset(); } rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback( - std::vector /*parameters*/) + std::vector/*parameters*/) { rcl_interfaces::msg::SetParametersResult result; - + return result; } void ParameterHandler::updateParametersCallback( - std::vector /*parameters*/) + std::vector/*parameters*/) { } diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 4420ffd7b04..2f7571cedf7 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -133,22 +133,22 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // Transform the pruned global plan to robot base frame auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; + stamped_pose.header.stamp = pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; - if (!nav2_util::transformPoseInTargetFrame( + if (!nav2_util::transformPoseInTargetFrame( stamped_pose, transformed_pose, *tf_buffer_, costmap_ros_->getBaseFrameID(), params_->transform_tolerance)) - { - throw nav2_core::ControllerTFError( + { + throw nav2_core::ControllerTFError( "Unable to transform plan pose into local frame"); - } + } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; nav_msgs::msg::Path transformed_plan; transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 856c6e85084..459b728b69f 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -524,7 +524,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path pruned_global_plan; - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, pruned_global_plan); + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, + pruned_global_plan); // Check results: the robot should rotate in place. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. @@ -600,7 +601,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path pruned_global_plan; - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, pruned_global_plan); + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, + pruned_global_plan); // Check results: the robot should go straight to the target. // So, linear velocity should be some positive value and angular velocity should be zero. @@ -681,7 +683,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path pruned_global_plan; - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, pruned_global_plan); + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, + pruned_global_plan); // Check results: the robot should go straight to the target. // So, both linear velocity should be some negative values. @@ -767,7 +770,8 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path pruned_global_plan; - auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, pruned_global_plan); + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, + pruned_global_plan); // Check results: the robot should do a final rotation near the target. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 86098b6682f..b076bc42c8b 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -101,7 +101,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( #endif std::lock_guard param_lock(*parameters_handler_->getLock()); - + nav_msgs::msg::Path transformed_plan; transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); transformed_plan.header.stamp = robot_pose.header.stamp; @@ -110,7 +110,8 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( // Find the furthest relevant pose on the path to consider within costmap // bounds // Transforming it to the costmap frame in the same loop - for (auto global_plan_pose = pruned_global_plan.poses.begin(); global_plan_pose != pruned_global_plan.poses.end(); + for (auto global_plan_pose = pruned_global_plan.poses.begin(); + global_plan_pose != pruned_global_plan.poses.end(); ++global_plan_pose) { // Transform from global plan frame to costmap frame 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 699db509b51..e6d53a5993c 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 @@ -180,22 +180,22 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Transform the pruned global plan to robot base frame auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; + stamped_pose.header.stamp = pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; - if (!nav2_util::transformPoseInTargetFrame( + if (!nav2_util::transformPoseInTargetFrame( stamped_pose, transformed_pose, *tf_, costmap_ros_->getBaseFrameID(), params_->transform_tolerance)) - { - throw nav2_core::ControllerTFError( + { + throw nav2_core::ControllerTFError( "Unable to transform plan pose into local frame"); - } + } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; nav_msgs::msg::Path transformed_plan; transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); @@ -397,7 +397,8 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = sign * linear_vel; } -void RegulatedPurePursuitController::newPathReceived(const nav_msgs::msg::Path & /*raw_global_path*/) +void RegulatedPurePursuitController::newPathReceived( + const nav_msgs::msg::Path & /*raw_global_path*/) { has_reached_xy_tolerance_ = false; } diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 750037f6d8b..6872ebcf429 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -190,7 +190,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands goal_checker->getTolerances(pose_tolerance, vel_tolerance); position_goal_checker_->setXYGoalTolerance(pose_tolerance.position.x); - if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity, pruned_global_plan)) { + if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity, + pruned_global_plan)) + { double pose_yaw = tf2::getYaw(pose.pose.orientation); double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation); @@ -256,7 +258,8 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands // If at this point, use the primary controller to path track in_rotation_ = false; - auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker, pruned_global_plan); + auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker, + pruned_global_plan); last_angular_vel_ = cmd_vel.twist.angular.z; return cmd_vel; } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index b80b763feee..6faefade0c5 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -268,12 +268,14 @@ TEST(RotationShimControllerTest, computeVelocityTests) // send without setting a path - should go to RPP immediately // then it should throw an exception because the path is empty and invalid nav_msgs::msg::Path pruned_global_plan; - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), std::runtime_error); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), + std::runtime_error); // Set with a path -- should attempt to find a sampled point but throw exception // because it cannot be found, then go to RPP and throw exception because it cannot be transformed controller->newPathReceived(path); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), std::runtime_error); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), + std::runtime_error); path.header.frame_id = "base_link"; path.poses[1].pose.position.x = 0.1; @@ -307,7 +309,8 @@ TEST(RotationShimControllerTest, computeVelocityTests) // and exception because it is off of the costmap controller->newPathReceived(path); tf_broadcaster->sendTransform(transform); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), std::runtime_error); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), + std::runtime_error); } TEST(RotationShimControllerTest, openLoopRotationTests) { From 55df4529136aca64c11b47057d0ae9d0cbe5a08f Mon Sep 17 00:00:00 2001 From: Maurice Date: Wed, 24 Sep 2025 15:06:28 +0800 Subject: [PATCH 16/62] Add prune distance Signed-off-by: Maurice Signed-off-by: mini-1235 --- .../include/nav2_controller/parameter_handler.hpp | 1 + nav2_controller/src/parameter_handler.cpp | 5 ++++- nav2_controller/src/path_handler.cpp | 12 ++++++++++-- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index 403aa18483c..440867bfc88 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -46,6 +46,7 @@ struct Parameters double odom_duration; bool interpolate_curvature_after_goal; double max_robot_pose_search_dist; + double prune_distance; bool enforce_path_inversion; float inversion_xy_tolerance; float inversion_yaw_tolerance; diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index fc42f92fa77..5786b81a5d1 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -66,7 +66,9 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, "interpolate_curvature_after_goal", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node, "max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_size_x)); + node, "max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_size_x / 2.0)); + declare_parameter_if_not_declared( + node, "prune_distance", rclcpp::ParameterValue(1.5)); declare_parameter_if_not_declared( node, "enforce_path_inversion", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( @@ -103,6 +105,7 @@ ParameterHandler::ParameterHandler( "interpolate_curvature_after_goal", params_.interpolate_curvature_after_goal); node->get_parameter("max_robot_pose_search_dist", params_.max_robot_pose_search_dist); + node->get_parameter("prune_distance", params_.prune_distance); node->get_parameter("costmap_update_timeout", params_.costmap_update_timeout); node->get_parameter("enforce_path_inversion", params_.enforce_path_inversion); node->get_parameter("inversion_xy_tolerance", params_.inversion_xy_tolerance); diff --git a/nav2_controller/src/path_handler.cpp b/nav2_controller/src/path_handler.cpp index dff8f5fe15f..de470cf83eb 100644 --- a/nav2_controller/src/path_handler.cpp +++ b/nav2_controller/src/path_handler.cpp @@ -129,13 +129,21 @@ nav_msgs::msg::Path PathHandler::pruneGlobalPlan( closest_point = std::prev(std::prev(closest_pose_upper_bound)); } - // We'll discard points on the plan that are outside the local costmap + auto pruned_plan_end = global_plan_up_to_inversion_.poses.end(); const double max_costmap_extent = getCostmapMaxExtent(); - auto pruned_plan_end = std::find_if( + //TODO: For RPP and graceful we don't have prune distance, maybe we need to support two kinds of pruned_plan_end here + // by parameter? + if (1){ + pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + } + else{ + pruned_plan_end = std::find_if( closest_point, global_plan_up_to_inversion_.poses.end(), [&](const auto & global_plan_pose) { return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; }); + } nav_msgs::msg::Path pruned_plan; pruned_plan.poses.insert(pruned_plan.poses.end(), From 99047cd5255f96709c3005de5e00583fe3f4a3d8 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 24 Sep 2025 15:42:37 +0000 Subject: [PATCH 17/62] Add dynamic parameter Signed-off-by: mini-1235 --- nav2_controller/src/parameter_handler.cpp | 77 ++++++++++++++++++++++- nav2_controller/src/path_handler.cpp | 2 +- 2 files changed, 75 insertions(+), 4 deletions(-) diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 5786b81a5d1..65ebac0426b 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -105,6 +105,12 @@ ParameterHandler::ParameterHandler( "interpolate_curvature_after_goal", params_.interpolate_curvature_after_goal); node->get_parameter("max_robot_pose_search_dist", params_.max_robot_pose_search_dist); + if (params_.max_robot_pose_search_dist < 0.0) { + RCLCPP_WARN( + logger_, "Max robot search distance is negative, setting to max to search" + " every point on path for the closest value."); + params_.max_robot_pose_search_dist = std::numeric_limits::max(); + } node->get_parameter("prune_distance", params_.prune_distance); node->get_parameter("costmap_update_timeout", params_.costmap_update_timeout); node->get_parameter("enforce_path_inversion", params_.enforce_path_inversion); @@ -199,17 +205,82 @@ ParameterHandler::~ParameterHandler() on_set_params_handler_.reset(); } rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback( - std::vector/*parameters*/) + std::vector parameters) { rcl_interfaces::msg::SetParametersResult result; - + result.successful = true; + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + // If we are trying to change the parameter of a plugin we can just skip it at this point + // as they handle parameter changes themselves and don't need to lock the mutex + if (param_name.find('.') != std::string::npos) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (parameter.as_double() < 0.0) { + RCLCPP_WARN( + logger_, "The value of parameter '%s' is incorrectly set to %f, " + "it should be >=0. Ignoring parameter update.", + param_name.c_str(), parameter.as_double()); + result.successful = false; + } + } + } return result; } void ParameterHandler::updateParametersCallback( - std::vector/*parameters*/) + std::vector parameters) { + std::lock_guard lock_reinit(mutex_); + + for (const auto & parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "controller_frequency") { + params_.controller_frequency = parameter.as_double(); + } else if (param_name == "transform_tolerance") { + params_.transform_tolerance = parameter.as_double(); + } else if (param_name == "min_x_velocity_threshold") { + params_.min_x_velocity_threshold = parameter.as_double(); + } else if (param_name == "min_y_velocity_threshold") { + params_.min_y_velocity_threshold = parameter.as_double(); + } else if (param_name == "min_theta_velocity_threshold") { + params_.min_theta_velocity_threshold = parameter.as_double(); + } else if (param_name == "failure_tolerance") { + params_.failure_tolerance = parameter.as_double(); + } else if (param_name == "costmap_update_timeout") { + params_.costmap_update_timeout = parameter.as_double(); + } else if (param_name == "odom_duration") { + params_.odom_duration = parameter.as_double(); + } else if (param_name == "max_robot_pose_search_dist") { + params_.max_robot_pose_search_dist = parameter.as_double(); + } else if (param_name == "prune_distance") { + params_.prune_distance = parameter.as_double(); + } else if (param_name == "inversion_xy_tolerance") { + params_.inversion_xy_tolerance = parameter.as_double(); + } else if (param_name == "inversion_yaw_tolerance") { + params_.inversion_yaw_tolerance = parameter.as_double(); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == "use_realtime_priority") { + params_.use_realtime_priority = parameter.as_bool(); + } else if (param_name == "publish_zero_velocity") { + params_.publish_zero_velocity = parameter.as_bool(); + } else if (param_name == "enforce_path_inversion") { + params_.enforce_path_inversion = parameter.as_bool(); + } + } else if (param_type == ParameterType::PARAMETER_STRING){ + if (param_name == "speed_limit_topic") { + params_.speed_limit_topic = parameter.as_string(); + } else if (param_name == "odom_topic") { + params_.odom_topic = parameter.as_string(); + } + } + } } } // namespace nav2_controller diff --git a/nav2_controller/src/path_handler.cpp b/nav2_controller/src/path_handler.cpp index de470cf83eb..9232751a2a1 100644 --- a/nav2_controller/src/path_handler.cpp +++ b/nav2_controller/src/path_handler.cpp @@ -135,7 +135,7 @@ nav_msgs::msg::Path PathHandler::pruneGlobalPlan( // by parameter? if (1){ pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( - closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + closest_point, global_plan_up_to_inversion_.poses.end(), params_->prune_distance); } else{ pruned_plan_end = std::find_if( From 8ed2ce4e023573774e0ac1b3d5b1519c3868eebc Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 25 Sep 2025 14:13:16 +0000 Subject: [PATCH 18/62] Revert unrelated changes Signed-off-by: mini-1235 --- nav2_controller/CMakeLists.txt | 14 +++----------- nav2_controller/src/controller_server.cpp | 2 -- 2 files changed, 3 insertions(+), 13 deletions(-) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index dfe875a0f71..93c58d62881 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -186,13 +186,7 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS - simple_progress_checker - position_goal_checker - pose_progress_checker - simple_goal_checker - stopped_goal_checker - ${library_name} +install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -208,14 +202,12 @@ install(DIRECTORY include/ ) ament_export_include_directories(include/${PROJECT_NAME}) -ament_export_libraries( - simple_progress_checker +ament_export_libraries(simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker position_goal_checker - ${library_name} -) + ${library_name}) ament_export_dependencies( geometry_msgs nav2_core diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 7c4a4d4b177..ea0f6fbd46c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -563,8 +563,6 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); pruned_global_plan_ = path_handler_->pruneGlobalPlan(pose); - RCLCPP_INFO(get_logger(), "compute remaining distance %lf ", - nav2_util::geometry_utils::calculate_path_length(pruned_global_plan_)); geometry_msgs::msg::TwistStamped cmd_vel_2d; From 4b3ffa98562c5ccc0e8c1eaa03c1a5ae253d8fed Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 26 Sep 2025 16:33:11 +0000 Subject: [PATCH 19/62] DWB Signed-off-by: mini-1235 --- .../include/dwb_core/dwb_local_planner.hpp | 22 ++--- .../dwb_core/include/dwb_core/publisher.hpp | 14 ++-- .../dwb_core/src/dwb_local_planner.cpp | 84 ++++++++----------- .../dwb_core/src/publisher.cpp | 14 ---- 4 files changed, 44 insertions(+), 90 deletions(-) diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 755ab6c1b6d..162d9f163db 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -105,13 +105,14 @@ class DWBLocalPlanner : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands + * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @return The best command for the robot to drive */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & transformed_global_plan) override; + nav_msgs::msg::Path & pruned_global_plan) override; /** * @brief Score a given command. Can be used for testing. @@ -138,12 +139,14 @@ class DWBLocalPlanner : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param results Output param, if not NULL, will be filled in with full evaluation results + * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @return Best command */ virtual nav_2d_msgs::msg::Twist2DStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const nav_2d_msgs::msg::Twist2D & velocity, - std::shared_ptr & results); + std::shared_ptr & results, + nav_msgs::msg::Path & pruned_global_plan); /** * @brief Limits the maximum linear speed of the robot. @@ -160,16 +163,6 @@ class DWBLocalPlanner : public nav2_core::Controller } protected: - /** - * @brief Helper method for two common operations for the operating on the global_plan - * - * Transforms the global plan (stored in global_plan_) relative to the pose and saves it in - * transformed_plan and possibly publishes it. Then it takes the last pose and transforms it - * to match the local costmap's frame - */ - void prepareGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, nav_msgs::msg::Path & transformed_plan, - geometry_msgs::msg::PoseStamped & goal_pose, bool publish_plan = true); /** * @brief Iterate through all the twists and find the best one @@ -179,13 +172,8 @@ class DWBLocalPlanner : public nav2_core::Controller const nav_2d_msgs::msg::Twist2D velocity, std::shared_ptr & results); - nav_msgs::msg::Path global_plan_; ///< Saved Global Plan - bool prune_plan_; - double prune_distance_; bool debug_trajectory_details_; double transform_tolerance_{0}; - bool shorten_transformed_plan_; - double forward_prune_distance_; /** * @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic" diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp index 57cc7e97cd9..b992719b8ad 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp @@ -59,12 +59,11 @@ namespace dwb_core * @brief Consolidation of all the publishing logic for the DWB Local Planner. * * Right now, it can publish - * 1) The Global Plan (as passed in using setPath) - * 2) The Local Plan (after it is calculated) - * 3) The Transformed Global Plan (since it may be different than the global) - * 4) The Full LocalPlanEvaluation - * 5) Markers representing the different trajectories evaluated - * 6) The CostGrid (in the form of a complex PointCloud2) + * 1) The Local Plan (after it is calculated) + * 2) The Transformed Global Plan (since it may be different than the global) + * 3) The Full LocalPlanEvaluation + * 4) Markers representing the different trajectories evaluated + * 5) The CostGrid (in the form of a complex PointCloud2) */ class DWBPublisher { @@ -94,7 +93,6 @@ class DWBPublisher void publishCostGrid( const std::shared_ptr costmap_ros, const std::vector critics); - void publishGlobalPlan(const nav_msgs::msg::Path plan); void publishTransformedPlan(const nav_msgs::msg::Path plan); void publishLocalPlan(const nav_msgs::msg::Path plan); @@ -108,7 +106,6 @@ class DWBPublisher // Flags for turning on/off publishing specific components bool publish_evaluation_; - bool publish_global_plan_; bool publish_transformed_; bool publish_local_plan_; bool publish_trajectories_; @@ -120,7 +117,6 @@ class DWBPublisher // Publisher Objects std::shared_ptr> eval_pub_; - std::shared_ptr> global_pub_; std::shared_ptr> transformed_pub_; std::shared_ptr> local_pub_; std::shared_ptr> marker_pub_; diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index bf5d84080e9..e6776f14523 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -83,24 +83,12 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".default_critic_namespaces", rclcpp::ParameterValue(std::vector())); - declare_parameter_if_not_declared( - node, dwb_plugin_name_ + ".prune_plan", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, dwb_plugin_name_ + ".prune_distance", - rclcpp::ParameterValue(2.0)); - declare_parameter_if_not_declared( - node, dwb_plugin_name_ + ".forward_prune_distance", - rclcpp::ParameterValue(2.0)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".debug_trajectory_details", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".trajectory_generator_name", rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator"))); - declare_parameter_if_not_declared( - node, dwb_plugin_name_ + ".shorten_transformed_plan", - rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", rclcpp::ParameterValue(true)); @@ -108,22 +96,11 @@ void DWBLocalPlanner::configure( std::string traj_generator_name; node->get_parameter("transform_tolerance", transform_tolerance_); - - node->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_); - node->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); - node->get_parameter(dwb_plugin_name_ + ".forward_prune_distance", forward_prune_distance_); - if (forward_prune_distance_ < 0.0) { - RCLCPP_WARN( - logger_, "Forward prune distance is negative, setting to max to search" - " every point on path for the closest value."); - forward_prune_distance_ = std::numeric_limits::max(); - } node->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); node->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); node->get_parameter( dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_); - node->get_parameter(dwb_plugin_name_ + ".shorten_transformed_plan", shorten_transformed_plan_); pub_ = std::make_unique(node, dwb_plugin_name_); pub_->on_configure(); @@ -228,16 +205,12 @@ DWBLocalPlanner::loadCritics() } void -DWBLocalPlanner::newPathReceived(const nav_msgs::msg::Path & raw_global_path) +DWBLocalPlanner::newPathReceived(const nav_msgs::msg::Path & /*raw_global_path*/) { for (TrajectoryCritic::Ptr & critic : critics_) { critic->reset(); } - traj_generator_->reset(); - - pub_->publishGlobalPlan(raw_global_path); - global_plan_ = raw_global_path; } geometry_msgs::msg::TwistStamped @@ -245,7 +218,7 @@ DWBLocalPlanner::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & /*transformed_global_plan*/) + nav_msgs::msg::Path & pruned_global_plan) { std::shared_ptr results = nullptr; if (pub_->shouldRecordEvaluation()) { @@ -255,7 +228,7 @@ DWBLocalPlanner::computeVelocityCommands( try { nav_2d_msgs::msg::Twist2DStamped cmd_vel2d = computeVelocityCommands( pose, - nav_2d_utils::twist3Dto2D(velocity), results); + nav_2d_utils::twist3Dto2D(velocity), results, pruned_global_plan); pub_->publishEvaluation(results); geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.twist = nav_2d_utils::twist2Dto3D(cmd_vel2d.velocity); @@ -275,28 +248,12 @@ DWBLocalPlanner::computeVelocityCommands( } } -void -DWBLocalPlanner::prepareGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, nav_msgs::msg::Path & transformed_plan, - geometry_msgs::msg::PoseStamped & goal_pose, bool publish_plan) -{ - if (publish_plan) { - pub_->publishTransformedPlan(transformed_plan); - } - - goal_pose.header.frame_id = global_plan_.header.frame_id; - goal_pose.header.stamp = pose.header.stamp; - goal_pose.pose = global_plan_.poses.back().pose; - nav2_util::transformPoseInTargetFrame( - goal_pose, goal_pose, *tf_, - costmap_ros_->getGlobalFrameID(), transform_tolerance_); -} - nav_2d_msgs::msg::Twist2DStamped DWBLocalPlanner::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const nav_2d_msgs::msg::Twist2D & velocity, - std::shared_ptr & results) + std::shared_ptr & results, + nav_msgs::msg::Path & pruned_global_plan) { if (results) { results->header.frame_id = pose.header.frame_id; @@ -304,9 +261,36 @@ DWBLocalPlanner::computeVelocityCommands( } nav_msgs::msg::Path transformed_plan; - geometry_msgs::msg::PoseStamped goal_pose; + transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); + transformed_plan.header.stamp = pose.header.stamp; + + // Transform the pruned global plan to global frame + auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; + stamped_pose.header.stamp = pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + + if (!nav2_util::transformPoseInTargetFrame( + stamped_pose, transformed_pose, *tf_, + transformed_plan.header.frame_id, transform_tolerance_)) + { + throw nav2_core::ControllerTFError( + "Unable to transform plan pose into local frame"); + } - prepareGlobalPlan(pose, transformed_plan, goal_pose); + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + std::transform( + pruned_global_plan.poses.begin(), + pruned_global_plan.poses.end(), + std::back_inserter(transformed_plan.poses), + transformGlobalPlanToLocal); + pub_->publishTransformedPlan(transformed_plan); + //TODO: handle goal pose here + geometry_msgs::msg::PoseStamped goal_pose; nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock lock(*(costmap->getMutex())); diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index 46651d586f6..d01b3290373 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -75,9 +75,6 @@ DWBPublisher::on_configure() declare_parameter_if_not_declared( node, plugin_name_ + ".publish_evaluation", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".publish_global_plan", - rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".publish_transformed_plan", rclcpp::ParameterValue(true)); @@ -95,14 +92,12 @@ DWBPublisher::on_configure() rclcpp::ParameterValue(0.1)); node->get_parameter(plugin_name_ + ".publish_evaluation", publish_evaluation_); - node->get_parameter(plugin_name_ + ".publish_global_plan", publish_global_plan_); node->get_parameter(plugin_name_ + ".publish_transformed_plan", publish_transformed_); node->get_parameter(plugin_name_ + ".publish_local_plan", publish_local_plan_); node->get_parameter(plugin_name_ + ".publish_trajectories", publish_trajectories_); node->get_parameter(plugin_name_ + ".publish_cost_grid_pc", publish_cost_grid_pc_); eval_pub_ = node->create_publisher("evaluation"); - global_pub_ = node->create_publisher("received_global_plan"); transformed_pub_ = node->create_publisher("transformed_global_plan"); local_pub_ = node->create_publisher("local_plan"); marker_pub_ = node->create_publisher("marker"); @@ -119,7 +114,6 @@ nav2::CallbackReturn DWBPublisher::on_activate() { eval_pub_->on_activate(); - global_pub_->on_activate(); transformed_pub_->on_activate(); local_pub_->on_activate(); marker_pub_->on_activate(); @@ -132,7 +126,6 @@ nav2::CallbackReturn DWBPublisher::on_deactivate() { eval_pub_->on_deactivate(); - global_pub_->on_deactivate(); transformed_pub_->on_deactivate(); local_pub_->on_deactivate(); marker_pub_->on_deactivate(); @@ -145,7 +138,6 @@ nav2::CallbackReturn DWBPublisher::on_cleanup() { eval_pub_.reset(); - global_pub_.reset(); transformed_pub_.reset(); local_pub_.reset(); marker_pub_.reset(); @@ -337,12 +329,6 @@ DWBPublisher::publishCostGrid( cost_grid_pc_pub_->publish(std::move(cost_grid_pc)); } -void -DWBPublisher::publishGlobalPlan(const nav_msgs::msg::Path plan) -{ - publishGenericPlan(plan, *global_pub_, publish_global_plan_); -} - void DWBPublisher::publishTransformedPlan(const nav_msgs::msg::Path plan) { From 0c00776dde19d41613202088ac0f7aa71a3bea5e Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 2 Oct 2025 16:36:43 +0000 Subject: [PATCH 20/62] Revert "Remove goal from MPPI" This reverts commit 5bc19f2da2649cd500ef11c374103f7de5a8116f. Signed-off-by: mini-1235 --- .../benchmark/optimizer_benchmark.cpp | 2 +- .../nav2_mppi_controller/critic_data.hpp | 3 ++- .../optimal_trajectory_validator.hpp | 4 +++- .../nav2_mppi_controller/optimizer.hpp | 8 +++++--- nav2_mppi_controller/src/controller.cpp | 5 ++++- nav2_mppi_controller/src/optimizer.cpp | 7 +++++-- .../test/critic_manager_test.cpp | 3 ++- nav2_mppi_controller/test/critics_tests.cpp | 19 ++++++++++--------- .../test/optimizer_smoke_test.cpp | 3 ++- .../test/optimizer_unit_tests.cpp | 5 +++-- nav2_mppi_controller/test/utils_test.cpp | 12 +++++++----- 11 files changed, 44 insertions(+), 27 deletions(-) diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index 173a141e745..afe8e83eea9 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -92,7 +92,7 @@ void prepareAndRunBenchmark( nav2_core::GoalChecker * dummy_goal_checker{nullptr}; for (auto _ : state) { - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, path.poses.back().pose, dummy_goal_checker); } } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index 38702da7ee1..8476dde055c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -34,13 +34,14 @@ namespace mppi /** * @struct mppi::CriticData * @brief Data to pass to critics for scoring, including state, trajectories, - * pruned path, costs, and important parameters to share + * pruned path, global goal, costs, and important parameters to share */ struct CriticData { const models::State & state; const models::Trajectories & trajectories; const models::Path & path; + const geometry_msgs::msg::Pose & goal; Eigen::ArrayXf & costs; float & model_dt; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp index 023de224729..b3f7b03faab 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp @@ -111,6 +111,7 @@ class OptimalTrajectoryValidator * @param robot_pose The current pose of the robot * @param robot_speed The current speed of the robot * @param plan The planned path for the robot + * @param goal The goal pose for the robot * @return True if the trajectory is valid, false otherwise */ virtual ValidationResult validateTrajectory( @@ -118,7 +119,8 @@ class OptimalTrajectoryValidator const models::ControlSequence & /*control_sequence*/, const geometry_msgs::msg::PoseStamped & /*robot_pose*/, const geometry_msgs::msg::Twist & /*robot_speed*/, - const nav_msgs::msg::Path & /*plan*/) + const nav_msgs::msg::Path & /*plan*/, + const geometry_msgs::msg::Pose & /*goal*/) { // The Optimizer automatically ensures that we are within Kinematic // and dynamic constraints, no need to check for those again. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 7d8dd64c147..1914abde647 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -91,13 +91,14 @@ class Optimizer * @param robot_pose Pose of the robot at given time * @param robot_speed Speed of the robot at given time * @param plan Path plan to track + * @param goal Given Goal pose to reach. * @param goal_checker Object to check if goal is completed * @return Tuple of [TwistStamped command, optimal trajectory] */ std::tuple evalControl( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - nav2_core::GoalChecker * goal_checker); + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** * @brief Get the trajectories generated in a cycle for visualization @@ -156,7 +157,7 @@ class Optimizer const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - nav2_core::GoalChecker * goal_checker); + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** * @brief Obtain the main controller's parameters @@ -278,10 +279,11 @@ class Optimizer std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; + geometry_msgs::msg::Pose goal_; Eigen::ArrayXf costs_; CriticData critics_data_ = { - state_, generated_trajectories_, path_, + state_, generated_trajectories_, path_, goal_, costs_, settings_.model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index b076bc42c8b..468a2bd7b2d 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -132,11 +132,14 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( transformed_plan.poses.push_back(costmap_plan_pose); } + //TODO: add goal here + geometry_msgs::msg::Pose goal; + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 80b1e89546a..013b29de502 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -183,9 +183,10 @@ std::tuple Optimizer::evalCon const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal_checker); + prepare(robot_pose, robot_speed, plan, goal, goal_checker); Eigen::ArrayXXf optimal_trajectory; bool trajectory_valid = true; @@ -193,7 +194,7 @@ std::tuple Optimizer::evalCon optimize(); optimal_trajectory = getOptimizedTrajectory(); switch (trajectory_validator_->validateTrajectory( - optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan)) + optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal)) { case mppi::ValidationResult::SOFT_RESET: trajectory_valid = false; @@ -251,6 +252,7 @@ void Optimizer::prepare( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { state_.pose = robot_pose; @@ -258,6 +260,7 @@ void Optimizer::prepare( state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan); path_ = utils::toTensor(plan); costs_.setZero(); + goal_ = goal; critics_data_.fail_flag = false; critics_data_.goal_checker = goal_checker; diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index b89029f4602..ff0116ed7de 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -110,10 +110,11 @@ TEST(CriticManagerTests, BasicCriticOperations) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.fail_flag = true; diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 370fe9dcfcd..88e823019a7 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -75,10 +75,11 @@ TEST(CriticTests, ConstraintsCritic) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); @@ -226,7 +227,7 @@ TEST(CriticTests, GoalAngleCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); @@ -289,7 +290,7 @@ TEST(CriticTests, GoalCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); @@ -345,7 +346,7 @@ TEST(CriticTests, PathAngleCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -466,7 +467,7 @@ TEST(CriticTests, PreferForwardCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -525,7 +526,7 @@ TEST(CriticTests, TwirlingCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -592,7 +593,7 @@ TEST(CriticTests, PathFollowCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -646,7 +647,7 @@ TEST(CriticTests, PathAlignCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -763,7 +764,7 @@ TEST(CriticTests, VelocityDeadbandCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index a417ec7882a..c88422f3a84 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -77,9 +77,10 @@ TEST_P(OptimizerSuite, OptimizerTest) { auto pose = getDummyPointStamped(node, start_pose); auto velocity = getDummyTwist(); auto path = getIncrementalDummyPath(node, path_settings); + auto goal = path.poses.back().pose; nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker); EXPECT_GT(trajectory.rows(), 0); EXPECT_GT(trajectory.cols(), 0); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 8219c76ceae..eec9ea0ec57 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -116,9 +116,10 @@ class OptimizerTester : public Optimizer const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal_checker); + prepare(robot_pose, robot_speed, plan, goal, goal_checker); EXPECT_EQ(critics_data_.goal_checker, nullptr); EXPECT_NEAR(costs_.sum(), 0, 1e-6); // should be reset @@ -392,7 +393,7 @@ TEST(OptimizerTests, PrepareTests) geometry_msgs::msg::Pose goal; path.poses.resize(17); - optimizer_tester.testPrepare(pose, speed, path, nullptr); + optimizer_tester.testPrepare(pose, speed, path, goal, nullptr); } TEST(OptimizerTests, shiftControlSequenceTests) diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 3d854adfc35..630695ebe99 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -176,12 +176,13 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) models::Trajectories generated_trajectories; generated_trajectories.reset(100, 2); models::Path path; + geometry_msgs::msg::Pose goal; path.reset(10); Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Attempt to set furthest point if notionally set, should not change @@ -191,7 +192,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) // Attempt to set if not set already with no other information, should fail CriticData data2 = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references setPathFurthestPointIfNotSet(data2); EXPECT_EQ(data2.furthest_reached_path_point, 0); @@ -210,7 +211,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) path = toTensor(plan); CriticData data3 = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5); } @@ -220,12 +221,13 @@ TEST(UtilsTests, findPathCosts) models::State state; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; path.reset(50); Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Test not set if already set, should not change @@ -238,7 +240,7 @@ TEST(UtilsTests, findPathCosts) EXPECT_EQ(data.path_pts_valid->size(), 10u); CriticData data3 = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references auto costmap_ros = std::make_shared( From 29932a1de4d04572eebfc386d426becdec62f6b6 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 4 Oct 2025 13:04:27 +0000 Subject: [PATCH 21/62] New idea Signed-off-by: mini-1235 --- .../include/nav2_controller/path_handler.hpp | 7 +++ nav2_controller/src/controller_server.cpp | 4 +- nav2_controller/src/path_handler.cpp | 18 ++++++ nav2_core/include/nav2_core/controller.hpp | 4 +- .../include/dwb_core/dwb_local_planner.hpp | 7 ++- .../dwb_core/include/dwb_core/publisher.hpp | 2 - .../dwb_core/src/dwb_local_planner.cpp | 42 ++------------ .../dwb_core/src/publisher.cpp | 14 ----- nav2_graceful_controller/README.md | 1 - .../graceful_controller.hpp | 5 +- .../src/graceful_controller.cpp | 55 ++++--------------- .../test/test_graceful_controller.cpp | 13 +++-- nav2_mppi_controller/README.md | 1 - .../benchmark/controller_benchmark.cpp | 3 +- .../nav2_mppi_controller/controller.hpp | 6 +- .../tools/trajectory_visualizer.hpp | 4 +- nav2_mppi_controller/src/controller.cpp | 43 ++------------- .../src/trajectory_visualizer.cpp | 12 +--- .../test/controller_state_transition_test.cpp | 3 +- .../test/trajectory_visualizer_tests.cpp | 40 ++------------ .../regulated_pure_pursuit_controller.hpp | 5 +- .../src/regulated_pure_pursuit_controller.cpp | 44 ++------------- .../nav2_rotation_shim_controller.hpp | 4 +- .../src/nav2_rotation_shim_controller.cpp | 5 +- .../test/test_shim_controller.cpp | 24 ++++---- .../controller/controller_error_plugins.hpp | 18 ++++-- 26 files changed, 123 insertions(+), 261 deletions(-) diff --git a/nav2_controller/include/nav2_controller/path_handler.hpp b/nav2_controller/include/nav2_controller/path_handler.hpp index e6545a59c5a..3bfc0d7df19 100644 --- a/nav2_controller/include/nav2_controller/path_handler.hpp +++ b/nav2_controller/include/nav2_controller/path_handler.hpp @@ -88,6 +88,13 @@ class PathHandler */ void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + /** + * @brief Get the global goal pose transformed to the local frame + * @param stamp Time to get the goal pose at + * @return Transformed goal pose + */ + geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); + protected: /** * Get the greatest extent of the costmap in meters from the center. diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ea0f6fbd46c..1f95a480a9c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -562,6 +562,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); + geometry_msgs::msg::Pose goal = path_handler_->getTransformedGoal(pose.header.stamp).pose; pruned_global_plan_ = path_handler_->pruneGlobalPlan(pose); geometry_msgs::msg::TwistStamped cmd_vel_2d; @@ -572,7 +573,8 @@ void ControllerServer::computeAndPublishVelocity() pose, twist, goal_checkers_[current_goal_checker_].get(), - pruned_global_plan_ + pruned_global_plan_, + goal ); last_valid_cmd_time_ = now(); cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); diff --git a/nav2_controller/src/path_handler.cpp b/nav2_controller/src/path_handler.cpp index 9232751a2a1..31c627d069a 100644 --- a/nav2_controller/src/path_handler.cpp +++ b/nav2_controller/src/path_handler.cpp @@ -83,6 +83,24 @@ void PathHandler::setPlan(const nav_msgs::msg::Path & path) } } +geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) +{ + auto goal = global_plan_.poses.back(); + goal.header.frame_id = global_plan_.header.frame_id; + goal.header.stamp = stamp; + if (goal.header.frame_id.empty()) { + throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); + } + geometry_msgs::msg::PoseStamped transformed_goal; + if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_, + costmap_ros_->getGlobalFrameID(), params_->transform_tolerance)) + { + throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); + } + return transformed_goal; +} + nav_msgs::msg::Path PathHandler::pruneGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 759fad89841..90d06a536dd 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -109,13 +109,15 @@ class Controller * @param velocity Current robot velocity * @param goal_checker Pointer to the current goal checker the task is utilizing * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param goal The last pose of the global plan * @return The best command for the robot to drive */ virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan) = 0; + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) = 0; /** * @brief Cancel the current control action diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 162d9f163db..09f2e1bd7a6 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -106,13 +106,15 @@ class DWBLocalPlanner : public nav2_core::Controller * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param goal The last pose of the global plan * @return The best command for the robot to drive */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & pruned_global_plan) override; + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) override; /** * @brief Score a given command. Can be used for testing. @@ -146,7 +148,8 @@ class DWBLocalPlanner : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & pose, const nav_2d_msgs::msg::Twist2D & velocity, std::shared_ptr & results, - nav_msgs::msg::Path & pruned_global_plan); + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal); /** * @brief Limits the maximum linear speed of the robot. diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp index b992719b8ad..8fa470d13c7 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp @@ -93,7 +93,6 @@ class DWBPublisher void publishCostGrid( const std::shared_ptr costmap_ros, const std::vector critics); - void publishTransformedPlan(const nav_msgs::msg::Path plan); void publishLocalPlan(const nav_msgs::msg::Path plan); protected: @@ -117,7 +116,6 @@ class DWBPublisher // Publisher Objects std::shared_ptr> eval_pub_; - std::shared_ptr> transformed_pub_; std::shared_ptr> local_pub_; std::shared_ptr> marker_pub_; std::shared_ptr> cost_grid_pc_pub_; diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index e6776f14523..53b51d6c635 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -218,7 +218,8 @@ DWBLocalPlanner::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & pruned_global_plan) + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) { std::shared_ptr results = nullptr; if (pub_->shouldRecordEvaluation()) { @@ -228,7 +229,7 @@ DWBLocalPlanner::computeVelocityCommands( try { nav_2d_msgs::msg::Twist2DStamped cmd_vel2d = computeVelocityCommands( pose, - nav_2d_utils::twist3Dto2D(velocity), results, pruned_global_plan); + nav_2d_utils::twist3Dto2D(velocity), results, pruned_global_plan, goal); pub_->publishEvaluation(results); geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.twist = nav_2d_utils::twist2Dto3D(cmd_vel2d.velocity); @@ -253,50 +254,19 @@ DWBLocalPlanner::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const nav_2d_msgs::msg::Twist2D & velocity, std::shared_ptr & results, - nav_msgs::msg::Path & pruned_global_plan) + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) { if (results) { results->header.frame_id = pose.header.frame_id; results->header.stamp = clock_->now(); } - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); - transformed_plan.header.stamp = pose.header.stamp; - - // Transform the pruned global plan to global frame - auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - - if (!nav2_util::transformPoseInTargetFrame( - stamped_pose, transformed_pose, *tf_, - transformed_plan.header.frame_id, transform_tolerance_)) - { - throw nav2_core::ControllerTFError( - "Unable to transform plan pose into local frame"); - } - - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - std::transform( - pruned_global_plan.poses.begin(), - pruned_global_plan.poses.end(), - std::back_inserter(transformed_plan.poses), - transformGlobalPlanToLocal); - pub_->publishTransformedPlan(transformed_plan); - //TODO: handle goal pose here - geometry_msgs::msg::PoseStamped goal_pose; - nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock lock(*(costmap->getMutex())); for (TrajectoryCritic::Ptr & critic : critics_) { - if (!critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan)) { + if (!critic->prepare(pose.pose, velocity, goal, pruned_global_plan)) { RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare"); } } diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index d01b3290373..4626b8f34d9 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -75,9 +75,6 @@ DWBPublisher::on_configure() declare_parameter_if_not_declared( node, plugin_name_ + ".publish_evaluation", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".publish_transformed_plan", - rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".publish_local_plan", rclcpp::ParameterValue(true)); @@ -92,13 +89,11 @@ DWBPublisher::on_configure() rclcpp::ParameterValue(0.1)); node->get_parameter(plugin_name_ + ".publish_evaluation", publish_evaluation_); - node->get_parameter(plugin_name_ + ".publish_transformed_plan", publish_transformed_); node->get_parameter(plugin_name_ + ".publish_local_plan", publish_local_plan_); node->get_parameter(plugin_name_ + ".publish_trajectories", publish_trajectories_); node->get_parameter(plugin_name_ + ".publish_cost_grid_pc", publish_cost_grid_pc_); eval_pub_ = node->create_publisher("evaluation"); - transformed_pub_ = node->create_publisher("transformed_global_plan"); local_pub_ = node->create_publisher("local_plan"); marker_pub_ = node->create_publisher("marker"); cost_grid_pc_pub_ = node->create_publisher("cost_cloud"); @@ -114,7 +109,6 @@ nav2::CallbackReturn DWBPublisher::on_activate() { eval_pub_->on_activate(); - transformed_pub_->on_activate(); local_pub_->on_activate(); marker_pub_->on_activate(); cost_grid_pc_pub_->on_activate(); @@ -126,7 +120,6 @@ nav2::CallbackReturn DWBPublisher::on_deactivate() { eval_pub_->on_deactivate(); - transformed_pub_->on_deactivate(); local_pub_->on_deactivate(); marker_pub_->on_deactivate(); cost_grid_pc_pub_->on_deactivate(); @@ -138,7 +131,6 @@ nav2::CallbackReturn DWBPublisher::on_cleanup() { eval_pub_.reset(); - transformed_pub_.reset(); local_pub_.reset(); marker_pub_.reset(); cost_grid_pc_pub_.reset(); @@ -329,12 +321,6 @@ DWBPublisher::publishCostGrid( cost_grid_pc_pub_->publish(std::move(cost_grid_pc)); } -void -DWBPublisher::publishTransformedPlan(const nav_msgs::msg::Path plan) -{ - publishGenericPlan(plan, *transformed_pub_, publish_transformed_); -} - void DWBPublisher::publishLocalPlan(const nav_msgs::msg::Path plan) { diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index fa1d4a123cc..5e5b20462f9 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -34,7 +34,6 @@ The smooth control law is a pose-following kinematic control law that generates | Topic | Type | Description | |-----|----|----| -| `transformed_global_plan` | `nav_msgs/Path` | The global plan transformed into the robot frame. | | `local_plan` | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. | | `motion_target` | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. | | `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 5f3379cce64..62c24542e2b 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -84,13 +84,15 @@ class GracefulController : public nav2_core::Controller * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param goal The last pose of the global plan * @return Best command */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan) override; + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) override; /** * @brief nav2_core newPathReceived - Receives a new plan from the Planner Server @@ -190,7 +192,6 @@ class GracefulController : public nav2_core::Controller // True from the time a new path arrives until we have completed an initial rotation bool do_initial_rotation_; - nav2::Publisher::SharedPtr transformed_path_pub_; nav2::Publisher::SharedPtr local_plan_pub_; nav2::Publisher::SharedPtr motion_target_pub_; nav2::Publisher::SharedPtr slowdown_pub_; diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 2f7571cedf7..386c98907d6 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -58,7 +58,6 @@ void GracefulController::configure( } // Publishers - transformed_path_pub_ = node->create_publisher("transformed_global_plan"); local_plan_pub_ = node->create_publisher("local_plan"); motion_target_pub_ = node->create_publisher("motion_target"); slowdown_pub_ = node->create_publisher("slowdown"); @@ -72,7 +71,6 @@ void GracefulController::cleanup() logger_, "Cleaning up controller: %s of type graceful_controller::GracefulController", plugin_name_.c_str()); - transformed_path_pub_.reset(); local_plan_pub_.reset(); motion_target_pub_.reset(); slowdown_pub_.reset(); @@ -87,7 +85,6 @@ void GracefulController::activate() logger_, "Activating controller: %s of type nav2_graceful_controller::GracefulController", plugin_name_.c_str()); - transformed_path_pub_->on_activate(); local_plan_pub_->on_activate(); motion_target_pub_->on_activate(); slowdown_pub_->on_activate(); @@ -99,7 +96,6 @@ void GracefulController::deactivate() logger_, "Deactivating controller: %s of type nav2_graceful_controller::GracefulController", plugin_name_.c_str()); - transformed_path_pub_->on_deactivate(); local_plan_pub_->on_deactivate(); motion_target_pub_->on_deactivate(); slowdown_pub_->on_deactivate(); @@ -109,7 +105,8 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & /*velocity*/, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan) + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & /*goal*/) { std::lock_guard param_lock(param_handler_->getMutex()); @@ -130,40 +127,8 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( params_->k_phi, params_->k_delta, params_->beta, params_->lambda); control_law_->setSlowdownRadius(params_->slowdown_radius); control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); - - // Transform the pruned global plan to robot base frame - auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - - if (!nav2_util::transformPoseInTargetFrame( - stamped_pose, transformed_pose, *tf_buffer_, - costmap_ros_->getBaseFrameID(), params_->transform_tolerance)) - { - throw nav2_core::ControllerTFError( - "Unable to transform plan pose into local frame"); - } - - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = pose.header.stamp; - std::transform( - pruned_global_plan.poses.begin(), - pruned_global_plan.poses.end(), - std::back_inserter(transformed_plan.poses), - transformGlobalPlanToLocal); - // Add proper orientations to plan, if needed - validateOrientations(transformed_plan.poses); - - // Publish plan for visualization - transformed_path_pub_->publish(transformed_plan); + validateOrientations(pruned_global_plan.poses); // Transform local frame to global frame to use in collision checking geometry_msgs::msg::TransformStamped costmap_transform; @@ -180,12 +145,12 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( } // Compute distance to goal as the path's integrated distance to account for path curvatures - double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(transformed_plan); + double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(pruned_global_plan); // If we've reached the XY goal tolerance, just rotate if (dist_to_goal < goal_dist_tolerance_ || goal_reached_) { goal_reached_ = true; - double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + double angle_to_goal = tf2::getYaw(pruned_global_plan.poses.back().pose.orientation); // Check for collisions between our current pose and goal pose size_t num_steps = fabs(angle_to_goal) / params_->in_place_collision_resolution; // Need to check at least the end pose @@ -221,10 +186,10 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( double dist_to_target; std::vector target_distances; - computeDistanceAlongPath(transformed_plan.poses, target_distances); + computeDistanceAlongPath(pruned_global_plan.poses, target_distances); bool is_first_iteration = true; - for (int i = transformed_plan.poses.size() - 1; i >= 0; --i) { + for (int i = pruned_global_plan.poses.size() - 1; i >= 0; --i) { if (is_first_iteration) { // Calculate target pose through lookahead interpolation to get most accurate // lookahead point, if possible @@ -232,7 +197,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // Interpolate after goal false for graceful controller // Requires interpolating the orientation which is not yet implemented // Updates dist_to_target for target_pose returned if using the point on the path - target_pose = nav2_util::getLookAheadPoint(dist_to_target, transformed_plan, false); + target_pose = nav2_util::getLookAheadPoint(dist_to_target, pruned_global_plan, false); is_first_iteration = false; } else { // Underlying control law needs a single target pose, which should: @@ -240,7 +205,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // * But no further than the max_lookahed_ distance // * Be feasible to reach in a collision free manner dist_to_target = target_distances[i]; - target_pose = transformed_plan.poses[i]; + target_pose = pruned_global_plan.poses[i]; } // Compute velocity at this moment if valid target pose is found @@ -254,7 +219,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( target_pose, params_->slowdown_radius); slowdown_pub_->publish(slowdown_marker); // Publish the local plan - local_plan.header = transformed_plan.header; + local_plan.header = pruned_global_plan.header; local_plan_pub_->publish(local_plan); // Successfully found velocity command return cmd_vel; diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 459b728b69f..3676db22df3 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -523,9 +523,9 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path pruned_global_plan; - + geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, - pruned_global_plan); + pruned_global_plan, goal); // Check results: the robot should rotate in place. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. @@ -601,8 +601,9 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path pruned_global_plan; + geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, - pruned_global_plan); + pruned_global_plan, goal); // Check results: the robot should go straight to the target. // So, linear velocity should be some positive value and angular velocity should be zero. @@ -683,8 +684,9 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path pruned_global_plan; + geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, - pruned_global_plan); + pruned_global_plan, goal); // Check results: the robot should go straight to the target. // So, both linear velocity should be some negative values. @@ -770,8 +772,9 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path pruned_global_plan; + geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, - pruned_global_plan); + pruned_global_plan, goal); // Check results: the robot should do a final rotation near the target. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index ee2fa3660df..75629aef773 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -290,7 +290,6 @@ controller_server: | Topic | Type | Description | |---------------------------|----------------------------------|-----------------------------------------------------------------------| | `trajectories` | `visualization_msgs/MarkerArray` | Randomly generated trajectories, including resulting control sequence | -| `transformed_global_plan` | `nav_msgs/Path` | Part of global plan considered by local planner | | `critics_stats` | `nav2_msgs/CriticsStats` | Statistics about each critic's performance (published when `publish_critics_stats` is enabled) | ## Notes to Users diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index 75e57edf230..025cc7f0470 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -110,8 +110,9 @@ void prepareAndRunBenchmark( nav2_core::GoalChecker * dummy_goal_checker{nullptr}; nav_msgs::msg::Path pruned_global_plan; + geometry_msgs::msg::Pose goal; for (auto _ : state) { - controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, pruned_global_plan); + controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, pruned_global_plan, goal); } map_odom_broadcaster.wait(); odom_base_link_broadcaster.wait(); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 2978521cc6e..0766ec3ba21 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -81,12 +81,14 @@ class MPPIController : public nav2_core::Controller * @param robot_speed Robot speed * @param goal_checker Pointer to the goal checker for awareness if completed task * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param goal The last pose of the global plan */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan) override; + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) override; /** * @brief Receives a new plan from the Planner Server @@ -104,12 +106,10 @@ class MPPIController : public nav2_core::Controller protected: /** * @brief Visualize trajectories - * @param transformed_plan Transformed input plan * @param cmd_stamp Command stamp * @param optimal_trajectory Optimal trajectory, if already computed */ void visualize( - nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp, const Eigen::ArrayXXf & optimal_trajectory); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp index f57d9b6b2b2..95573146a3f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp @@ -85,9 +85,8 @@ class TrajectoryVisualizer /** * @brief Visualize the plan - * @param plan Plan to visualize */ - void visualize(const nav_msgs::msg::Path & plan); + void visualize(); /** * @brief Reset object @@ -98,7 +97,6 @@ class TrajectoryVisualizer std::string frame_id_; nav2::Publisher::SharedPtr trajectories_publisher_; - nav2::Publisher::SharedPtr transformed_path_pub_; nav2::Publisher::SharedPtr optimal_path_pub_; std::unique_ptr optimal_path_; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 468a2bd7b2d..5eea8d14249 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -94,7 +94,8 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan) + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) { #ifdef BENCHMARK_TESTING auto start = std::chrono::system_clock::now(); @@ -102,44 +103,11 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( std::lock_guard param_lock(*parameters_handler_->getLock()); - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); - transformed_plan.header.stamp = robot_pose.header.stamp; - - unsigned int mx, my; - // Find the furthest relevant pose on the path to consider within costmap - // bounds - // Transforming it to the costmap frame in the same loop - for (auto global_plan_pose = pruned_global_plan.poses.begin(); - global_plan_pose != pruned_global_plan.poses.end(); - ++global_plan_pose) - { - // Transform from global plan frame to costmap frame - geometry_msgs::msg::PoseStamped costmap_plan_pose; - global_plan_pose->header.stamp = robot_pose.header.stamp; - global_plan_pose->header.frame_id = pruned_global_plan.header.frame_id; - nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_, - costmap_ros_->getGlobalFrameID(), transform_tolerance_); - - // Check if pose is inside the costmap - if (!costmap_ros_->getCostmap()->worldToMap( - costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) - { - break; - } - - // Filling the transformed plan to return with the transformed pose - transformed_plan.poses.push_back(costmap_plan_pose); - } - - //TODO: add goal here - geometry_msgs::msg::Pose goal; - nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, pruned_global_plan, goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); @@ -161,20 +129,19 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( } if (visualize_) { - visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); + visualize(cmd.header.stamp, optimal_trajectory); } return cmd; } void MPPIController::visualize( - nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp, const Eigen::ArrayXXf & optimal_trajectory) { trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); - trajectory_visualizer_.visualize(std::move(transformed_plan)); + trajectory_visualizer_.visualize(); } void MPPIController::newPathReceived(const nav_msgs::msg::Path & /*raw_global_path*/) diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 6960b5676c6..6521d3c1151 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -27,8 +27,6 @@ void TrajectoryVisualizer::on_configure( frame_id_ = frame_id; trajectories_publisher_ = node->create_publisher("~/candidate_trajectories"); - transformed_path_pub_ = node->create_publisher( - "~/transformed_global_plan"); optimal_path_pub_ = node->create_publisher("~/optimal_path"); parameters_handler_ = parameters_handler; @@ -43,21 +41,18 @@ void TrajectoryVisualizer::on_configure( void TrajectoryVisualizer::on_cleanup() { trajectories_publisher_.reset(); - transformed_path_pub_.reset(); optimal_path_pub_.reset(); } void TrajectoryVisualizer::on_activate() { trajectories_publisher_->on_activate(); - transformed_path_pub_->on_activate(); optimal_path_pub_->on_activate(); } void TrajectoryVisualizer::on_deactivate() { trajectories_publisher_->on_deactivate(); - transformed_path_pub_->on_deactivate(); optimal_path_pub_->on_deactivate(); } @@ -135,7 +130,7 @@ void TrajectoryVisualizer::reset() optimal_path_ = std::make_unique(); } -void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) +void TrajectoryVisualizer::visualize() { if (trajectories_publisher_->get_subscription_count() > 0) { trajectories_publisher_->publish(std::move(points_)); @@ -146,11 +141,6 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) } reset(); - - if (transformed_path_pub_->get_subscription_count() > 0) { - auto plan_ptr = std::make_unique(plan); - transformed_path_pub_->publish(std::move(plan_ptr)); - } } } // namespace mppi diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index d748550a963..052bd234d05 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -58,7 +58,8 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) controller->newPathReceived(path); nav_msgs::msg::Path transformed_plan; - EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}, transformed_plan)); + geometry_msgs::msg::Pose goal; + EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}, transformed_plan, goal)); controller->setSpeedLimit(0.5, true); controller->setSpeedLimit(0.5, false); diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 1228e81d60a..4f4a4c7ac4f 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -36,33 +36,6 @@ TEST(TrajectoryVisualizerTests, StateTransition) vis.on_cleanup(); } -TEST(TrajectoryVisualizerTests, VisPathRepub) -{ - auto node = std::make_shared("my_node"); - std::string name = "test"; - auto parameters_handler = std::make_unique(node, name); - nav_msgs::msg::Path received_path; - nav_msgs::msg::Path pub_path; - pub_path.header.frame_id = "fake_frame"; - pub_path.poses.resize(5); - - auto my_sub = node->create_subscription( - "~/transformed_global_plan", - [&](const nav_msgs::msg::Path msg) {received_path = msg;}); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - TrajectoryVisualizer vis; - vis.on_configure(node, "my_name", "map", parameters_handler.get()); - vis.on_activate(); - vis.visualize(pub_path); - - executor.spin_some(); - EXPECT_EQ(received_path.poses.size(), 5u); - EXPECT_EQ(received_path.header.frame_id, "fake_frame"); -} - TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) { auto node = std::make_shared("my_node"); @@ -83,8 +56,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) vis.on_activate(); builtin_interfaces::msg::Time bogus_stamp; vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); - nav_msgs::msg::Path bogus_path; - vis.visualize(bogus_path); + vis.visualize(); executor.spin_some(); EXPECT_EQ(received_msg.markers.size(), 0u); @@ -92,7 +64,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) // Now populated with content, should publish optimal_trajectory = Eigen::ArrayXXf::Ones(20, 3); vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); - vis.visualize(bogus_path); + vis.visualize(); executor.spin_some(); @@ -151,8 +123,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); vis.add(candidate_trajectories, "Candidate Trajectories"); - nav_msgs::msg::Path bogus_path; - vis.visualize(bogus_path); + vis.visualize(); executor.spin_some(); // 40 * 4, for 5 trajectory steps + 3 point steps @@ -182,8 +153,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); - nav_msgs::msg::Path bogus_path; - vis.visualize(bogus_path); + vis.visualize(); executor.spin_some(); EXPECT_EQ(received_path.poses.size(), 0u); @@ -196,7 +166,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) optimal_trajectory(i, 2) = static_cast(i); } vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); - vis.visualize(bogus_path); + vis.visualize(); executor.spin_some(); diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index e1b612794dd..1601eaec7ef 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -90,13 +90,15 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param goal The last pose of the global plan * @return Best command */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & pruned_global_plan) override; + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) override; bool cancel() override; @@ -190,7 +192,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool is_rotating_to_heading_ = false; bool has_reached_xy_tolerance_ = false; - nav2::Publisher::SharedPtr transformed_path_pub_; nav2::Publisher::SharedPtr carrot_pub_; nav2::Publisher::SharedPtr curvature_carrot_pub_; nav2::Publisher::SharedPtr is_rotating_to_heading_pub_; 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 e6d53a5993c..fade67bdb15 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 @@ -69,7 +69,6 @@ void RegulatedPurePursuitController::configure( node->get_parameter("controller_frequency", control_frequency); control_duration_ = 1.0 / control_frequency; - transformed_path_pub_ = node->create_publisher("transformed_global_plan"); carrot_pub_ = node->create_publisher("lookahead_point"); curvature_carrot_pub_ = node->create_publisher( "curvature_lookahead_point"); @@ -84,7 +83,6 @@ void RegulatedPurePursuitController::cleanup() "Cleaning up controller: %s of type" " regulated_pure_pursuit_controller::RegulatedPurePursuitController", plugin_name_.c_str()); - transformed_path_pub_.reset(); carrot_pub_.reset(); curvature_carrot_pub_.reset(); is_rotating_to_heading_pub_.reset(); @@ -97,7 +95,6 @@ void RegulatedPurePursuitController::activate() "Activating controller: %s of type " "regulated_pure_pursuit_controller::RegulatedPurePursuitController", plugin_name_.c_str()); - transformed_path_pub_->on_activate(); carrot_pub_->on_activate(); curvature_carrot_pub_->on_activate(); is_rotating_to_heading_pub_->on_activate(); @@ -110,7 +107,6 @@ void RegulatedPurePursuitController::deactivate() "Deactivating controller: %s of type " "regulated_pure_pursuit_controller::RegulatedPurePursuitController", plugin_name_.c_str()); - transformed_path_pub_->on_activate(); carrot_pub_->on_deactivate(); curvature_carrot_pub_->on_deactivate(); is_rotating_to_heading_pub_->on_deactivate(); @@ -162,7 +158,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & speed, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan) + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & /*goal*/) { std::lock_guard lock_reinit(param_handler_->getMutex()); @@ -178,41 +175,12 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity goal_dist_tol_ = pose_tolerance.position.x; } - // Transform the pruned global plan to robot base frame - auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = pruned_global_plan.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - - if (!nav2_util::transformPoseInTargetFrame( - stamped_pose, transformed_pose, *tf_, - costmap_ros_->getBaseFrameID(), params_->transform_tolerance)) - { - throw nav2_core::ControllerTFError( - "Unable to transform plan pose into local frame"); - } - - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = pose.header.stamp; - std::transform( - pruned_global_plan.poses.begin(), - pruned_global_plan.poses.end(), - std::back_inserter(transformed_plan.poses), - transformGlobalPlanToLocal); - transformed_path_pub_->publish(transformed_plan); - // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); double curv_lookahead_dist = params_->curvature_lookahead_dist; // Get the particular point on the path at the lookahead distance - auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_plan); + auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, pruned_global_plan); auto rotate_to_path_carrot_pose = carrot_pose; carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -224,7 +192,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity if (params_->use_fixed_curvature_lookahead) { auto curvature_lookahead_pose = nav2_util::getLookAheadPoint( curv_lookahead_dist, - transformed_plan, params_->interpolate_curvature_after_goal); + pruned_global_plan, params_->interpolate_curvature_after_goal); rotate_to_path_carrot_pose = curvature_lookahead_pose; regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position); curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose)); @@ -246,7 +214,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity double angle_to_heading; if (shouldRotateToGoalHeading(carrot_pose)) { is_rotating_to_heading_ = true; - double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + double angle_to_goal = tf2::getYaw(pruned_global_plan.poses.back().pose.orientation); rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); } else if (shouldRotateToPath(rotate_to_path_carrot_pose, angle_to_heading, x_vel_sign)) { is_rotating_to_heading_ = true; @@ -255,7 +223,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity is_rotating_to_heading_ = false; applyConstraints( regulation_curvature, speed, - collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, + collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), pruned_global_plan, linear_vel, x_vel_sign); if (cancelling_) { diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index 2aa0a7568da..6305e60613e 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -83,13 +83,15 @@ class RotationShimController : public nav2_core::Controller * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param goal The last pose of the global plan * @return Best command */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & pruned_global_plan) override; + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) override; /** * @brief nav2_core newPathReceived - Receives a new plan from the Planner Server diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 6872ebcf429..2fb7f4116a1 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -169,7 +169,8 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan) + nav_msgs::msg::Path & pruned_global_plan, + const geometry_msgs::msg::Pose & goal) { // Rotate to goal heading when in goal xy tolerance if (rotate_to_goal_heading_) { @@ -259,7 +260,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands // If at this point, use the primary controller to path track in_rotation_ = false; auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker, - pruned_global_plan); + pruned_global_plan, goal); last_angular_vel_ = cmd_vel.twist.angular.z; return cmd_vel; } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 6faefade0c5..f1c1233efb6 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -268,13 +268,14 @@ TEST(RotationShimControllerTest, computeVelocityTests) // send without setting a path - should go to RPP immediately // then it should throw an exception because the path is empty and invalid nav_msgs::msg::Path pruned_global_plan; - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), + geometry_msgs::msg::Pose goal; + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal), std::runtime_error); // Set with a path -- should attempt to find a sampled point but throw exception // because it cannot be found, then go to RPP and throw exception because it cannot be transformed controller->newPathReceived(path); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal), std::runtime_error); path.header.frame_id = "base_link"; @@ -291,7 +292,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // is 0.5 and that should cause a rotation in place controller->newPathReceived(path); tf_broadcaster->sendTransform(transform); - auto effort = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); + auto effort = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); path.header.frame_id = "base_link"; @@ -309,7 +310,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // and exception because it is off of the costmap controller->newPathReceived(path); tf_broadcaster->sendTransform(transform); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan), + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal), std::runtime_error); } @@ -383,12 +384,13 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { // Calculate first velocity command controller->newPathReceived(path); nav_msgs::msg::Path pruned_global_plan; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); + geometry_msgs::msg::Pose goal; + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); // Test second velocity command with wrong odometry velocity.angular.z = 1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4); } @@ -455,14 +457,15 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { controller->newPathReceived(path); nav_msgs::msg::Path pruned_global_plan; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); + geometry_msgs::msg::Pose goal; + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); // goal heading 45 degrees to the right path.poses[3].pose.orientation.z = 0.3826834; path.poses[3].pose.orientation.w = 0.9238795; controller->newPathReceived(path); - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } @@ -536,12 +539,13 @@ TEST(RotationShimControllerTest, accelerationTests) { // Test acceleration limits controller->newPathReceived(path); nav_msgs::msg::Path pruned_global_plan; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); + geometry_msgs::msg::Pose goal; + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); // Test slowing down to avoid overshooting velocity.angular.z = -1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -std::sqrt(2 * 0.5 * M_PI / 4), 1e-4); } diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp index e699d5f7283..88bf5c01770 100644 --- a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -47,7 +47,8 @@ class UnknownErrorController : public nav2_core::Controller const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, - nav_msgs::msg::Path &) + nav_msgs::msg::Path &, + const geometry_msgs::msg::Pose &) { throw nav2_core::ControllerException("Unknown Error"); } @@ -61,7 +62,8 @@ class TFErrorController : public UnknownErrorController const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, - nav_msgs::msg::Path &) + nav_msgs::msg::Path &, + const geometry_msgs::msg::Pose &) { throw nav2_core::ControllerTFError("TF error"); } @@ -73,7 +75,8 @@ class FailedToMakeProgressErrorController : public UnknownErrorController const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, - nav_msgs::msg::Path &) + nav_msgs::msg::Path &, + const geometry_msgs::msg::Pose &) { throw nav2_core::FailedToMakeProgress("Failed to make progress"); } @@ -85,7 +88,8 @@ class PatienceExceededErrorController : public UnknownErrorController const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, - nav_msgs::msg::Path &) + nav_msgs::msg::Path &, + const geometry_msgs::msg::Pose &) { throw nav2_core::PatienceExceeded("Patience exceeded"); } @@ -97,7 +101,8 @@ class InvalidPathErrorController : public UnknownErrorController const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, - nav_msgs::msg::Path &) + nav_msgs::msg::Path &, + const geometry_msgs::msg::Pose &) { throw nav2_core::InvalidPath("Invalid path"); } @@ -109,7 +114,8 @@ class NoValidControlErrorController : public UnknownErrorController const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, - nav_msgs::msg::Path &) + nav_msgs::msg::Path &, + const geometry_msgs::msg::Pose &) { throw nav2_core::NoValidControl("No valid control"); } From 013a3a464df68fd717ac79fe1aae55fb9ea23da0 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 4 Oct 2025 13:18:41 +0000 Subject: [PATCH 22/62] Rename Signed-off-by: mini-1235 --- .../nav2_controller/controller_server.hpp | 2 +- .../plugins/position_goal_checker.hpp | 2 +- .../plugins/simple_goal_checker.hpp | 2 +- .../plugins/stopped_goal_checker.hpp | 2 +- .../plugins/simple_goal_checker.cpp | 4 +- .../plugins/stopped_goal_checker.cpp | 4 +- nav2_controller/plugins/test/goal_checker.cpp | 44 +++++++++---------- nav2_controller/src/controller_server.cpp | 6 +-- nav2_core/include/nav2_core/controller.hpp | 4 +- nav2_core/include/nav2_core/goal_checker.hpp | 2 +- .../include/dwb_core/dwb_local_planner.hpp | 8 ++-- .../dwb_core/src/dwb_local_planner.cpp | 8 ++-- .../graceful_controller.hpp | 4 +- .../src/graceful_controller.cpp | 18 ++++---- .../test/test_graceful_controller.cpp | 16 +++---- .../benchmark/controller_benchmark.cpp | 4 +- .../nav2_mppi_controller/controller.hpp | 4 +- nav2_mppi_controller/src/controller.cpp | 4 +- nav2_mppi_controller/test/utils_test.cpp | 2 +- .../regulated_pure_pursuit_controller.hpp | 4 +- .../src/regulated_pure_pursuit_controller.cpp | 10 ++--- .../nav2_rotation_shim_controller.hpp | 4 +- .../src/nav2_rotation_shim_controller.cpp | 6 +-- .../test/test_shim_controller.cpp | 30 ++++++------- 24 files changed, 97 insertions(+), 97 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 2affa7911db..021cd873292 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -260,7 +260,7 @@ class ControllerServer : public nav2::LifecycleNode // Current path container nav_msgs::msg::Path current_path_; - nav_msgs::msg::Path pruned_global_plan_; + nav_msgs::msg::Path transformed_global_plan_; std::unique_ptr path_handler_; std::unique_ptr param_handler_; Parameters * params_; diff --git a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp index 39f614a5ac6..e49d03445dc 100644 --- a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp @@ -45,7 +45,7 @@ class PositionGoalChecker : public nav2_core::GoalChecker bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & transformed_global_plan) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 846a56792aa..d07e0719617 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -66,7 +66,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker void reset() override; bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & transformed_global_plan) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, geometry_msgs::msg::Twist & vel_tolerance) override; diff --git a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index bd78a6a832f..3b3ff0e768d 100644 --- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -61,7 +61,7 @@ class StoppedGoalChecker : public SimpleGoalChecker const std::shared_ptr costmap_ros) override; bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & transformed_global_plan) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, geometry_msgs::msg::Twist & vel_tolerance) override; diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 0ae3e1bf9f3..f9a7ab8e167 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -102,9 +102,9 @@ void SimpleGoalChecker::reset() bool SimpleGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path & current_path) + const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path & transformed_global_plan) { - if (nav2_util::geometry_utils::calculate_path_length(current_path) > path_length_tolerance_) { + if (nav2_util::geometry_utils::calculate_path_length(transformed_global_plan) > path_length_tolerance_) { return false; } if (check_xy_) { diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index cdfcd5e357f..92c0602cc5b 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -82,9 +82,9 @@ void StoppedGoalChecker::initialize( bool StoppedGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & transformed_global_plan) { - bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity, current_path); + bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity, transformed_global_plan); if (!ret) { return ret; } diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index b0426f46c4e..b92545fcd36 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -71,11 +71,11 @@ void checkMacro( v.linear.y = yv; v.angular.z = thetav; - nav_msgs::msg::Path current_path; + nav_msgs::msg::Path transformed_global_plan; if (expected_result) { - EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v, current_path)); + EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v, transformed_global_plan)); } else { - EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v, current_path)); + EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v, transformed_global_plan)); } } @@ -257,24 +257,24 @@ TEST(StoppedGoalChecker, is_reached) // Current linear x position is tolerance away from goal current_pose.position.x = 0.25; velocity.linear.x = 0.25; - nav_msgs::msg::Path current_path_; - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + nav_msgs::msg::Path transformed_global_plan_; + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); // Current linear x speed exceeds tolerance velocity.linear.x = 0.25 + std::numeric_limits::epsilon(); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); // Current linear x position is further than tolerance away from goal current_pose.position.x = 0.25 + std::numeric_limits::epsilon(); velocity.linear.x = 0.25; - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); current_pose.position.x = 0.0; @@ -285,16 +285,16 @@ TEST(StoppedGoalChecker, is_reached) current_pose.position.y = 0.25 / std::sqrt(2); velocity.linear.x = 0.25 / std::sqrt(2); velocity.linear.y = 0.25 / std::sqrt(2); - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); // Current linear speed exceeds tolerance velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); @@ -303,8 +303,8 @@ TEST(StoppedGoalChecker, is_reached) current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); velocity.linear.x = 0.25 / std::sqrt(2); velocity.linear.y = 0.25 / std::sqrt(2); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); @@ -323,15 +323,15 @@ TEST(StoppedGoalChecker, is_reached) current_pose.orientation.z = quat[2]; current_pose.orientation.w = quat[3]; velocity.angular.z = 0.25; - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); // Current angular speed exceeds tolerance velocity.angular.z = 0.25 + std::numeric_limits::epsilon(); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); @@ -339,8 +339,8 @@ TEST(StoppedGoalChecker, is_reached) current_pose.orientation.z = quat_epsilon[2]; current_pose.orientation.w = quat_epsilon[3]; velocity.angular.z = 0.25; - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, current_path_)); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); } int main(int argc, char ** argv) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 1f95a480a9c..2c07efa1b97 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -563,7 +563,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); geometry_msgs::msg::Pose goal = path_handler_->getTransformedGoal(pose.header.stamp).pose; - pruned_global_plan_ = path_handler_->pruneGlobalPlan(pose); + transformed_global_plan_ = path_handler_->pruneGlobalPlan(pose); geometry_msgs::msg::TwistStamped cmd_vel_2d; @@ -573,7 +573,7 @@ void ControllerServer::computeAndPublishVelocity() pose, twist, goal_checkers_[current_goal_checker_].get(), - pruned_global_plan_, + transformed_global_plan_, goal ); last_valid_cmd_time_ = now(); @@ -744,7 +744,7 @@ bool ControllerServer::isGoalReached() return goal_checkers_[current_goal_checker_]->isGoalReached( pose.pose, transformed_end_pose.pose, - velocity, pruned_global_plan_); + velocity, transformed_global_plan_); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 90d06a536dd..e57aedb5968 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -108,7 +108,7 @@ class Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Pointer to the current goal checker the task is utilizing - * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @param goal The last pose of the global plan * @return The best command for the robot to drive */ @@ -116,7 +116,7 @@ class Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) = 0; /** diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp index b43dec159c0..9e206a1bfc2 100644 --- a/nav2_core/include/nav2_core/goal_checker.hpp +++ b/nav2_core/include/nav2_core/goal_checker.hpp @@ -87,7 +87,7 @@ class GoalChecker const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist & velocity, - const nav_msgs::msg::Path & current_path) = 0; + const nav_msgs::msg::Path & transformed_global_plan) = 0; /** * @brief Get the maximum possible tolerances used for goal checking in the major types. diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 09f2e1bd7a6..e6e429243fa 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -105,7 +105,7 @@ class DWBLocalPlanner : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands - * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @param goal The last pose of the global plan * @return The best command for the robot to drive */ @@ -113,7 +113,7 @@ class DWBLocalPlanner : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) override; /** @@ -141,14 +141,14 @@ class DWBLocalPlanner : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param results Output param, if not NULL, will be filled in with full evaluation results - * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @return Best command */ virtual nav_2d_msgs::msg::Twist2DStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const nav_2d_msgs::msg::Twist2D & velocity, std::shared_ptr & results, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal); /** diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 53b51d6c635..7c090d0c39e 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -218,7 +218,7 @@ DWBLocalPlanner::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) { std::shared_ptr results = nullptr; @@ -229,7 +229,7 @@ DWBLocalPlanner::computeVelocityCommands( try { nav_2d_msgs::msg::Twist2DStamped cmd_vel2d = computeVelocityCommands( pose, - nav_2d_utils::twist3Dto2D(velocity), results, pruned_global_plan, goal); + nav_2d_utils::twist3Dto2D(velocity), results, transformed_global_plan, goal); pub_->publishEvaluation(results); geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.twist = nav_2d_utils::twist2Dto3D(cmd_vel2d.velocity); @@ -254,7 +254,7 @@ DWBLocalPlanner::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const nav_2d_msgs::msg::Twist2D & velocity, std::shared_ptr & results, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) { if (results) { @@ -266,7 +266,7 @@ DWBLocalPlanner::computeVelocityCommands( std::unique_lock lock(*(costmap->getMutex())); for (TrajectoryCritic::Ptr & critic : critics_) { - if (!critic->prepare(pose.pose, velocity, goal, pruned_global_plan)) { + if (!critic->prepare(pose.pose, velocity, goal, transformed_global_plan)) { RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare"); } } diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 62c24542e2b..a2c7852af0d 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -83,7 +83,7 @@ class GracefulController : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands - * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @param goal The last pose of the global plan * @return Best command */ @@ -91,7 +91,7 @@ class GracefulController : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) override; /** diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 386c98907d6..c73149b1ab2 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -105,7 +105,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & /*velocity*/, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & /*goal*/) { std::lock_guard param_lock(param_handler_->getMutex()); @@ -128,7 +128,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( control_law_->setSlowdownRadius(params_->slowdown_radius); control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); // Add proper orientations to plan, if needed - validateOrientations(pruned_global_plan.poses); + validateOrientations(transformed_global_plan.poses); // Transform local frame to global frame to use in collision checking geometry_msgs::msg::TransformStamped costmap_transform; @@ -145,12 +145,12 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( } // Compute distance to goal as the path's integrated distance to account for path curvatures - double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(pruned_global_plan); + double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(transformed_global_plan); // If we've reached the XY goal tolerance, just rotate if (dist_to_goal < goal_dist_tolerance_ || goal_reached_) { goal_reached_ = true; - double angle_to_goal = tf2::getYaw(pruned_global_plan.poses.back().pose.orientation); + double angle_to_goal = tf2::getYaw(transformed_global_plan.poses.back().pose.orientation); // Check for collisions between our current pose and goal pose size_t num_steps = fabs(angle_to_goal) / params_->in_place_collision_resolution; // Need to check at least the end pose @@ -186,10 +186,10 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( double dist_to_target; std::vector target_distances; - computeDistanceAlongPath(pruned_global_plan.poses, target_distances); + computeDistanceAlongPath(transformed_global_plan.poses, target_distances); bool is_first_iteration = true; - for (int i = pruned_global_plan.poses.size() - 1; i >= 0; --i) { + for (int i = transformed_global_plan.poses.size() - 1; i >= 0; --i) { if (is_first_iteration) { // Calculate target pose through lookahead interpolation to get most accurate // lookahead point, if possible @@ -197,7 +197,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // Interpolate after goal false for graceful controller // Requires interpolating the orientation which is not yet implemented // Updates dist_to_target for target_pose returned if using the point on the path - target_pose = nav2_util::getLookAheadPoint(dist_to_target, pruned_global_plan, false); + target_pose = nav2_util::getLookAheadPoint(dist_to_target, transformed_global_plan, false); is_first_iteration = false; } else { // Underlying control law needs a single target pose, which should: @@ -205,7 +205,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // * But no further than the max_lookahed_ distance // * Be feasible to reach in a collision free manner dist_to_target = target_distances[i]; - target_pose = pruned_global_plan.poses[i]; + target_pose = transformed_global_plan.poses[i]; } // Compute velocity at this moment if valid target pose is found @@ -219,7 +219,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( target_pose, params_->slowdown_radius); slowdown_pub_->publish(slowdown_marker); // Publish the local plan - local_plan.header = pruned_global_plan.header; + local_plan.header = transformed_global_plan.header; local_plan_pub_->publish(local_plan); // Successfully found velocity command return cmd_vel; diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 3676db22df3..6026a3a0350 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -522,10 +522,10 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path pruned_global_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, - pruned_global_plan, goal); + transformed_global_plan, goal); // Check results: the robot should rotate in place. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. @@ -600,10 +600,10 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path pruned_global_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, - pruned_global_plan, goal); + transformed_global_plan, goal); // Check results: the robot should go straight to the target. // So, linear velocity should be some positive value and angular velocity should be zero. @@ -683,10 +683,10 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path pruned_global_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, - pruned_global_plan, goal); + transformed_global_plan, goal); // Check results: the robot should go straight to the target. // So, both linear velocity should be some negative values. @@ -771,10 +771,10 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path pruned_global_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, - pruned_global_plan, goal); + transformed_global_plan, goal); // Check results: the robot should do a final rotation near the target. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index 025cc7f0470..77f985b77a8 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -109,10 +109,10 @@ void prepareAndRunBenchmark( controller->newPathReceived(path); nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - nav_msgs::msg::Path pruned_global_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; for (auto _ : state) { - controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, pruned_global_plan, goal); + controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, transformed_global_plan, goal); } map_odom_broadcaster.wait(); odom_base_link_broadcaster.wait(); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 0766ec3ba21..eda97f4ead4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -80,14 +80,14 @@ class MPPIController : public nav2_core::Controller * @param robot_pose Robot pose * @param robot_speed Robot speed * @param goal_checker Pointer to the goal checker for awareness if completed task - * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @param goal The last pose of the global plan */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) override; /** diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 5eea8d14249..e12361e88dd 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -94,7 +94,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) { #ifdef BENCHMARK_TESTING @@ -107,7 +107,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, pruned_global_plan, goal, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_global_plan, goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 630695ebe99..c444b685c8c 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -42,7 +42,7 @@ class TestGoalChecker : public nav2_core::GoalChecker const geometry_msgs::msg::Pose & /*query_pose*/, const geometry_msgs::msg::Pose & /*goal_pose*/, const geometry_msgs::msg::Twist & /*velocity*/, - const nav_msgs::msg::Path & /*current_path*/) {return false;} + const nav_msgs::msg::Path & /*transformed_global_plan*/) {return false;} virtual bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 1601eaec7ef..53953b9f5b1 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -89,7 +89,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands - * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @param goal The last pose of the global plan * @return Best command */ @@ -97,7 +97,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) override; bool cancel() override; 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 fade67bdb15..11b04341fb7 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 @@ -158,7 +158,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & speed, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & /*goal*/) { std::lock_guard lock_reinit(param_handler_->getMutex()); @@ -180,7 +180,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity double curv_lookahead_dist = params_->curvature_lookahead_dist; // Get the particular point on the path at the lookahead distance - auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, pruned_global_plan); + auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_global_plan); auto rotate_to_path_carrot_pose = carrot_pose; carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -192,7 +192,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity if (params_->use_fixed_curvature_lookahead) { auto curvature_lookahead_pose = nav2_util::getLookAheadPoint( curv_lookahead_dist, - pruned_global_plan, params_->interpolate_curvature_after_goal); + transformed_global_plan, params_->interpolate_curvature_after_goal); rotate_to_path_carrot_pose = curvature_lookahead_pose; regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position); curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose)); @@ -214,7 +214,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity double angle_to_heading; if (shouldRotateToGoalHeading(carrot_pose)) { is_rotating_to_heading_ = true; - double angle_to_goal = tf2::getYaw(pruned_global_plan.poses.back().pose.orientation); + double angle_to_goal = tf2::getYaw(transformed_global_plan.poses.back().pose.orientation); rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); } else if (shouldRotateToPath(rotate_to_path_carrot_pose, angle_to_heading, x_vel_sign)) { is_rotating_to_heading_ = true; @@ -223,7 +223,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity is_rotating_to_heading_ = false; applyConstraints( regulation_curvature, speed, - collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), pruned_global_plan, + collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_global_plan, linear_vel, x_vel_sign); if (cancelling_) { diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index 6305e60613e..65a08d549d1 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -82,7 +82,7 @@ class RotationShimController : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands - * @param pruned_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap * @param goal The last pose of the global plan * @return Best command */ @@ -90,7 +90,7 @@ class RotationShimController : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) override; /** diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 2fb7f4116a1..54bfe6a33c7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -169,7 +169,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, - nav_msgs::msg::Path & pruned_global_plan, + nav_msgs::msg::Path & transformed_global_plan, const geometry_msgs::msg::Pose & goal) { // Rotate to goal heading when in goal xy tolerance @@ -192,7 +192,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands position_goal_checker_->setXYGoalTolerance(pose_tolerance.position.x); if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity, - pruned_global_plan)) + transformed_global_plan)) { double pose_yaw = tf2::getYaw(pose.pose.orientation); double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation); @@ -260,7 +260,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands // If at this point, use the primary controller to path track in_rotation_ = false; auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker, - pruned_global_plan, goal); + transformed_global_plan, goal); last_angular_vel_ = cmd_vel.twist.angular.z; return cmd_vel; } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index f1c1233efb6..709b008a7d7 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -112,7 +112,7 @@ TEST(RotationShimControllerTest, lifecycleTransitions) ctrl->cleanup(); } -TEST(RotationShimControllerTest, newPathReceivedAndSampledPointsTests) +TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); @@ -267,15 +267,15 @@ TEST(RotationShimControllerTest, computeVelocityTests) // send without setting a path - should go to RPP immediately // then it should throw an exception because the path is empty and invalid - nav_msgs::msg::Path pruned_global_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal), + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal), std::runtime_error); // Set with a path -- should attempt to find a sampled point but throw exception // because it cannot be found, then go to RPP and throw exception because it cannot be transformed controller->newPathReceived(path); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal), + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal), std::runtime_error); path.header.frame_id = "base_link"; @@ -292,7 +292,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // is 0.5 and that should cause a rotation in place controller->newPathReceived(path); tf_broadcaster->sendTransform(transform); - auto effort = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); + auto effort = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); path.header.frame_id = "base_link"; @@ -310,7 +310,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // and exception because it is off of the costmap controller->newPathReceived(path); tf_broadcaster->sendTransform(transform); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal), + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal), std::runtime_error); } @@ -383,14 +383,14 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { // Calculate first velocity command controller->newPathReceived(path); - nav_msgs::msg::Path pruned_global_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); // Test second velocity command with wrong odometry velocity.angular.z = 1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4); } @@ -456,16 +456,16 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[3].header.frame_id = "base_link"; controller->newPathReceived(path); - nav_msgs::msg::Path pruned_global_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); // goal heading 45 degrees to the right path.poses[3].pose.orientation.z = 0.3826834; path.poses[3].pose.orientation.w = 0.9238795; controller->newPathReceived(path); - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } @@ -538,14 +538,14 @@ TEST(RotationShimControllerTest, accelerationTests) { // Test acceleration limits controller->newPathReceived(path); - nav_msgs::msg::Path pruned_global_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); // Test slowing down to avoid overshooting velocity.angular.z = -1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, pruned_global_plan, goal); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -std::sqrt(2 * 0.5 * M_PI / 4), 1e-4); } From a9dffa03ca8108ab811129d7bb6774221e9d25d7 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 4 Oct 2025 15:16:29 +0000 Subject: [PATCH 23/62] Use new api Signed-off-by: mini-1235 --- .../include/nav2_controller/path_handler.hpp | 67 ------------- nav2_controller/src/parameter_handler.cpp | 94 +++++-------------- nav2_controller/src/path_handler.cpp | 5 +- nav2_core/include/nav2_core/controller.hpp | 2 +- nav2_core/include/nav2_core/goal_checker.hpp | 1 + .../include/dwb_core/dwb_local_planner.hpp | 4 +- .../dwb_core/include/dwb_core/publisher.hpp | 8 +- .../graceful_controller.hpp | 2 +- .../nav2_mppi_controller/controller.hpp | 2 +- .../test/controller_state_transition_test.cpp | 4 +- .../regulated_pure_pursuit_controller.hpp | 2 +- .../nav2_rotation_shim_controller.hpp | 2 +- .../include/nav2_util/controller_utils.hpp | 67 +++++++++++++ nav2_util/test/test_controller_utils.cpp | 63 +++++++++++++ 14 files changed, 168 insertions(+), 155 deletions(-) diff --git a/nav2_controller/include/nav2_controller/path_handler.hpp b/nav2_controller/include/nav2_controller/path_handler.hpp index 3bfc0d7df19..bef92dabeb8 100644 --- a/nav2_controller/include/nav2_controller/path_handler.hpp +++ b/nav2_controller/include/nav2_controller/path_handler.hpp @@ -102,73 +102,6 @@ class PathHandler */ double getCostmapMaxExtent() const; - /** - * @brief Find the iterator of the first pose at which there is an inversion on the path, - * @param path to check for inversion - * @return the first point after the inversion found in the path - */ - inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) - { - // At least 3 poses for a possible inversion - if (path.poses.size() < 3) { - return path.poses.size(); - } - - // Iterating through the path to determine the position of the path inversion - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - float oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - float oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - float ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - float ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existence of cusp, in the path, using the dot product. - float dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0f) { - return idx + 1; - } - - if ( - (hypot(oa_x, oa_y) == 0.0 && - path.poses[idx - 1].pose.orientation != - path.poses[idx].pose.orientation) - || - (hypot(ab_x, ab_y) == 0.0 && - path.poses[idx].pose.orientation != - path.poses[idx + 1].pose.orientation)) - { - // returning the distance since the points overlap - // but are not simply duplicate points (e.g. in place rotation) - return idx + 1; - } - } - - return path.poses.size(); - } - - /** - * @brief Find and remove poses after the first inversion in the path - * @param path to check for inversion - * @return The location of the inversion, return 0 if none exist - */ - inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) - { - nav_msgs::msg::Path cropped_path = path; - const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); - if (first_after_inversion == path.poses.size()) { - return 0u; - } - - cropped_path.poses.erase( - cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); - path = cropped_path; - return first_after_inversion; - } - std::shared_ptr tf_; std::shared_ptr costmap_ros_; nav_msgs::msg::Path global_plan_; diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 65ebac0426b..5eb2a0ae98e 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -35,90 +35,39 @@ ParameterHandler::ParameterHandler( node_ = node; logger_ = logger; - declare_parameter_if_not_declared( - node, "controller_frequency", rclcpp::ParameterValue(20.0)); - declare_parameter_if_not_declared( - node, "transform_tolerance", rclcpp::ParameterValue(0.1)); - - declare_parameter_if_not_declared( - node, "min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); - declare_parameter_if_not_declared( - node, "min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); - declare_parameter_if_not_declared( - node, "min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001)); - - declare_parameter_if_not_declared( - node, "speed_limit_topic", rclcpp::ParameterValue("speed_limit")); - - declare_parameter_if_not_declared( - node, "failure_tolerance", rclcpp::ParameterValue(0.0)); - declare_parameter_if_not_declared( - node, "use_realtime_priority", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, "publish_zero_velocity", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, "costmap_update_timeout", rclcpp::ParameterValue(0.30)); - - declare_parameter_if_not_declared( - node, "odom_topic", rclcpp::ParameterValue("odom")); - declare_parameter_if_not_declared( - node, "odom_duration", rclcpp::ParameterValue(0.3)); - declare_parameter_if_not_declared( - node, "interpolate_curvature_after_goal", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, "max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_size_x / 2.0)); - declare_parameter_if_not_declared( - node, "prune_distance", rclcpp::ParameterValue(1.5)); - declare_parameter_if_not_declared( - node, "enforce_path_inversion", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, "inversion_xy_tolerance", rclcpp::ParameterValue(0.2)); - declare_parameter_if_not_declared( - node, "inversion_yaw_tolerance", rclcpp::ParameterValue(0.4)); - - declare_parameter_if_not_declared( - node, "progress_checker_plugins", rclcpp::ParameterValue(default_progress_checker_ids_)); - declare_parameter_if_not_declared( - node, "goal_checker_plugins", rclcpp::ParameterValue(default_goal_checker_ids_)); - declare_parameter_if_not_declared( - node, "controller_plugins", rclcpp::ParameterValue(default_controller_ids_)); - - node->get_parameter("controller_frequency", params_.controller_frequency); - node->get_parameter("transform_tolerance", params_.transform_tolerance); - node->get_parameter("min_x_velocity_threshold", params_.min_x_velocity_threshold); - node->get_parameter("min_y_velocity_threshold", params_.min_y_velocity_threshold); - node->get_parameter("min_theta_velocity_threshold", params_.min_theta_velocity_threshold); + params_.controller_frequency = node->declare_or_get_parameter("controller_frequency", 20.0); + params_.transform_tolerance = node->declare_or_get_parameter("transform_tolerance", 0.1); + params_.min_x_velocity_threshold = node->declare_or_get_parameter("min_x_velocity_threshold", 0.0001); + params_.min_y_velocity_threshold = node->declare_or_get_parameter("min_y_velocity_threshold", 0.0001); + params_.min_theta_velocity_threshold = node->declare_or_get_parameter("min_theta_velocity_threshold", 0.0001); + params_.speed_limit_topic = node->declare_or_get_parameter("speed_limit_topic", std::string("speed_limit")); + params_.failure_tolerance = node->declare_or_get_parameter("failure_tolerance", 0.0); + params_.use_realtime_priority = node->declare_or_get_parameter("use_realtime_priority", false); + params_.publish_zero_velocity = node->declare_or_get_parameter("publish_zero_velocity", true); + params_.costmap_update_timeout = node->declare_or_get_parameter("costmap_update_timeout", 0.30); + params_.odom_topic = node->declare_or_get_parameter("odom_topic", std::string("odom")); + params_.odom_duration = node->declare_or_get_parameter("odom_duration", 0.3); + params_.interpolate_curvature_after_goal = node->declare_or_get_parameter("interpolate_curvature_after_goal", false); + params_.max_robot_pose_search_dist = node->declare_or_get_parameter("max_robot_pose_search_dist", costmap_size_x / 2.0); + params_.prune_distance = node->declare_or_get_parameter("prune_distance", 1.5); + params_.enforce_path_inversion = node->declare_or_get_parameter("enforce_path_inversion", false); + params_.inversion_xy_tolerance = node->declare_or_get_parameter("inversion_xy_tolerance", 0.2); + params_.inversion_yaw_tolerance = node->declare_or_get_parameter("inversion_yaw_tolerance", 0.4); RCLCPP_INFO( logger_, "Controller frequency set to %.4fHz", params_.controller_frequency); - node->get_parameter("speed_limit_topic", params_.speed_limit_topic); - node->get_parameter("odom_topic", params_.odom_topic); - node->get_parameter("odom_duration", params_.odom_duration); - - node->get_parameter("failure_tolerance", params_.failure_tolerance); - node->get_parameter("use_realtime_priority", params_.use_realtime_priority); - node->get_parameter("publish_zero_velocity", params_.publish_zero_velocity); - node->get_parameter( - "interpolate_curvature_after_goal", - params_.interpolate_curvature_after_goal); - node->get_parameter("max_robot_pose_search_dist", params_.max_robot_pose_search_dist); if (params_.max_robot_pose_search_dist < 0.0) { RCLCPP_WARN( logger_, "Max robot search distance is negative, setting to max to search" " every point on path for the closest value."); params_.max_robot_pose_search_dist = std::numeric_limits::max(); } - node->get_parameter("prune_distance", params_.prune_distance); - node->get_parameter("costmap_update_timeout", params_.costmap_update_timeout); - node->get_parameter("enforce_path_inversion", params_.enforce_path_inversion); - node->get_parameter("inversion_xy_tolerance", params_.inversion_xy_tolerance); - node->get_parameter("inversion_yaw_tolerance", params_.inversion_yaw_tolerance); RCLCPP_INFO(logger_, "getting progress checker plugins.."); - node->get_parameter("progress_checker_plugins", params_.progress_checker_ids); + params_.progress_checker_ids = node->declare_or_get_parameter("progress_checker_plugins", default_progress_checker_ids_); if (params_.progress_checker_ids == default_progress_checker_ids_) { for (size_t i = 0; i < default_progress_checker_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( @@ -128,7 +77,7 @@ ParameterHandler::ParameterHandler( } RCLCPP_INFO(logger_, "getting goal checker plugins.."); - node->get_parameter("goal_checker_plugins", params_.goal_checker_ids); + params_.goal_checker_ids = node->declare_or_get_parameter("goal_checker_plugins", default_goal_checker_ids_); if (params_.goal_checker_ids == default_goal_checker_ids_) { for (size_t i = 0; i < default_goal_checker_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( @@ -137,7 +86,8 @@ ParameterHandler::ParameterHandler( } } - node->get_parameter("controller_plugins", params_.controller_ids); + RCLCPP_INFO(logger_, "getting controller plugins.."); + params_.controller_ids = node->declare_or_get_parameter("controller_plugins", default_controller_ids_); if (params_.controller_ids == default_controller_ids_) { for (size_t i = 0; i < default_controller_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( diff --git a/nav2_controller/src/path_handler.cpp b/nav2_controller/src/path_handler.cpp index 31c627d069a..25f653b58f6 100644 --- a/nav2_controller/src/path_handler.cpp +++ b/nav2_controller/src/path_handler.cpp @@ -23,6 +23,7 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/controller_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "angles/angles.h" @@ -79,7 +80,7 @@ void PathHandler::setPlan(const nav_msgs::msg::Path & path) global_plan_ = path; global_plan_up_to_inversion_ = global_plan_; if(params_->enforce_path_inversion) { - inversion_locale_ = removePosesAfterFirstInversion(global_plan_up_to_inversion_); + inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); } } @@ -176,7 +177,7 @@ nav_msgs::msg::Path PathHandler::pruneGlobalPlan( if (isWithinInversionTolerances(robot_pose)) { prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); global_plan_up_to_inversion_ = global_plan_; - inversion_locale_ = removePosesAfterFirstInversion(global_plan_up_to_inversion_); + inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); } } diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index e57aedb5968..d9206f8252a 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -108,7 +108,7 @@ class Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Pointer to the current goal checker the task is utilizing - * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The global plan after being processed by the path handler * @param goal The last pose of the global plan * @return The best command for the robot to drive */ diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp index 9e206a1bfc2..2fde1fad80a 100644 --- a/nav2_core/include/nav2_core/goal_checker.hpp +++ b/nav2_core/include/nav2_core/goal_checker.hpp @@ -81,6 +81,7 @@ class GoalChecker * @param query_pose The pose to check * @param goal_pose The pose to check against * @param velocity The robot's current velocity + * @param transformed_global_plan The global plan after being processed by the path handler * @return True if goal is reached */ virtual bool isGoalReached( diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index e6e429243fa..cefdb83fcc2 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -105,7 +105,7 @@ class DWBLocalPlanner : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands - * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The global plan after being processed by the path handler * @param goal The last pose of the global plan * @return The best command for the robot to drive */ @@ -141,7 +141,7 @@ class DWBLocalPlanner : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param results Output param, if not NULL, will be filled in with full evaluation results - * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The global plan after being processed by the path handler * @return Best command */ virtual nav_2d_msgs::msg::Twist2DStamped computeVelocityCommands( diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp index 8fa470d13c7..75cdee9ad41 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp @@ -60,10 +60,9 @@ namespace dwb_core * * Right now, it can publish * 1) The Local Plan (after it is calculated) - * 2) The Transformed Global Plan (since it may be different than the global) - * 3) The Full LocalPlanEvaluation - * 4) Markers representing the different trajectories evaluated - * 5) The CostGrid (in the form of a complex PointCloud2) + * 2) The Full LocalPlanEvaluation + * 3) Markers representing the different trajectories evaluated + * 4) The CostGrid (in the form of a complex PointCloud2) */ class DWBPublisher { @@ -105,7 +104,6 @@ class DWBPublisher // Flags for turning on/off publishing specific components bool publish_evaluation_; - bool publish_transformed_; bool publish_local_plan_; bool publish_trajectories_; bool publish_cost_grid_pc_; diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index a2c7852af0d..6241df6cd2d 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -83,7 +83,7 @@ class GracefulController : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands - * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The global plan after being processed by the path handler * @param goal The last pose of the global plan * @return Best command */ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index eda97f4ead4..b28375b3c75 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -80,7 +80,7 @@ class MPPIController : public nav2_core::Controller * @param robot_pose Robot pose * @param robot_speed Robot speed * @param goal_checker Pointer to the goal checker for awareness if completed task - * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The global plan after being processed by the path handler * @param goal The last pose of the global plan */ geometry_msgs::msg::TwistStamped computeVelocityCommands( diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index 052bd234d05..00525445ebd 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -57,9 +57,9 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) pose.header.frame_id = costmap_ros->getGlobalFrameID(); controller->newPathReceived(path); - nav_msgs::msg::Path transformed_plan; + nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}, transformed_plan, goal)); + EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}, transformed_global_plan, goal)); controller->setSpeedLimit(0.5, true); controller->setSpeedLimit(0.5, false); diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 53953b9f5b1..cb3cbf1dabe 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -89,7 +89,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands - * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The global plan after being processed by the path handler * @param goal The last pose of the global plan * @return Best command */ diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index 65a08d549d1..b9021de9265 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -82,7 +82,7 @@ class RotationShimController : public nav2_core::Controller * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands - * @param transformed_global_plan The pruned portion of the global plan, bounded around the robot's position and within the local costmap + * @param transformed_global_plan The global plan after being processed by the path handler * @param goal The last pose of the global plan * @return Best command */ diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp index be8c88c0013..8b19f984e0d 100644 --- a/nav2_util/include/nav2_util/controller_utils.hpp +++ b/nav2_util/include/nav2_util/controller_utils.hpp @@ -62,6 +62,73 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint( double &, const nav_msgs::msg::Path &, const bool interpolate_after_goal = false); +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ +inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existence of cusp, in the path, using the dot product. + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0f) { + return idx + 1; + } + + if ( + (hypot(oa_x, oa_y) == 0.0 && + path.poses[idx - 1].pose.orientation != + path.poses[idx].pose.orientation) + || + (hypot(ab_x, ab_y) == 0.0 && + path.poses[idx].pose.orientation != + path.poses[idx + 1].pose.orientation)) + { + // returning the distance since the points overlap + // but are not simply duplicate points (e.g. in place rotation) + return idx + 1; + } + } + + return path.poses.size(); +} + +/** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; +} + } // namespace nav2_util #endif // NAV2_UTIL__CONTROLLER_UTILS_HPP_ diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp index e6117db8e11..2141ddba9b4 100644 --- a/nav2_util/test/test_controller_utils.cpp +++ b/nav2_util/test/test_controller_utils.cpp @@ -387,3 +387,66 @@ TEST(InterpolationUtils, lookaheadExtrapolation) EXPECT_NEAR(pt.pose.position.x, 2.0 + cos(-45.0 * M_PI / 180) * 10.0, EPSILON); EXPECT_NEAR(pt.pose.position.y, -2.0 + sin(-45.0 * M_PI / 180) * 10.0, EPSILON); } + +TEST(UtilsTests, FindPathInversionTest) +{ + // Straight path, no inversions to be found + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 10u); + + // To short to process + path.poses.erase(path.poses.begin(), path.poses.begin() + 7); + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 3u); + + // Has inversion at index 10, so should return 11 for the first point afterwards + // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 + path.poses.clear(); + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 11u); +} + +TEST(UtilsTests, RemovePosesAfterPathInversionTest) +{ + nav_msgs::msg::Path path; + // straight path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u); + + // try empty path + path.poses.clear(); + EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u); + + // cusping path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 11u); + // Check to see if removed + EXPECT_EQ(path.poses.size(), 11u); + EXPECT_EQ(path.poses.back().pose.position.x, 10); +} \ No newline at end of file From 6f3171c02d87eb3c658769f11d0f989841b58b53 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 5 Oct 2025 13:13:37 +0000 Subject: [PATCH 24/62] Make path handler a controller server's plugin Signed-off-by: mini-1235 --- nav2_behavior_tree/CMakeLists.txt | 3 + .../plugins/action/follow_path_action.hpp | 1 + .../action/path_handler_selector_node.hpp | 110 +++++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 7 + .../plugins/action/follow_path_action.cpp | 9 + .../action/path_handler_selector_node.cpp | 104 ++++++++ nav2_controller/CMakeLists.txt | 24 +- .../nav2_controller/controller_server.hpp | 25 +- .../nav2_controller/parameter_handler.hpp | 12 +- .../include/nav2_controller/path_handler.hpp | 115 --------- .../plugins/position_goal_checker.hpp | 3 +- .../plugins/simple_goal_checker.hpp | 3 +- .../plugins/simple_path_handler.hpp | 86 +++++++ .../plugins/stopped_goal_checker.hpp | 3 +- nav2_controller/plugins.xml | 5 + .../plugins/pose_progress_checker.cpp | 5 +- .../plugins/position_goal_checker.cpp | 11 +- .../plugins/simple_goal_checker.cpp | 26 +- .../plugins/simple_path_handler.cpp | 233 ++++++++++++++++++ .../plugins/simple_progress_checker.cpp | 12 +- .../plugins/stopped_goal_checker.cpp | 16 +- nav2_controller/src/controller_server.cpp | 109 +++++++- nav2_controller/src/parameter_handler.cpp | 74 +++--- nav2_controller/src/path_handler.cpp | 191 -------------- nav2_core/README.md | 1 + nav2_core/include/nav2_core/path_handler.hpp | 72 ++++++ .../include/dwb_core/dwb_local_planner.hpp | 1 - .../benchmark/controller_benchmark.cpp | 3 +- .../test/controller_state_transition_test.cpp | 3 +- nav2_msgs/action/FollowPath.action | 1 + .../src/regulated_pure_pursuit_controller.cpp | 4 +- .../test/test_shim_controller.cpp | 30 ++- .../include/nav2_rviz_plugins/selector.hpp | 3 + nav2_rviz_plugins/src/selector.cpp | 20 +- nav2_util/test/test_controller_utils.cpp | 2 +- 35 files changed, 901 insertions(+), 426 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/path_handler_selector_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/path_handler_selector_node.cpp delete mode 100644 nav2_controller/include/nav2_controller/path_handler.hpp create mode 100644 nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp create mode 100644 nav2_controller/plugins/simple_path_handler.cpp delete mode 100644 nav2_controller/src/path_handler.cpp create mode 100644 nav2_core/include/nav2_core/path_handler.hpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 1827fb5665b..3f6b6db20fc 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -246,6 +246,9 @@ list(APPEND plugin_libs nav2_goal_checker_selector_bt_node) add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp) list(APPEND plugin_libs nav2_progress_checker_selector_bt_node) +add_library(nav2_path_handler_selector_bt_node SHARED plugins/action/path_handler_selector_node.cpp) +list(APPEND plugin_libs nav2_path_handler_selector_bt_node) + add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp) list(APPEND plugin_libs nav2_goal_updated_controller_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index 1dc46f19e9f..492dbd8d8a5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -96,6 +96,7 @@ class FollowPathAction : public BtActionNode BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), BT::InputPort("progress_checker_id", ""), + BT::InputPort("path_handler_id", ""), BT::OutputPort( "error_code_id", "The follow path error code"), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/path_handler_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/path_handler_selector_node.hpp new file mode 100644 index 00000000000..be86f55d6a0 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/path_handler_selector_node.hpp @@ -0,0 +1,110 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PATH_HANDLER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PATH_HANDLER_SELECTOR_NODE_HPP_ + +#include +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp/action_node.h" + +#include "nav2_ros_common/lifecycle_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The PathHandlerSelector behavior is used to switch the path handler + * of the controller server. It subscribes to a topic "path_handler_selector" + * to get the decision about what path_handler must be used. It is usually used before + * the FollowPath. The selected_path_handler output port is passed to path_handler_id + * input port of the FollowPath + * @note This is an Asynchronous node. It will re-initialize when halted. + */ +class PathHandlerSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::PathHandlerSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + PathHandlerSelector( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "default_path_handler", + "the default path_handler to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "path_handler_selector", + "the input topic name to select the path_handler"), + + BT::OutputPort( + "selected_path_handler", + "Selected path_handler by subscription") + }; + } + +private: + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** + * @brief Function to create ROS interfaces + */ + void createROSInterfaces(); + + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the path_handler_selector topic + * + * @param msg the message with the id of the path_handler_selector + */ + void callbackPathHandlerSelect(const std_msgs::msg::String::SharedPtr msg); + + nav2::Subscription::SharedPtr path_handler_selector_sub_; + + std::string last_selected_path_handler_; + + nav2::LifecycleNode::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::string topic_name_; + std::chrono::milliseconds bt_loop_duration_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PATH_HANDLER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 7161f6419ce..84d6f149843 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -180,6 +180,7 @@ Path to follow Goal checker Progress checker + Path handler Action server name Server timeout Follow Path error code @@ -257,6 +258,12 @@ Name of the selected progress checker received from the topic subscription + + Name of the topic to receive path handler selection commands + Default path handler of the controller selector + Name of the selected path handler received from the topic subscription + + Spin distance Allowed time for spinning diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index e89a23e282a..4184e68bf4c 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -34,6 +34,7 @@ void FollowPathAction::on_tick() getInput("controller_id", goal_.controller_id); getInput("goal_checker_id", goal_.goal_checker_id); getInput("progress_checker_id", goal_.progress_checker_id); + getInput("path_handler_id", goal_.path_handler_id); } BT::NodeStatus FollowPathAction::on_success() @@ -101,6 +102,14 @@ void FollowPathAction::on_wait_for_result( goal_.progress_checker_id = new_progress_checker_id; goal_updated_ = true; } + + std::string new_path_handler_id; + getInput("path_handler_id", new_path_handler_id); + + if (goal_.path_handler_id != new_path_handler_id) { + goal_.path_handler_id = new_path_handler_id; + goal_updated_ = true; + } } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/path_handler_selector_node.cpp b/nav2_behavior_tree/plugins/action/path_handler_selector_node.cpp new file mode 100644 index 00000000000..47e230f666c --- /dev/null +++ b/nav2_behavior_tree/plugins/action/path_handler_selector_node.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/path_handler_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +PathHandlerSelector::PathHandlerSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + initialize(); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); +} + +void PathHandlerSelector::initialize() +{ + createROSInterfaces(); +} + +void PathHandlerSelector::createROSInterfaces() +{ + std::string topic_new; + getInput("topic_name", topic_new); + if (topic_new != topic_name_ || !path_handler_selector_sub_) { + topic_name_ = topic_new; + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + path_handler_selector_sub_ = node_->create_subscription( + topic_name_, + std::bind(&PathHandlerSelector::callbackPathHandlerSelect, this, _1), + nav2::qos::LatchedSubscriptionQoS(), + callback_group_); + } +} + +BT::NodeStatus PathHandlerSelector::tick() +{ + if (!BT::isStatusActive(status())) { + initialize(); + } + + callback_group_executor_.spin_all(bt_loop_duration_); + + // This behavior always use the last selected path_handler received from the topic input. + // When no input is specified it uses the default path handler. + // If the default path handler is not specified then we work in "required path handler mode": + // In this mode, the behavior returns failure if the path handler selection is not received from + // the topic input. + if (last_selected_path_handler_.empty()) { + std::string default_path_handler; + getInput("default_path_handler", default_path_handler); + if (default_path_handler.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_path_handler_ = default_path_handler; + } + } + + setOutput("selected_path_handler", last_selected_path_handler_); + + return BT::NodeStatus::SUCCESS; +} + +void +PathHandlerSelector::callbackPathHandlerSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_path_handler_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("PathHandlerSelector"); +} diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 93c58d62881..bc67cdd0671 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -27,7 +27,6 @@ set(library_name ${executable_name}_core) add_library(${library_name} SHARED src/parameter_handler.cpp - src/path_handler.cpp src/controller_server.cpp ) target_include_directories(${library_name} @@ -170,6 +169,26 @@ target_link_libraries(position_goal_checker PRIVATE pluginlib::pluginlib ) +add_library(simple_path_handler SHARED plugins/simple_path_handler.cpp) +target_include_directories(simple_path_handler + PUBLIC + "$" + "$" +) +target_link_libraries(simple_path_handler PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_ros_common::nav2_ros_common + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + angles::angles + ${rcl_interfaces_TARGETS} +) +target_link_libraries(simple_path_handler PRIVATE + nav2_util::nav2_util_core + pluginlib::pluginlib +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -186,7 +205,7 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name} +install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker simple_path_handler ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -207,6 +226,7 @@ ament_export_libraries(simple_progress_checker simple_goal_checker stopped_goal_checker position_goal_checker + simple_path_handler ${library_name}) ament_export_dependencies( geometry_msgs diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 021cd873292..64c86f76f06 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -25,6 +25,7 @@ #include "nav2_core/controller.hpp" #include "nav2_core/progress_checker.hpp" #include "nav2_core/goal_checker.hpp" +#include "nav2_core/path_handler.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "tf2_ros/transform_listener.hpp" #include "nav2_msgs/action/follow_path.hpp" @@ -36,7 +37,6 @@ #include "nav2_util/twist_publisher.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_controller/path_handler.hpp" #include "nav2_controller/parameter_handler.hpp" namespace nav2_controller @@ -54,6 +54,7 @@ class ControllerServer : public nav2::LifecycleNode using ControllerMap = std::unordered_map; using GoalCheckerMap = std::unordered_map; using ProgressCheckerMap = std::unordered_map; + using PathHandlerMap = std::unordered_map; /** * @brief Constructor for nav2_controller::ControllerServer @@ -155,6 +156,15 @@ class ControllerServer : public nav2::LifecycleNode */ bool findProgressCheckerId(const std::string & c_name, std::string & name); + /** + * @brief Find the valid path handler ID name for the specified parameter + * + * @param c_name The path handler name + * @param name Reference to the name to use for path handling if any valid available + * @return bool Whether it found a valid path handler to use + */ + bool findPathHandlerId(const std::string & c_name, std::string & name); + /** * @brief Assigns path to controller * @param path Path received from action server @@ -222,6 +232,13 @@ class ControllerServer : public nav2::LifecycleNode return twist_thresh; } + /** + * @brief Get the global goal pose transformed to the local frame + * @param stamp Time to get the goal pose at + * @return Transformed goal pose + */ + geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); + // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; @@ -250,6 +267,11 @@ class ControllerServer : public nav2::LifecycleNode ControllerMap controllers_; std::string controller_ids_concat_, current_controller_; + // Path Handler Plugins + pluginlib::ClassLoader path_handler_loader_; + PathHandlerMap path_handlers_; + std::string path_handler_ids_concat_, current_path_handler_; + rclcpp::Duration costmap_update_timeout_; // Whether we've published the single controller warning yet @@ -261,7 +283,6 @@ class ControllerServer : public nav2::LifecycleNode // Current path container nav_msgs::msg::Path current_path_; nav_msgs::msg::Path transformed_global_plan_; - std::unique_ptr path_handler_; std::unique_ptr param_handler_; Parameters * params_; diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index 440867bfc88..90cbc1da995 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -44,18 +44,14 @@ struct Parameters double costmap_update_timeout; std::string odom_topic; double odom_duration; - bool interpolate_curvature_after_goal; - double max_robot_pose_search_dist; - double prune_distance; - bool enforce_path_inversion; - float inversion_xy_tolerance; - float inversion_yaw_tolerance; std::vector progress_checker_ids; std::vector progress_checker_types; std::vector goal_checker_ids; std::vector goal_checker_types; std::vector controller_ids; std::vector controller_types; + std::vector path_handler_ids; + std::vector path_handler_types; }; /** @@ -70,7 +66,7 @@ class ParameterHandler */ ParameterHandler( nav2::LifecycleNode::SharedPtr node, - const rclcpp::Logger & logger, const double costmap_size_x); + const rclcpp::Logger & logger); /** * @brief Destrructor for nav2_controller::ParameterHandler @@ -106,6 +102,8 @@ class ParameterHandler const std::vector default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}; const std::vector default_controller_ids_{"FollowPath"}; const std::vector default_controller_types_{"dwb_core::DWBLocalPlanner"}; + const std::vector default_path_handler_ids_{"path_handler"}; + const std::vector default_path_handler_types_{"nav2_controller::SimplePathHandler"}; }; } // namespace nav2_controller diff --git a/nav2_controller/include/nav2_controller/path_handler.hpp b/nav2_controller/include/nav2_controller/path_handler.hpp deleted file mode 100644 index bef92dabeb8..00000000000 --- a/nav2_controller/include/nav2_controller/path_handler.hpp +++ /dev/null @@ -1,115 +0,0 @@ -// Copyright (c) 2022 Samsung Research America -// -// 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_CONTROLLER__PATH_HANDLER_HPP_ -#define NAV2_CONTROLLER__PATH_HANDLER_HPP_ - -#include -#include -#include -#include -#include - -#include "nav2_ros_common/lifecycle_node.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/footprint_collision_checker.hpp" -#include "nav2_util/odometry_utils.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav2_core/controller_exceptions.hpp" -#include "nav2_controller/parameter_handler.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" - -namespace nav2_controller -{ -using PathIterator = std::vector::iterator; - -/** - * @class nav2_controller::PathHandler - * @brief Handles input paths to prune them within the local costmap - */ -class PathHandler -{ -public: - /** - * @brief Constructor for nav2_controller::PathHandler - */ - PathHandler( - Parameters * params, - std::shared_ptr tf, - std::shared_ptr costmap_ros); - - /** - * @brief Destructor for nav2_controller::PathHandler - */ - ~PathHandler() = default; - - /** - * @brief Prunes global plan, bounded around the robot's position and within the local costmap - * @param pose pose to transform - * @return Path after pruned - */ - nav_msgs::msg::Path pruneGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose); - - /** - * @brief Set new reference plan - * @param Path Path to use - */ - void setPlan(const nav_msgs::msg::Path & path); - - /** - * @brief Get reference plan - * @return Path - */ - nav_msgs::msg::Path getPlan() {return global_plan_;} - - /** - * @brief Check if the robot pose is within the set inversion tolerances - * @param robot_pose Robot's current pose to check - * @return bool If the robot pose is within the set inversion tolerances - */ - bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); - - /** - * @brief Prune a path to only interesting portions - * @param plan Plan to prune - * @param end Final path iterator - */ - void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); - - /** - * @brief Get the global goal pose transformed to the local frame - * @param stamp Time to get the goal pose at - * @return Transformed goal pose - */ - geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); - -protected: - /** - * Get the greatest extent of the costmap in meters from the center. - * @return max of distance from center in meters to edge of costmap - */ - double getCostmapMaxExtent() const; - - std::shared_ptr tf_; - std::shared_ptr costmap_ros_; - nav_msgs::msg::Path global_plan_; - nav_msgs::msg::Path global_plan_up_to_inversion_; - unsigned int inversion_locale_{0u}; - Parameters * params_; -}; - -} // namespace nav2_controller - -#endif // NAV2_CONTROLLER__PATH_HANDLER_HPP_ diff --git a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp index e49d03445dc..fb1f8241e60 100644 --- a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp @@ -45,7 +45,8 @@ class PositionGoalChecker : public nav2_core::GoalChecker bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & transformed_global_plan) override; + const geometry_msgs::msg::Twist & velocity, + const nav_msgs::msg::Path & transformed_global_plan) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index d07e0719617..826391992f5 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -66,7 +66,8 @@ class SimpleGoalChecker : public nav2_core::GoalChecker void reset() override; bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & transformed_global_plan) override; + const geometry_msgs::msg::Twist & velocity, + const nav_msgs::msg::Path & transformed_global_plan) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, geometry_msgs::msg::Twist & vel_tolerance) override; diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp new file mode 100644 index 00000000000..208e5c1177d --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -0,0 +1,86 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// 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_CONTROLLER__PLUGINS__SIMPLE_PATH_HANDLER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PATH_HANDLER_HPP_ + +#include +#include +#include "nav2_core/path_handler.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +namespace nav2_controller +{ +using PathIterator = std::vector::iterator; +/** +* @class PoseProgressChecker +* @brief This plugin is used to check the position and the angle of the robot to make sure +* that it is actually progressing or rotating towards a goal. +*/ + +class SimplePathHandler : public nav2_core::PathHandler +{ +public: + void initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros, + std::shared_ptr tf) override; + void setPlan(const nav_msgs::msg::Path & path) override; + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) override; + +protected: + /** + * Get the greatest extent of the costmap in meters from the center. + * @return max of distance from center in meters to edge of costmap + */ + double getCostmapMaxExtent() const; + + /** + * @brief Check if the robot pose is within the set inversion tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the set inversion tolerances + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); + + /** + * @brief Prune a path to only interesting portions + * @param plan Plan to prune + * @param end Final path iterator + */ + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::string plugin_name_; + std::shared_ptr tf_; + std::shared_ptr costmap_ros_; + nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; + unsigned int inversion_locale_{0u}; + bool interpolate_curvature_after_goal_, enforce_path_inversion_; + double max_robot_pose_search_dist_, transform_tolerance_; + float inversion_xy_tolerance_, inversion_yaw_tolerance_; + + /** + * @brief Callback executed when a parameter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); +}; +} // namespace nav2_controller + +#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_PATH_HANDLER_HPP_ diff --git a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index 3b3ff0e768d..303fed07625 100644 --- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -61,7 +61,8 @@ class StoppedGoalChecker : public SimpleGoalChecker const std::shared_ptr costmap_ros) override; bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & transformed_global_plan) override; + const geometry_msgs::msg::Twist & velocity, + const nav_msgs::msg::Path & transformed_global_plan) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, geometry_msgs::msg::Twist & vel_tolerance) override; diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index e3e9c7cf63b..1b3d382f1c6 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -24,4 +24,9 @@ Goal checker that only checks XY position and ignores orientation + + + Checks if distance between current and previous pose is above a threshold + + diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index a891104cac0..f5a5f828a6d 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -39,9 +39,8 @@ void PoseProgressChecker::initialize( SimpleProgressChecker::initialize(parent, plugin_name); auto node = parent.lock(); - nav2::declare_parameter_if_not_declared( - node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5)); - node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5); + required_movement_angle_ = node->declare_or_get_parameter(plugin_name + + ".required_movement_angle", 0.5); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index 776347d20cb..4f78118e659 100644 --- a/nav2_controller/plugins/position_goal_checker.cpp +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -41,15 +41,8 @@ void PositionGoalChecker::initialize( plugin_name_ = plugin_name; auto node = parent.lock(); - nav2::declare_parameter_if_not_declared( - node, - plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25)); - nav2::declare_parameter_if_not_declared( - node, - plugin_name + ".stateful", rclcpp::ParameterValue(true)); - - node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); - node->get_parameter(plugin_name + ".stateful", stateful_); + xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25); + stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index f9a7ab8e167..ccc470f072e 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -70,23 +70,11 @@ void SimpleGoalChecker::initialize( plugin_name_ = plugin_name; auto node = parent.lock(); - nav2::declare_parameter_if_not_declared( - node, - plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25)); - nav2::declare_parameter_if_not_declared( - node, - plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25)); - nav2::declare_parameter_if_not_declared( - node, - plugin_name + ".path_length_tolerance", rclcpp::ParameterValue(1.0)); - nav2::declare_parameter_if_not_declared( - node, - plugin_name + ".stateful", rclcpp::ParameterValue(true)); - - node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); - node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); - node->get_parameter(plugin_name + ".yaw_goal_tolerance", path_length_tolerance_); - node->get_parameter(plugin_name + ".stateful", stateful_); + xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25); + yaw_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".yaw_goal_tolerance", 0.25); + path_length_tolerance_ = node->declare_or_get_parameter(plugin_name + ".path_length_tolerance", + 1.0); + stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; @@ -104,7 +92,9 @@ bool SimpleGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path & transformed_global_plan) { - if (nav2_util::geometry_utils::calculate_path_length(transformed_global_plan) > path_length_tolerance_) { + if (nav2_util::geometry_utils::calculate_path_length(transformed_global_plan) > + path_length_tolerance_) + { return false; } if (check_xy_) { diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp new file mode 100644 index 00000000000..0bc7c0cc8af --- /dev/null +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -0,0 +1,233 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// 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 "angles/angles.h" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_controller/plugins/simple_path_handler.hpp" +#include "nav2_util/controller_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" + + +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace nav2_controller +{ +using nav2_util::geometry_utils::euclidean_distance; + +void SimplePathHandler::initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros, + std::shared_ptr tf) +{ + plugin_name_ = plugin_name; + auto node = parent.lock(); + costmap_ros_ = costmap_ros; + tf_ = tf; + interpolate_curvature_after_goal_ = node->declare_or_get_parameter(plugin_name + + ".interpolate_curvature_after_goal", false); + max_robot_pose_search_dist_ = node->declare_or_get_parameter(plugin_name + + ".max_robot_pose_search_dist", costmap_ros_->getCostmap()->getSizeInMetersX()); + enforce_path_inversion_ = node->declare_or_get_parameter(plugin_name + ".enforce_path_inversion", + false); + inversion_xy_tolerance_ = node->declare_or_get_parameter(plugin_name + ".inversion_xy_tolerance", + 0.2); + inversion_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + + ".inversion_yaw_tolerance", 0.4); + node->get_parameter("transform_tolerance", transform_tolerance_); + if (max_robot_pose_search_dist_ < 0.0) { + // RCLCPP_WARN( + // logger_, "Max robot search distance is negative, setting to max to search" + // " every point on path for the closest value."); + max_robot_pose_search_dist_ = std::numeric_limits::max(); + } + if(enforce_path_inversion_) { + inversion_locale_ = 0u; + } + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&SimplePathHandler::dynamicParametersCallback, this, _1)); +} + +double SimplePathHandler::getCostmapMaxExtent() const +{ + const double max_costmap_dim_meters = std::max( + costmap_ros_->getCostmap()->getSizeInMetersX(), + costmap_ros_->getCostmap()->getSizeInMetersY()); + return max_costmap_dim_meters / 2.0; +} + +void SimplePathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) +{ + plan.poses.erase(plan.poses.begin(), end); +} + +bool SimplePathHandler::isWithinInversionTolerances( + const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + float distance = hypotf( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + float angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= inversion_xy_tolerance_ && + fabs(angle_distance) <= inversion_yaw_tolerance_; +} + +void SimplePathHandler::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; + global_plan_up_to_inversion_ = global_plan_; + if(enforce_path_inversion_) { + inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } +} + +nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_up_to_inversion_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + if (interpolate_curvature_after_goal_ && global_plan_up_to_inversion_.poses.size() == 1) { + throw nav2_core::InvalidPath("Received plan with length of one"); + } + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, + global_plan_up_to_inversion_.header.frame_id, + transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); + } + + // Limit the search for the closest pose up to max_robot_pose_search_dist on the path + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); + + // First find the closest pose on the path to the robot + // bounded by when the path turns around (if it does) so we don't get a pose from a later + // portion of the path + auto closest_point = + nav2_util::geometry_utils::min_by( + global_plan_up_to_inversion_.poses.begin(), closest_pose_upper_bound, + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // Make sure we always have at least 2 points on the transformed plan and that we don't prune + // the global plan below 2 points in order to have always enough point to interpolate the + // end of path direction + if (global_plan_up_to_inversion_.poses.begin() != closest_pose_upper_bound && + global_plan_up_to_inversion_.poses.size() > 1 && + closest_point == std::prev(closest_pose_upper_bound)) + { + closest_point = std::prev(std::prev(closest_pose_upper_bound)); + } + + const double max_costmap_extent = getCostmapMaxExtent(); + auto pruned_plan_end = std::find_if( + closest_point, global_plan_up_to_inversion_.poses.end(), + [&](const auto & global_plan_pose) { + return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_up_to_inversion_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_, + costmap_ros_->getBaseFrameID(), transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); + } + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + closest_point, pruned_plan_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + prunePlan(global_plan_up_to_inversion_, closest_point); + + if (enforce_path_inversion_ && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(robot_pose)) { + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +rcl_interfaces::msg::SetParametersResult +SimplePathHandler::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "max_robot_pose_search_dist") { + max_robot_pose_search_dist_ = parameter.as_double(); + } else if (param_name == "inversion_xy_tolerance") { + inversion_xy_tolerance_ = parameter.as_double(); + } else if (param_name == "inversion_yaw_tolerance") { + inversion_yaw_tolerance_ = parameter.as_double(); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == "enforce_path_inversion") { + enforce_path_inversion_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimplePathHandler, nav2_core::PathHandler) diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index df3cb77df69..900c7bf584a 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -36,15 +36,9 @@ void SimpleProgressChecker::initialize( clock_ = node->get_clock(); - nav2::declare_parameter_if_not_declared( - node, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); - nav2::declare_parameter_if_not_declared( - node, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); - // Scale is set to 0 by default, so if it was not set otherwise, set to 0 - node->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); - double time_allowance_param = 0.0; - node->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); - time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); + radius_ = node->declare_or_get_parameter(plugin_name + ".required_movement_radius", 0.5); + time_allowance_ = rclcpp::Duration::from_seconds(node->declare_or_get_parameter(plugin_name + + ".movement_time_allowance", 10.0)); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index 92c0602cc5b..ee1f4f9941d 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -65,15 +65,10 @@ void StoppedGoalChecker::initialize( auto node = parent.lock(); - nav2::declare_parameter_if_not_declared( - node, - plugin_name + ".rot_stopped_velocity", rclcpp::ParameterValue(0.25)); - nav2::declare_parameter_if_not_declared( - node, - plugin_name + ".trans_stopped_velocity", rclcpp::ParameterValue(0.25)); - - node->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_); - node->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_); + rot_stopped_velocity_ = node->declare_or_get_parameter(plugin_name + ".rot_stopped_velocity", + 0.25); + trans_stopped_velocity_ = node->declare_or_get_parameter(plugin_name + ".trans_stopped_velocity", + 0.25); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( @@ -84,7 +79,8 @@ bool StoppedGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & transformed_global_plan) { - bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity, transformed_global_plan); + bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity, + transformed_global_plan); if (!ret) { return ret; } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2c07efa1b97..bcd603bff6c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -38,6 +38,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), lp_loader_("nav2_core", "nav2_core::Controller"), + path_handler_loader_("nav2_core", "nav2_core::PathHandler"), costmap_update_timeout_(300ms) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -53,6 +54,7 @@ ControllerServer::~ControllerServer() progress_checkers_.clear(); goal_checkers_.clear(); controllers_.clear(); + path_handlers_.clear(); costmap_thread_.reset(); } @@ -68,15 +70,13 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_thread_ = std::make_unique(costmap_ros_); try { param_handler_ = std::make_unique( - node, get_logger(), costmap_ros_->getCostmap()->getSizeInMetersX()); + node, get_logger()); params_ = param_handler_->getParams(); } catch (const std::exception & ex) { RCLCPP_FATAL(get_logger(), "%s", ex.what()); on_cleanup(state); return nav2::CallbackReturn::FAILURE; } - path_handler_ = std::make_unique( - params_, costmap_ros_->getTfBuffer(), costmap_ros_); for (size_t i = 0; i != params_->progress_checker_ids.size(); i++) { try { @@ -158,6 +158,33 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_logger(), "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); + for (size_t i = 0; i != params_->path_handler_ids.size(); i++) { + try { + nav2_core::PathHandler::Ptr path_handler = + path_handler_loader_.createUniqueInstance(params_->path_handler_types[i]); + RCLCPP_INFO( + get_logger(), "Created path handler : %s of type %s", + params_->path_handler_ids[i].c_str(), params_->path_handler_types[i].c_str()); + path_handler->initialize(node, params_->path_handler_ids[i], costmap_ros_, + costmap_ros_->getTfBuffer()); + path_handlers_.insert({params_->path_handler_ids[i], path_handler}); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + get_logger(), + "Failed to create path handler Exception: %s", ex.what()); + on_cleanup(state); + return nav2::CallbackReturn::FAILURE; + } + } + + for (size_t i = 0; i != params_->path_handler_ids.size(); i++) { + path_handler_ids_concat_ += params_->path_handler_ids[i] + std::string(" "); + } + + RCLCPP_INFO( + get_logger(), + "Controller Server has %s path handlers available.", path_handler_ids_concat_.c_str()); + odom_sub_ = std::make_unique(node, params_->odom_duration, params_->odom_topic); vel_publisher_ = std::make_unique(node, "cmd_vel"); @@ -257,6 +284,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) goal_checkers_.clear(); progress_checkers_.clear(); + path_handlers_.clear(); costmap_ros_->cleanup(); @@ -356,6 +384,51 @@ bool ControllerServer::findProgressCheckerId( return true; } +bool ControllerServer::findPathHandlerId( + const std::string & c_name, + std::string & current_path_handler) +{ + if (path_handlers_.find(c_name) == path_handlers_.end()) { + if (path_handlers_.size() == 1 && c_name.empty()) { + RCLCPP_WARN_ONCE( + get_logger(), "No path handler was specified in parameter 'current_path_handler'." + " Server will use only plugin loaded %s. " + "This warning will appear once.", path_handler_ids_concat_.c_str()); + current_path_handler = path_handlers_.begin()->first; + } else { + RCLCPP_ERROR( + get_logger(), "FollowPath called with path_handler name %s in parameter" + " 'current_path_handler', which does not exist. Available path handlers are: %s.", + c_name.c_str(), path_handler_ids_concat_.c_str()); + return false; + } + } else { + RCLCPP_DEBUG(get_logger(), "Selected path handler: %s.", c_name.c_str()); + current_path_handler = c_name; + } + + return true; +} + +geometry_msgs::msg::PoseStamped ControllerServer::getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) +{ + auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; + goal.header.stamp = stamp; + if (goal.header.frame_id.empty()) { + throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); + } + geometry_msgs::msg::PoseStamped transformed_goal; + if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *costmap_ros_->getTfBuffer(), + costmap_ros_->getGlobalFrameID(), params_->transform_tolerance)) + { + throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); + } + return transformed_goal; +} + + void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); @@ -392,6 +465,14 @@ void ControllerServer::computeControl() throw nav2_core::ControllerException("Failed to find progress checker name: " + pc_name); } + std::string ph_name = goal->path_handler_id; + std::string current_path_handler; + if(findPathHandlerId(ph_name, current_path_handler)) { + current_path_handler_ = current_path_handler; + } else { + throw nav2_core::ControllerException("Failed to find path handler name: " + ph_name); + } + setPlannerPath(goal->path); progress_checkers_[current_progress_checker_]->reset(); @@ -535,7 +616,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) throw nav2_core::InvalidPath("Path is empty."); } controllers_[current_controller_]->newPathReceived(path); - path_handler_->setPlan(path); + path_handlers_[current_path_handler_]->setPlan(path); end_pose_ = path.poses.back(); end_pose_.header.frame_id = path.header.frame_id; @@ -562,8 +643,8 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); - geometry_msgs::msg::Pose goal = path_handler_->getTransformedGoal(pose.header.stamp).pose; - transformed_global_plan_ = path_handler_->pruneGlobalPlan(pose); + geometry_msgs::msg::Pose goal = getTransformedGoal(pose.header.stamp).pose; + transformed_global_plan_ = path_handlers_[current_path_handler_]->transformGlobalPlan(pose); geometry_msgs::msg::TwistStamped cmd_vel_2d; @@ -684,6 +765,22 @@ void ControllerServer::updateGlobalPath() action_server_->terminate_current(result); return; } + std::string current_path_handler; + if (findPathHandlerId(goal->path_handler_id, current_path_handler)) { + if(current_path_handler_ != current_path_handler) { + RCLCPP_INFO( + get_logger(), "Change of path handler %s requested, resetting it", + goal->path_handler_id.c_str()); + current_path_handler_ = current_path_handler; + } + } else { + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Result::INVALID_CONTROLLER; + result->error_msg = "Terminating action, invalid path handler" + + goal->path_handler_id + " requested."; + action_server_->terminate_current(result); + return; + } setPlannerPath(goal->path); } } diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 5eb2a0ae98e..cb7b1217317 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -29,45 +29,36 @@ using rcl_interfaces::msg::ParameterType; ParameterHandler::ParameterHandler( nav2::LifecycleNode::SharedPtr node, - const rclcpp::Logger & logger, - const double costmap_size_x) + const rclcpp::Logger & logger) { node_ = node; logger_ = logger; params_.controller_frequency = node->declare_or_get_parameter("controller_frequency", 20.0); params_.transform_tolerance = node->declare_or_get_parameter("transform_tolerance", 0.1); - params_.min_x_velocity_threshold = node->declare_or_get_parameter("min_x_velocity_threshold", 0.0001); - params_.min_y_velocity_threshold = node->declare_or_get_parameter("min_y_velocity_threshold", 0.0001); - params_.min_theta_velocity_threshold = node->declare_or_get_parameter("min_theta_velocity_threshold", 0.0001); - params_.speed_limit_topic = node->declare_or_get_parameter("speed_limit_topic", std::string("speed_limit")); + params_.min_x_velocity_threshold = node->declare_or_get_parameter("min_x_velocity_threshold", + 0.0001); + params_.min_y_velocity_threshold = node->declare_or_get_parameter("min_y_velocity_threshold", + 0.0001); + params_.min_theta_velocity_threshold = + node->declare_or_get_parameter("min_theta_velocity_threshold", 0.0001); + params_.speed_limit_topic = node->declare_or_get_parameter("speed_limit_topic", + std::string("speed_limit")); params_.failure_tolerance = node->declare_or_get_parameter("failure_tolerance", 0.0); params_.use_realtime_priority = node->declare_or_get_parameter("use_realtime_priority", false); params_.publish_zero_velocity = node->declare_or_get_parameter("publish_zero_velocity", true); params_.costmap_update_timeout = node->declare_or_get_parameter("costmap_update_timeout", 0.30); params_.odom_topic = node->declare_or_get_parameter("odom_topic", std::string("odom")); params_.odom_duration = node->declare_or_get_parameter("odom_duration", 0.3); - params_.interpolate_curvature_after_goal = node->declare_or_get_parameter("interpolate_curvature_after_goal", false); - params_.max_robot_pose_search_dist = node->declare_or_get_parameter("max_robot_pose_search_dist", costmap_size_x / 2.0); - params_.prune_distance = node->declare_or_get_parameter("prune_distance", 1.5); - params_.enforce_path_inversion = node->declare_or_get_parameter("enforce_path_inversion", false); - params_.inversion_xy_tolerance = node->declare_or_get_parameter("inversion_xy_tolerance", 0.2); - params_.inversion_yaw_tolerance = node->declare_or_get_parameter("inversion_yaw_tolerance", 0.4); RCLCPP_INFO( logger_, "Controller frequency set to %.4fHz", params_.controller_frequency); - if (params_.max_robot_pose_search_dist < 0.0) { - RCLCPP_WARN( - logger_, "Max robot search distance is negative, setting to max to search" - " every point on path for the closest value."); - params_.max_robot_pose_search_dist = std::numeric_limits::max(); - } - RCLCPP_INFO(logger_, "getting progress checker plugins.."); - params_.progress_checker_ids = node->declare_or_get_parameter("progress_checker_plugins", default_progress_checker_ids_); + params_.progress_checker_ids = node->declare_or_get_parameter("progress_checker_plugins", + default_progress_checker_ids_); if (params_.progress_checker_ids == default_progress_checker_ids_) { for (size_t i = 0; i < default_progress_checker_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( @@ -77,7 +68,8 @@ ParameterHandler::ParameterHandler( } RCLCPP_INFO(logger_, "getting goal checker plugins.."); - params_.goal_checker_ids = node->declare_or_get_parameter("goal_checker_plugins", default_goal_checker_ids_); + params_.goal_checker_ids = node->declare_or_get_parameter("goal_checker_plugins", + default_goal_checker_ids_); if (params_.goal_checker_ids == default_goal_checker_ids_) { for (size_t i = 0; i < default_goal_checker_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( @@ -87,7 +79,8 @@ ParameterHandler::ParameterHandler( } RCLCPP_INFO(logger_, "getting controller plugins.."); - params_.controller_ids = node->declare_or_get_parameter("controller_plugins", default_controller_ids_); + params_.controller_ids = node->declare_or_get_parameter("controller_plugins", + default_controller_ids_); if (params_.controller_ids == default_controller_ids_) { for (size_t i = 0; i < default_controller_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( @@ -96,9 +89,21 @@ ParameterHandler::ParameterHandler( } } + RCLCPP_INFO(logger_, "getting path handler plugins.."); + params_.path_handler_ids = node->declare_or_get_parameter("path_handler_plugins", + default_path_handler_ids_); + if (params_.path_handler_ids == default_path_handler_ids_) { + for (size_t i = 0; i < default_path_handler_ids_.size(); ++i) { + nav2::declare_parameter_if_not_declared( + node, default_path_handler_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_path_handler_types_[i])); + } + } + params_.controller_types.resize(params_.controller_ids.size()); params_.goal_checker_types.resize(params_.goal_checker_ids.size()); params_.progress_checker_types.resize(params_.progress_checker_ids.size()); + params_.path_handler_types.resize(params_.path_handler_ids.size()); for (size_t i = 0; i != params_.progress_checker_ids.size(); i++) { try { @@ -132,6 +137,17 @@ ParameterHandler::ParameterHandler( } } + for (size_t i = 0; i != params_.path_handler_ids.size(); i++) { + try { + params_.path_handler_types[i] = nav2::get_plugin_type_param(node, + params_.path_handler_ids[i]); + } catch (const std::exception & ex) { + throw std::runtime_error( + std::string("Failed to get type for path handler plugins '") + + params_.path_handler_types[i] + "': " + ex.what()); + } + } + // post_set_params_handler_ = node->add_post_set_parameters_callback( // std::bind( // &ParameterHandler::updateParametersCallback, @@ -190,7 +206,7 @@ ParameterHandler::updateParametersCallback( const auto & param_name = parameter.get_name(); if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == "controller_frequency") { + if (param_name == "controller_frequency") { params_.controller_frequency = parameter.as_double(); } else if (param_name == "transform_tolerance") { params_.transform_tolerance = parameter.as_double(); @@ -206,24 +222,14 @@ ParameterHandler::updateParametersCallback( params_.costmap_update_timeout = parameter.as_double(); } else if (param_name == "odom_duration") { params_.odom_duration = parameter.as_double(); - } else if (param_name == "max_robot_pose_search_dist") { - params_.max_robot_pose_search_dist = parameter.as_double(); - } else if (param_name == "prune_distance") { - params_.prune_distance = parameter.as_double(); - } else if (param_name == "inversion_xy_tolerance") { - params_.inversion_xy_tolerance = parameter.as_double(); - } else if (param_name == "inversion_yaw_tolerance") { - params_.inversion_yaw_tolerance = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == "use_realtime_priority") { params_.use_realtime_priority = parameter.as_bool(); } else if (param_name == "publish_zero_velocity") { params_.publish_zero_velocity = parameter.as_bool(); - } else if (param_name == "enforce_path_inversion") { - params_.enforce_path_inversion = parameter.as_bool(); } - } else if (param_type == ParameterType::PARAMETER_STRING){ + } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == "speed_limit_topic") { params_.speed_limit_topic = parameter.as_string(); } else if (param_name == "odom_topic") { diff --git a/nav2_controller/src/path_handler.cpp b/nav2_controller/src/path_handler.cpp deleted file mode 100644 index 25f653b58f6..00000000000 --- a/nav2_controller/src/path_handler.cpp +++ /dev/null @@ -1,191 +0,0 @@ -// Copyright (c) 2022 Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include - -#include "nav2_controller/path_handler.hpp" -#include "nav2_core/controller_exceptions.hpp" -#include "nav2_ros_common/node_utils.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav2_util/controller_utils.hpp" -#include "nav2_util/robot_utils.hpp" -#include "angles/angles.h" - -namespace nav2_controller -{ - -using nav2_util::geometry_utils::euclidean_distance; - -PathHandler::PathHandler( - Parameters * params, - std::shared_ptr tf, - std::shared_ptr costmap_ros) -{ - params_ = params; - costmap_ros_ = costmap_ros; - tf_ = tf; - - if(params_->enforce_path_inversion) { - inversion_locale_ = 0u; - } -} - -double PathHandler::getCostmapMaxExtent() const -{ - const double max_costmap_dim_meters = std::max( - costmap_ros_->getCostmap()->getSizeInMetersX(), - costmap_ros_->getCostmap()->getSizeInMetersY()); - return max_costmap_dim_meters / 2.0; -} - -void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) -{ - plan.poses.erase(plan.poses.begin(), end); -} - -bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) -{ - // Keep full path if we are within tolerance of the inversion pose - const auto last_pose = global_plan_up_to_inversion_.poses.back(); - float distance = hypotf( - robot_pose.pose.position.x - last_pose.pose.position.x, - robot_pose.pose.position.y - last_pose.pose.position.y); - - float angle_distance = angles::shortest_angular_distance( - tf2::getYaw(robot_pose.pose.orientation), - tf2::getYaw(last_pose.pose.orientation)); - - return distance <= params_->inversion_xy_tolerance && - fabs(angle_distance) <= params_->inversion_yaw_tolerance; -} - -void PathHandler::setPlan(const nav_msgs::msg::Path & path) -{ - global_plan_ = path; - global_plan_up_to_inversion_ = global_plan_; - if(params_->enforce_path_inversion) { - inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); - } -} - -geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( - const builtin_interfaces::msg::Time & stamp) -{ - auto goal = global_plan_.poses.back(); - goal.header.frame_id = global_plan_.header.frame_id; - goal.header.stamp = stamp; - if (goal.header.frame_id.empty()) { - throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); - } - geometry_msgs::msg::PoseStamped transformed_goal; - if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_, - costmap_ros_->getGlobalFrameID(), params_->transform_tolerance)) - { - throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); - } - return transformed_goal; -} - -nav_msgs::msg::Path PathHandler::pruneGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose) -{ - if (global_plan_up_to_inversion_.poses.empty()) { - throw nav2_core::InvalidPath("Received plan with zero length"); - } - - if (params_->interpolate_curvature_after_goal && global_plan_up_to_inversion_.poses.size() == 1) { - throw nav2_core::InvalidPath("Received plan with length of one"); - } - - // let's get the pose of the robot in the frame of the plan - geometry_msgs::msg::PoseStamped robot_pose; - if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, - global_plan_up_to_inversion_.header.frame_id, - params_->transform_tolerance)) - { - throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); - } - - // Limit the search for the closest pose up to max_robot_pose_search_dist on the path - auto closest_pose_upper_bound = - nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), - params_->max_robot_pose_search_dist); - - // First find the closest pose on the path to the robot - // bounded by when the path turns around (if it does) so we don't get a pose from a later - // portion of the path - auto closest_point = - nav2_util::geometry_utils::min_by( - global_plan_up_to_inversion_.poses.begin(), closest_pose_upper_bound, - [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(robot_pose, ps); - }); - - // Make sure we always have at least 2 points on the transformed plan and that we don't prune - // the global plan below 2 points in order to have always enough point to interpolate the - // end of path direction - if (global_plan_up_to_inversion_.poses.begin() != closest_pose_upper_bound && - global_plan_up_to_inversion_.poses.size() > 1 && - closest_point == std::prev(closest_pose_upper_bound)) - { - closest_point = std::prev(std::prev(closest_pose_upper_bound)); - } - - auto pruned_plan_end = global_plan_up_to_inversion_.poses.end(); - const double max_costmap_extent = getCostmapMaxExtent(); - //TODO: For RPP and graceful we don't have prune distance, maybe we need to support two kinds of pruned_plan_end here - // by parameter? - if (1){ - pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( - closest_point, global_plan_up_to_inversion_.poses.end(), params_->prune_distance); - } - else{ - pruned_plan_end = std::find_if( - closest_point, global_plan_up_to_inversion_.poses.end(), - [&](const auto & global_plan_pose) { - return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; - }); - } - - nav_msgs::msg::Path pruned_plan; - pruned_plan.poses.insert(pruned_plan.poses.end(), - closest_point, pruned_plan_end); - pruned_plan.header = global_plan_.header; - - // Remove the portion of the global plan that we've already passed so we don't - // process it on the next iteration (this is called path pruning) - prunePlan(global_plan_up_to_inversion_, closest_point); - - if (params_->enforce_path_inversion && inversion_locale_ != 0u) { - if (isWithinInversionTolerances(robot_pose)) { - prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); - global_plan_up_to_inversion_ = global_plan_; - inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); - } - } - - if (pruned_plan.poses.empty()) { - throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); - } - - return pruned_plan; -} - -} // namespace nav2_controller diff --git a/nav2_core/README.md b/nav2_core/README.md index 0034e20e243..4fd061c0c2b 100644 --- a/nav2_core/README.md +++ b/nav2_core/README.md @@ -8,6 +8,7 @@ This package hosts the abstract interface (virtual base classes) for plugins to - goal checker (e.g. `simple_goal_checker`) - behaviors (e.g. `drive_on_heading`) - progress checker (e.g. `simple_progress_checker`) +- path handler (e.g. `simple_path_handler`) - waypoint task executor (e.g. `take_pictures`) - exceptions in planning and control diff --git a/nav2_core/include/nav2_core/path_handler.hpp b/nav2_core/include/nav2_core/path_handler.hpp new file mode 100644 index 00000000000..65be0fbd5f5 --- /dev/null +++ b/nav2_core/include/nav2_core/path_handler.hpp @@ -0,0 +1,72 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// 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_CORE__PATH_HANDLER_HPP_ +#define NAV2_CORE__PATH_HANDLER_HPP_ + +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace nav2_core +{ + +/** + * @class PathHandler + * @brief Function-object for handling the path from Planner Server + * + * This class defines the plugin interface for determining whether you have reached + * the goal state. This primarily consists of checking the relative positions of two poses + * (which are presumed to be in the same frame). It can also check the velocity, as some + * applications require that robot be stopped to be considered as having reached the goal. + */ +class PathHandler +{ +public: + typedef std::shared_ptr Ptr; + + virtual ~PathHandler() {} + + /** + * @brief Initialize any parameters from the NodeHandle + * @param parent Node pointer for grabbing parameters + */ + virtual void initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros, + std::shared_ptr tf) = 0; + + /** + * @brief Set new reference plan + * @param Path Path to use + */ + virtual void setPlan(const nav_msgs::msg::Path & path) = 0; + + /** + * @brief Prunes global plan, bounded around the robot's position and within the local costmap + * @param pose pose to transform + * @return Path after pruned + */ + virtual nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) = 0; +}; + +} // namespace nav2_core + +#endif // NAV2_CORE__PATH_HANDLER_HPP_ diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index cefdb83fcc2..d5468438ee2 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -166,7 +166,6 @@ class DWBLocalPlanner : public nav2_core::Controller } protected: - /** * @brief Iterate through all the twists and find the best one */ diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index 77f985b77a8..daddbbb473b 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -112,7 +112,8 @@ void prepareAndRunBenchmark( nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; for (auto _ : state) { - controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, transformed_global_plan, goal); + controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, transformed_global_plan, + goal); } map_odom_broadcaster.wait(); odom_base_link_broadcaster.wait(); diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index 00525445ebd..9a21c299a06 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -59,7 +59,8 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) controller->newPathReceived(path); nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}, transformed_global_plan, goal)); + EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}, transformed_global_plan, + goal)); controller->setSpeedLimit(0.5, true); controller->setSpeedLimit(0.5, false); diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 80b0a1d4efc..4992da5cf41 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -3,6 +3,7 @@ nav_msgs/Path path string controller_id string goal_checker_id string progress_checker_id +string path_handler_id --- #result definition 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 11b04341fb7..faf5b87c646 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 @@ -223,8 +223,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity is_rotating_to_heading_ = false; applyConstraints( regulation_curvature, speed, - collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_global_plan, - linear_vel, x_vel_sign); + collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), + transformed_global_plan, linear_vel, x_vel_sign); if (cancelling_) { const double & dt = control_duration_; diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 709b008a7d7..61a6ad492c6 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -269,13 +269,15 @@ TEST(RotationShimControllerTest, computeVelocityTests) // then it should throw an exception because the path is empty and invalid nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal), + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, + transformed_global_plan, goal), std::runtime_error); // Set with a path -- should attempt to find a sampled point but throw exception // because it cannot be found, then go to RPP and throw exception because it cannot be transformed controller->newPathReceived(path); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal), + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, + transformed_global_plan, goal), std::runtime_error); path.header.frame_id = "base_link"; @@ -292,7 +294,8 @@ TEST(RotationShimControllerTest, computeVelocityTests) // is 0.5 and that should cause a rotation in place controller->newPathReceived(path); tf_broadcaster->sendTransform(transform); - auto effort = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); + auto effort = controller->computeVelocityCommands(pose, velocity, &checker, + transformed_global_plan, goal); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); path.header.frame_id = "base_link"; @@ -310,7 +313,8 @@ TEST(RotationShimControllerTest, computeVelocityTests) // and exception because it is off of the costmap controller->newPathReceived(path); tf_broadcaster->sendTransform(transform); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal), + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, + transformed_global_plan, goal), std::runtime_error); } @@ -385,12 +389,14 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { controller->newPathReceived(path); nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, + transformed_global_plan, goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); // Test second velocity command with wrong odometry velocity.angular.z = 1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, + goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4); } @@ -458,14 +464,16 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { controller->newPathReceived(path); nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, + transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); // goal heading 45 degrees to the right path.poses[3].pose.orientation.z = 0.3826834; path.poses[3].pose.orientation.w = 0.9238795; controller->newPathReceived(path); - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, + goal); EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } @@ -540,12 +548,14 @@ TEST(RotationShimControllerTest, accelerationTests) { controller->newPathReceived(path); nav_msgs::msg::Path transformed_global_plan; geometry_msgs::msg::Pose goal; - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, + transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); // Test slowing down to avoid overshooting velocity.angular.z = -1.8; - cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, + goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -std::sqrt(2 * 0.5 * M_PI / 4), 1e-4); } diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp index 3e55aae4d1c..1a9dcf2b505 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp @@ -50,6 +50,7 @@ class Selector : public rviz_common::Panel rclcpp::Publisher::SharedPtr pub_goal_checker_; rclcpp::Publisher::SharedPtr pub_smoother_; rclcpp::Publisher::SharedPtr pub_progress_checker_; + rclcpp::Publisher::SharedPtr pub_path_handler_; bool plugins_loaded_ = false; bool server_failed_ = false; @@ -68,12 +69,14 @@ class Selector : public rviz_common::Panel QComboBox * goal_checker_; QComboBox * smoother_; QComboBox * progress_checker_; + QComboBox * path_handler_; void setController(); void setPlanner(); void setGoalChecker(); void setSmoother(); void setProgressChecker(); + void setPathHandler(); /* * @brief Set the selection from the combo box diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index 1fffd8ff425..c83930f02a0 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -35,6 +35,8 @@ Selector::Selector(QWidget * parent) pub_smoother_ = client_node_->create_publisher("smoother_selector", qos); pub_progress_checker_ = client_node_->create_publisher("progress_checker_selector", qos); + pub_path_handler_ = + client_node_->create_publisher("path_handler_selector", qos); main_layout_ = new QVBoxLayout; row_1_label_layout_ = new QHBoxLayout; @@ -48,6 +50,7 @@ Selector::Selector(QWidget * parent) goal_checker_ = new QComboBox; smoother_ = new QComboBox; progress_checker_ = new QComboBox; + path_handler_ = new QComboBox; main_layout_->setContentsMargins(10, 10, 10, 10); @@ -61,6 +64,8 @@ Selector::Selector(QWidget * parent) row_2_layout_->addWidget(smoother_); row_3_label_layout_->addWidget(new QLabel("Progress Checker")); row_3_layout_->addWidget(progress_checker_); + row_3_label_layout_->addWidget(new QLabel("Path Handler")); + row_3_layout_->addWidget(path_handler_); main_layout_->addLayout(row_1_label_layout_); main_layout_->addLayout(row_1_layout_); @@ -91,6 +96,10 @@ Selector::Selector(QWidget * parent) connect( progress_checker_, QOverload::of(&QComboBox::activated), this, &Selector::setProgressChecker); + + connect( + path_handler_, QOverload::of(&QComboBox::activated), this, + &Selector::setPathHandler); } Selector::~Selector() @@ -146,6 +155,11 @@ void Selector::setProgressChecker() setSelection(progress_checker_, pub_progress_checker_); } +void Selector::setPathHandler() +{ + setSelection(path_handler_, pub_path_handler_); +} + void Selector::loadPlugins() { @@ -166,11 +180,15 @@ Selector::loadPlugins() nav2_rviz_plugins::pluginLoader( client_node_, server_failed_, "controller_server", "progress_checker_plugins", progress_checker_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "path_handler_plugins", + path_handler_); if (controller_->count() > 0 && planner_->count() > 0 && goal_checker_->count() > 0 && smoother_->count() > 0 && - progress_checker_->count() > 0) + progress_checker_->count() > 0 && + path_handler_->count() > 0) { plugins_loaded_ = true; } else { diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp index 2141ddba9b4..57a206fce35 100644 --- a/nav2_util/test/test_controller_utils.cpp +++ b/nav2_util/test/test_controller_utils.cpp @@ -449,4 +449,4 @@ TEST(UtilsTests, RemovePosesAfterPathInversionTest) // Check to see if removed EXPECT_EQ(path.poses.size(), 11u); EXPECT_EQ(path.poses.back().pose.position.x, 10); -} \ No newline at end of file +} From 8773c1975f6c908b279d51f70e3f5b3cbf37f882 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Tue, 7 Oct 2025 11:09:50 +0000 Subject: [PATCH 25/62] Fix dynamic parameters handling Signed-off-by: mini-1235 --- .../nav2_controller/controller_server.hpp | 5 +---- .../nav2_controller/parameter_handler.hpp | 2 +- .../plugins/simple_path_handler.hpp | 2 ++ nav2_controller/plugins/simple_path_handler.cpp | 10 ++++++---- nav2_controller/src/controller_server.cpp | 13 ++++++++----- nav2_controller/src/parameter_handler.cpp | 16 ++++++++-------- nav2_core/include/nav2_core/controller.hpp | 3 +++ nav2_core/include/nav2_core/path_handler.hpp | 1 + 8 files changed, 30 insertions(+), 22 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 64c86f76f06..73962f0b8ed 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -239,10 +239,6 @@ class ControllerServer : public nav2::LifecycleNode */ geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); - // Dynamic parameters handler - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; - std::mutex dynamic_params_lock_; - // The controller needs a costmap node std::shared_ptr costmap_ros_; std::unique_ptr costmap_thread_; @@ -285,6 +281,7 @@ class ControllerServer : public nav2::LifecycleNode nav_msgs::msg::Path transformed_global_plan_; std::unique_ptr param_handler_; Parameters * params_; + nav2::Publisher::SharedPtr transformed_plan_pub_; private: /** diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index 90cbc1da995..bbf7942deb6 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -93,7 +93,7 @@ class ParameterHandler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_; Parameters params_; std::string plugin_name_; - rclcpp::Logger logger_ {rclcpp::get_logger("GracefulMotionController")}; + rclcpp::Logger logger_ {rclcpp::get_logger("Controller Server")}; const std::vector default_progress_checker_ids_{"progress_checker"}; const std::vector default_progress_checker_types_{ diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index 208e5c1177d..7b06a0ad53d 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -34,6 +34,7 @@ class SimplePathHandler : public nav2_core::PathHandler public: void initialize( const nav2::LifecycleNode::WeakPtr & parent, + const rclcpp::Logger & logger, const std::string & plugin_name, const std::shared_ptr costmap_ros, std::shared_ptr tf) override; @@ -64,6 +65,7 @@ class SimplePathHandler : public nav2_core::PathHandler // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + rclcpp::Logger logger_ {rclcpp::get_logger("Controller Server")}; std::string plugin_name_; std::shared_ptr tf_; std::shared_ptr costmap_ros_; diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index 0bc7c0cc8af..a8242c29c29 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -30,10 +30,12 @@ using nav2_util::geometry_utils::euclidean_distance; void SimplePathHandler::initialize( const nav2::LifecycleNode::WeakPtr & parent, + const rclcpp::Logger & logger, const std::string & plugin_name, const std::shared_ptr costmap_ros, std::shared_ptr tf) { + logger_ = logger; plugin_name_ = plugin_name; auto node = parent.lock(); costmap_ros_ = costmap_ros; @@ -41,7 +43,7 @@ void SimplePathHandler::initialize( interpolate_curvature_after_goal_ = node->declare_or_get_parameter(plugin_name + ".interpolate_curvature_after_goal", false); max_robot_pose_search_dist_ = node->declare_or_get_parameter(plugin_name + - ".max_robot_pose_search_dist", costmap_ros_->getCostmap()->getSizeInMetersX()); + ".max_robot_pose_search_dist", costmap_ros_->getCostmap()->getSizeInMetersX() / 2); enforce_path_inversion_ = node->declare_or_get_parameter(plugin_name + ".enforce_path_inversion", false); inversion_xy_tolerance_ = node->declare_or_get_parameter(plugin_name + ".inversion_xy_tolerance", @@ -50,9 +52,9 @@ void SimplePathHandler::initialize( ".inversion_yaw_tolerance", 0.4); node->get_parameter("transform_tolerance", transform_tolerance_); if (max_robot_pose_search_dist_ < 0.0) { - // RCLCPP_WARN( - // logger_, "Max robot search distance is negative, setting to max to search" - // " every point on path for the closest value."); + RCLCPP_WARN( + logger_, "Max robot search distance is negative, setting to max to search" + " every point on path for the closest value."); max_robot_pose_search_dist_ = std::numeric_limits::max(); } if(enforce_path_inversion_) { diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index bcd603bff6c..67e6acb0893 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -165,7 +165,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO( get_logger(), "Created path handler : %s of type %s", params_->path_handler_ids[i].c_str(), params_->path_handler_types[i].c_str()); - path_handler->initialize(node, params_->path_handler_ids[i], costmap_ros_, + path_handler->initialize(node, get_logger(), params_->path_handler_ids[i], costmap_ros_, costmap_ros_->getTfBuffer()); path_handlers_.insert({params_->path_handler_ids[i], path_handler}); } catch (const pluginlib::PluginlibException & ex) { @@ -188,6 +188,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) odom_sub_ = std::make_unique(node, params_->odom_duration, params_->odom_topic); vel_publisher_ = std::make_unique(node, "cmd_vel"); + transformed_plan_pub_ = node->create_publisher("transformed_global_plan"); costmap_update_timeout_ = rclcpp::Duration::from_seconds(params_->costmap_update_timeout); @@ -228,6 +229,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) it->second->activate(); } vel_publisher_->on_activate(); + transformed_plan_pub_->on_activate(); action_server_->activate(); auto node = shared_from_this(); @@ -260,9 +262,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) publishZeroVelocity(); vel_publisher_->on_deactivate(); - - remove_on_set_parameters_callback(dyn_params_handler_.get()); - dyn_params_handler_.reset(); + transformed_plan_pub_->on_deactivate(); // destroy bond connection destroyBond(); @@ -294,6 +294,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) odom_sub_.reset(); costmap_thread_.reset(); vel_publisher_.reset(); + transformed_plan_pub_.reset(); speed_limit_sub_.reset(); return nav2::CallbackReturn::SUCCESS; @@ -431,7 +432,7 @@ geometry_msgs::msg::PoseStamped ControllerServer::getTransformedGoal( void ControllerServer::computeControl() { - std::lock_guard lock(dynamic_params_lock_); + std::lock_guard lock_reinit(param_handler_->getMutex()); RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); @@ -645,6 +646,8 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Pose goal = getTransformedGoal(pose.header.stamp).pose; transformed_global_plan_ = path_handlers_[current_path_handler_]->transformGlobalPlan(pose); + auto path = std::make_unique(transformed_global_plan_); + transformed_plan_pub_->publish(std::move(path)); geometry_msgs::msg::TwistStamped cmd_vel_2d; diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index cb7b1217317..83b1a4b8633 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -148,14 +148,14 @@ ParameterHandler::ParameterHandler( } } - // post_set_params_handler_ = node->add_post_set_parameters_callback( - // std::bind( - // &ParameterHandler::updateParametersCallback, - // this, std::placeholders::_1)); - // on_set_params_handler_ = node->add_on_set_parameters_callback( - // std::bind( - // &ParameterHandler::validateParameterUpdatesCallback, - // this, std::placeholders::_1)); + post_set_params_handler_ = node->add_post_set_parameters_callback( + std::bind( + &ParameterHandler::updateParametersCallback, + this, std::placeholders::_1)); + on_set_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &ParameterHandler::validateParameterUpdatesCallback, + this, std::placeholders::_1)); } ParameterHandler::~ParameterHandler() diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index d9206f8252a..04f2c1b6dc3 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -94,6 +94,9 @@ class Controller /** * @brief local setPlan - Notifies the Controller that a new plan is received from the Planner Server * @param path The global plan + * @note This callback should only perform minimal work, such as extracting global information that may + * be of interest (e.g. reset internal states when new path received). The controller will be provided + * with the transformed and pruned plan in the local frame during computeVelocityCommands(). */ virtual void newPathReceived(const nav_msgs::msg::Path & raw_global_path) = 0; diff --git a/nav2_core/include/nav2_core/path_handler.hpp b/nav2_core/include/nav2_core/path_handler.hpp index 65be0fbd5f5..b5b1bdde0d8 100644 --- a/nav2_core/include/nav2_core/path_handler.hpp +++ b/nav2_core/include/nav2_core/path_handler.hpp @@ -48,6 +48,7 @@ class PathHandler */ virtual void initialize( const nav2::LifecycleNode::WeakPtr & parent, + const rclcpp::Logger & logger, const std::string & plugin_name, const std::shared_ptr costmap_ros, std::shared_ptr tf) = 0; From 5a873f10291233e951a3e609431137908799a219 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 8 Oct 2025 14:50:30 +0000 Subject: [PATCH 26/62] Add mppi path handler Signed-off-by: mini-1235 --- nav2_controller/CMakeLists.txt | 26 +++- .../plugins/mppi_path_handler.hpp | 66 +++++++++ .../plugins/simple_path_handler.hpp | 28 +++- nav2_controller/plugins.xml | 7 +- nav2_controller/plugins/mppi_path_handler.cpp | 128 ++++++++++++++++++ .../plugins/simple_path_handler.cpp | 40 ++++-- nav2_core/include/nav2_core/path_handler.hpp | 11 +- 7 files changed, 284 insertions(+), 22 deletions(-) create mode 100644 nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp create mode 100644 nav2_controller/plugins/mppi_path_handler.cpp diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index bc67cdd0671..bfa48e91955 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -189,6 +189,29 @@ target_link_libraries(simple_path_handler PRIVATE pluginlib::pluginlib ) +add_library(mppi_path_handler SHARED plugins/mppi_path_handler.cpp) +target_include_directories(mppi_path_handler + PUBLIC + "$" + "$" +) +target_link_libraries(mppi_path_handler PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_ros_common::nav2_ros_common + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + simple_path_handler +) +target_link_libraries(mppi_path_handler PRIVATE + angles::angles + nav2_util::nav2_util_core + pluginlib::pluginlib + tf2::tf2 +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -205,7 +228,7 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker simple_path_handler ${library_name} +install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker simple_path_handler mppi_path_handler ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -227,6 +250,7 @@ ament_export_libraries(simple_progress_checker stopped_goal_checker position_goal_checker simple_path_handler + mppi_path_handler ${library_name}) ament_export_dependencies( geometry_msgs diff --git a/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp new file mode 100644 index 00000000000..6bc0c2aebb1 --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// 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_CONTROLLER__PLUGINS__MPPI_PATH_HANDLER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__MPPI_PATH_HANDLER_HPP_ + +#include +#include +#include "nav2_controller/plugins/simple_path_handler.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +namespace nav2_controller +{ +/** +* @class MPPIPathHandler +* @brief This plugin manages the global plan by clipping it to the local +* segment relevant to the controller—typically bounded by the local costmap size +* and transforming the resulting path into the costmap's global frame. +*/ + +class MPPIPathHandler : public SimplePathHandler +{ +public: + void initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const rclcpp::Logger & logger, + const std::string & plugin_name, + const std::shared_ptr costmap_ros, + std::shared_ptr tf) override; + +protected: + /** + * @brief Get global plan within window of the local costmap size + * @param global_pose Robot pose + * @return plan transformed in the costmap's global frame and iterator to the first pose of the global + * plan (for pruning) + */ + std::pair getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose) override; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + rclcpp::Logger logger_ {rclcpp::get_logger("Controller Server")}; + std::string plugin_name_; + double prune_distance_; + + /** + * @brief Callback executed when a parameter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); +}; +} // namespace nav2_controller + +#endif // NAV2_CONTROLLER__PLUGINS__MPPI_PATH_HANDLER_HPP_ diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index 7b06a0ad53d..0c5382d6449 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -1,4 +1,6 @@ -// Copyright (c) 2025 Maurice Alexander Purnawan +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// 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. @@ -24,9 +26,10 @@ namespace nav2_controller { using PathIterator = std::vector::iterator; /** -* @class PoseProgressChecker -* @brief This plugin is used to check the position and the angle of the robot to make sure -* that it is actually progressing or rotating towards a goal. +* @class SimplePathHandler +* @brief This plugin manages the global plan by clipping it to the local +* segment relevant to the controller—typically bounded by the local costmap size +* and transforming the resulting path into the robot base frame. */ class SimplePathHandler : public nav2_core::PathHandler @@ -43,6 +46,23 @@ class SimplePathHandler : public nav2_core::PathHandler const geometry_msgs::msg::PoseStamped & pose) override; protected: + /** + * @brief Transform a pose to the global reference frame + * @param pose Current pose + * @return output poose in global reference frame + */ + geometry_msgs::msg::PoseStamped transformToGlobalPlanFrame( + const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Get global plan within window of the local costmap size + * @param global_pose Robot pose + * @return plan transformed in the robot base frame frame and iterator to the first pose of the global + * plan + */ + virtual std::pair getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose); + /** * Get the greatest extent of the costmap in meters from the center. * @return max of distance from center in meters to edge of costmap diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index 1b3d382f1c6..ea599b069de 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -26,7 +26,12 @@ - Checks if distance between current and previous pose is above a threshold + Handles the path from planner server and transform it to robot base frame + + + + + Handles the path from planner server and transform it to costmap's global frame diff --git a/nav2_controller/plugins/mppi_path_handler.cpp b/nav2_controller/plugins/mppi_path_handler.cpp new file mode 100644 index 00000000000..a43228330a3 --- /dev/null +++ b/nav2_controller/plugins/mppi_path_handler.cpp @@ -0,0 +1,128 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// 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 "angles/angles.h" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_controller/plugins/mppi_path_handler.hpp" +#include "nav2_util/controller_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" + + +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace nav2_controller +{ +using nav2_util::geometry_utils::euclidean_distance; + +void MPPIPathHandler::initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const rclcpp::Logger & logger, + const std::string & plugin_name, + const std::shared_ptr costmap_ros, + std::shared_ptr tf) +{ + logger_ = logger; + plugin_name_ = plugin_name; + SimplePathHandler::initialize(parent, logger, plugin_name, costmap_ros, tf); + auto node = parent.lock(); + prune_distance_ = node->declare_or_get_parameter(plugin_name + ".prune_distance", + 0.2); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&MPPIPathHandler::dynamicParametersCallback, this, _1)); +} + +std::pair +MPPIPathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose) +{ + auto begin = global_plan_up_to_inversion_.poses.begin(); + + // Limit the search for the closest pose up to max_robot_pose_search_dist on the path + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); + + // Find closest point to the robot + auto closest_point = nav2_util::geometry_utils::min_by( + begin, closest_pose_upper_bound, + [&global_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(global_pose, ps); + }); + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); + transformed_plan.header.stamp = global_pose.header.stamp; + + auto pruned_plan_end = + nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + + unsigned int mx, my; + // Find the furthest relevant pose on the path to consider within costmap + // bounds + // Transforming it to the costmap frame in the same loop + for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; + ++global_plan_pose) + { + // Transform from global plan frame to costmap frame + geometry_msgs::msg::PoseStamped costmap_plan_pose; + global_plan_pose->header.stamp = global_pose.header.stamp; + global_plan_pose->header.frame_id = global_plan_.header.frame_id; + nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_, + costmap_ros_->getGlobalFrameID(), transform_tolerance_); + + // Check if pose is inside the costmap + if (!costmap_ros_->getCostmap()->worldToMap( + costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) + { + return {transformed_plan, closest_point}; + } + + // Filling the transformed plan to return with the transformed pose + transformed_plan.poses.push_back(costmap_plan_pose); + } + + return {transformed_plan, closest_point}; +} + +rcl_interfaces::msg::SetParametersResult +MPPIPathHandler::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "prune_distance") { + prune_distance_ = parameter.as_double(); + } + } + } + result.successful = true; + return result; +} + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::MPPIPathHandler, nav2_core::PathHandler) diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index a8242c29c29..22c0d2a9528 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -1,4 +1,6 @@ -// Copyright (c) 2025 Maurice Alexander Purnawan +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// 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. @@ -43,7 +45,7 @@ void SimplePathHandler::initialize( interpolate_curvature_after_goal_ = node->declare_or_get_parameter(plugin_name + ".interpolate_curvature_after_goal", false); max_robot_pose_search_dist_ = node->declare_or_get_parameter(plugin_name + - ".max_robot_pose_search_dist", costmap_ros_->getCostmap()->getSizeInMetersX() / 2); + ".max_robot_pose_search_dist", getCostmapMaxExtent()); enforce_path_inversion_ = node->declare_or_get_parameter(plugin_name + ".enforce_path_inversion", false); inversion_xy_tolerance_ = node->declare_or_get_parameter(plugin_name + ".inversion_xy_tolerance", @@ -69,8 +71,8 @@ void SimplePathHandler::initialize( double SimplePathHandler::getCostmapMaxExtent() const { const double max_costmap_dim_meters = std::max( - costmap_ros_->getCostmap()->getSizeInMetersX(), - costmap_ros_->getCostmap()->getSizeInMetersY()); + costmap_ros_->getCostmap()->getSizeInCellsX(), + costmap_ros_->getCostmap()->getSizeInCellsY()); return max_costmap_dim_meters / 2.0; } @@ -105,7 +107,7 @@ void SimplePathHandler::setPlan(const nav_msgs::msg::Path & path) } } -nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( +geometry_msgs::msg::PoseStamped SimplePathHandler::transformToGlobalPlanFrame( const geometry_msgs::msg::PoseStamped & pose) { if (global_plan_up_to_inversion_.poses.empty()) { @@ -125,6 +127,13 @@ nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); } + return robot_pose; +} + +std::pair +SimplePathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose) +{ // Limit the search for the closest pose up to max_robot_pose_search_dist on the path auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( @@ -137,8 +146,8 @@ nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( auto closest_point = nav2_util::geometry_utils::min_by( global_plan_up_to_inversion_.poses.begin(), closest_pose_upper_bound, - [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(robot_pose, ps); + [&global_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(global_pose, ps); }); // Make sure we always have at least 2 points on the transformed plan and that we don't prune @@ -155,14 +164,14 @@ nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( auto pruned_plan_end = std::find_if( closest_point, global_plan_up_to_inversion_.poses.end(), [&](const auto & global_plan_pose) { - return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; + return euclidean_distance(global_plan_pose, global_pose) > max_costmap_extent; }); // Lambda to transform a PoseStamped from global frame to local auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; stamped_pose.header.frame_id = global_plan_up_to_inversion_.header.frame_id; - stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.header.stamp = global_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_, costmap_ros_->getBaseFrameID(), transform_tolerance_)) @@ -180,14 +189,23 @@ nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( std::back_inserter(transformed_plan.poses), transformGlobalPoseToLocal); transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = robot_pose.header.stamp; + transformed_plan.header.stamp = global_pose.header.stamp; + + return {transformed_plan, closest_point}; +} + +nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) +{ + geometry_msgs::msg::PoseStamped global_pose = transformToGlobalPlanFrame(pose); + auto [transformed_plan, closest_point] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); // Remove the portion of the global plan that we've already passed so we don't // process it on the next iteration (this is called path pruning) prunePlan(global_plan_up_to_inversion_, closest_point); if (enforce_path_inversion_ && inversion_locale_ != 0u) { - if (isWithinInversionTolerances(robot_pose)) { + if (isWithinInversionTolerances(global_pose)) { prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); global_plan_up_to_inversion_ = global_plan_; inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); diff --git a/nav2_core/include/nav2_core/path_handler.hpp b/nav2_core/include/nav2_core/path_handler.hpp index b5b1bdde0d8..98a03780c92 100644 --- a/nav2_core/include/nav2_core/path_handler.hpp +++ b/nav2_core/include/nav2_core/path_handler.hpp @@ -30,10 +30,10 @@ namespace nav2_core * @class PathHandler * @brief Function-object for handling the path from Planner Server * - * This class defines the plugin interface for determining whether you have reached - * the goal state. This primarily consists of checking the relative positions of two poses - * (which are presumed to be in the same frame). It can also check the velocity, as some - * applications require that robot be stopped to be considered as having reached the goal. + * This class defines the plugin interface used by the Controller Server to manage the + * path received from the Planner Server. Its primary responsibilities are pruning + * path segments the robot has already traversed and transforming the remaining, + * relevant portion of the path into the costmap's global or base frame. */ class PathHandler { @@ -60,7 +60,8 @@ class PathHandler virtual void setPlan(const nav_msgs::msg::Path & path) = 0; /** - * @brief Prunes global plan, bounded around the robot's position and within the local costmap + * @brief transform global plan to local applying constraints, + * then prune global plan * @param pose pose to transform * @return Path after pruned */ From af0aeb50ff3cdc5bb6633b2ae834a54ae0869a13 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 9 Oct 2025 17:45:38 +0000 Subject: [PATCH 27/62] Fix tests Signed-off-by: mini-1235 --- nav2_behavior_tree/nav2_tree_nodes.xml | 2 +- .../test/plugins/action/CMakeLists.txt | 2 + .../test_path_handler_selector_node.cpp | 204 ++++++++++++++++++ nav2_controller/CMakeLists.txt | 10 +- .../plugins/mppi_path_handler.hpp | 2 + .../plugins/simple_path_handler.hpp | 4 +- nav2_controller/plugins/mppi_path_handler.cpp | 2 +- .../plugins/simple_path_handler.cpp | 3 +- nav2_graceful_controller/test/CMakeLists.txt | 1 + .../test/test_graceful_controller.cpp | 21 +- nav2_mppi_controller/README.md | 9 - .../test/CMakeLists.txt | 1 + .../test/test_shim_controller.cpp | 40 ++-- .../src/costmap_filters/keepout_params.yaml | 8 +- .../costmap_filters/speed_global_params.yaml | 6 +- .../costmap_filters/speed_local_params.yaml | 6 +- .../src/error_codes/error_code_param.yaml | 6 +- .../gps_navigation/nav2_no_map_params.yaml | 9 +- nav2_system_tests/src/route/CMakeLists.txt | 1 + .../src/route/test_route_launch.py | 3 + nav2_system_tests/src/system/CMakeLists.txt | 7 + .../src/system/nav2_system_params.yaml | 6 +- .../src/system/test_system_launch.py | 3 + .../test_system_with_obstacle_launch.py | 3 + .../src/system/test_wrong_init_pose_launch.py | 3 + .../src/waypoint_follower/test_case_launch.py | 7 +- 26 files changed, 317 insertions(+), 52 deletions(-) create mode 100644 nav2_behavior_tree/test/plugins/action/test_path_handler_selector_node.cpp diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 84d6f149843..c65fc774b90 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -258,7 +258,7 @@ Name of the selected progress checker received from the topic subscription - + Name of the topic to receive path handler selection commands Default path handler of the controller selector Name of the selected path handler received from the topic subscription diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 56f144fc4f1..c15bb7944f3 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -91,6 +91,8 @@ plugin_add_test(test_goal_checker_selector_node test_goal_checker_selector_node. plugin_add_test(test_progress_checker_selector_node test_progress_checker_selector_node.cpp nav2_progress_checker_selector_bt_node) +plugin_add_test(test_path_handler_selector_node test_path_handler_selector_node.cpp nav2_path_handler_selector_bt_node) + plugin_add_test(test_toggle_collision_monitor_service test_toggle_collision_monitor_service.cpp nav2_toggle_collision_monitor_service_bt_node) diff --git a/nav2_behavior_tree/test/plugins/action/test_path_handler_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_path_handler_selector_node.cpp new file mode 100644 index 00000000000..4f3b2a2c250 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_path_handler_selector_node.cpp @@ -0,0 +1,204 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "behaviortree_cpp/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/path_handler_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class PathHandlerSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("path_handler_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); + + // Configure and activate the lifecycle node + node_->configure(); + node_->activate(); + + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, config); + }; + + factory_->registerBuilder( + "PathHandlerSelector", + builder); + } + + static void TearDownTestCase() + { + // Properly deactivate and cleanup the lifecycle node + node_->deactivate(); + node_->cleanup(); + + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + executor_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +nav2::LifecycleNode::SharedPtr PathHandlerSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr PathHandlerSelectorTestFixture::executor_ = + nullptr; + +BT::NodeConfiguration * PathHandlerSelectorTestFixture::config_ = nullptr; +std::shared_ptr PathHandlerSelectorTestFixture::factory_ = nullptr; +std::shared_ptr PathHandlerSelectorTestFixture::tree_ = nullptr; + +TEST_F(PathHandlerSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_path_handler_result; + EXPECT_TRUE(config_->blackboard->get("selected_path_handler", selected_path_handler_result)); + + EXPECT_EQ(selected_path_handler_result, "SimplePathHandler"); + + std_msgs::msg::String selected_path_handler_cmd; + + selected_path_handler_cmd.data = "HardPathHandler"; + + rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); + + auto path_handler_selector_pub = + node_->create_publisher("path_handler_selector_custom_topic_name", qos); + path_handler_selector_pub->on_activate(); + + // publish a few updates of the selected_path_handler + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + path_handler_selector_pub->publish(selected_path_handler_cmd); + + executor_->spin_some(); + } + + // check path_handler updated + EXPECT_TRUE(config_->blackboard->get("selected_path_handler", selected_path_handler_result)); + EXPECT_EQ("HardPathHandler", selected_path_handler_result); +} + +TEST_F(PathHandlerSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_path_handler_result; + EXPECT_TRUE(config_->blackboard->get("selected_path_handler", selected_path_handler_result)); + + EXPECT_EQ(selected_path_handler_result, "GridBased"); + + std_msgs::msg::String selected_path_handler_cmd; + + selected_path_handler_cmd.data = "RRT"; + + rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); + + auto path_handler_selector_pub = + node_->create_publisher("path_handler_selector", qos); + path_handler_selector_pub->on_activate(); + + // publish a few updates of the selected_path_handler + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + path_handler_selector_pub->publish(selected_path_handler_cmd); + + executor_->spin_some(); + } + + // check path_handler updated + EXPECT_TRUE(config_->blackboard->get("selected_path_handler", selected_path_handler_result)); + EXPECT_EQ("RRT", selected_path_handler_result); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index bfa48e91955..7e33619f341 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -228,7 +228,15 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker simple_path_handler mppi_path_handler ${library_name} +install(TARGETS + simple_progress_checker + position_goal_checker + pose_progress_checker + simple_goal_checker + stopped_goal_checker + simple_path_handler + mppi_path_handler + ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp index 6bc0c2aebb1..679549a72fe 100644 --- a/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include "nav2_controller/plugins/simple_path_handler.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index 0c5382d6449..ceb26eed100 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include "nav2_core/path_handler.hpp" #include "nav2_ros_common/lifecycle_node.hpp" @@ -52,7 +54,7 @@ class SimplePathHandler : public nav2_core::PathHandler * @return output poose in global reference frame */ geometry_msgs::msg::PoseStamped transformToGlobalPlanFrame( - const geometry_msgs::msg::PoseStamped & pose); + const geometry_msgs::msg::PoseStamped & pose); /** * @brief Get global plan within window of the local costmap size diff --git a/nav2_controller/plugins/mppi_path_handler.cpp b/nav2_controller/plugins/mppi_path_handler.cpp index a43228330a3..7b13bde539b 100644 --- a/nav2_controller/plugins/mppi_path_handler.cpp +++ b/nav2_controller/plugins/mppi_path_handler.cpp @@ -40,7 +40,7 @@ void MPPIPathHandler::initialize( SimplePathHandler::initialize(parent, logger, plugin_name, costmap_ros, tf); auto node = parent.lock(); prune_distance_ = node->declare_or_get_parameter(plugin_name + ".prune_distance", - 0.2); + 2.0); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index 22c0d2a9528..5f38a6177ce 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -198,7 +198,8 @@ nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { geometry_msgs::msg::PoseStamped global_pose = transformToGlobalPlanFrame(pose); - auto [transformed_plan, closest_point] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); + auto [transformed_plan, + closest_point] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); // Remove the portion of the global plan that we've already passed so we don't // process it on the next iteration (this is called path pruning) diff --git a/nav2_graceful_controller/test/CMakeLists.txt b/nav2_graceful_controller/test/CMakeLists.txt index d78b32e5943..8637a4403cb 100644 --- a/nav2_graceful_controller/test/CMakeLists.txt +++ b/nav2_graceful_controller/test/CMakeLists.txt @@ -8,6 +8,7 @@ target_link_libraries(test_graceful_controller ${library_name} ${geometry_msgs_TARGETS} nav2_controller::simple_goal_checker + nav2_controller::simple_path_handler nav2_core::nav2_core nav2_costmap_2d::nav2_costmap_2d_core nav2_util::nav2_util_core diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 6026a3a0350..aff652954dd 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -17,6 +17,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/simple_path_handler.hpp" #include "nav2_core/controller_exceptions.hpp" #include "nav2_graceful_controller/ego_polar_coords.hpp" #include "nav2_graceful_controller/smooth_control_law.hpp" @@ -477,6 +478,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { auto controller = std::make_shared(); controller->configure(node, "test", tf, costmap_ros); controller->activate(); + nav2_controller::SimplePathHandler path_handler; + path_handler.initialize(node, node->get_logger(), "path_handler", costmap_ros, tf); // Create the robot pose geometry_msgs::msg::PoseStamped robot_pose; @@ -513,6 +516,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { plan.poses[2].pose.position.y = 1.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); controller->newPathReceived(plan); + path_handler.setPlan(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -522,7 +526,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path transformed_global_plan; + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(robot_pose); geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan, goal); @@ -555,6 +559,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { auto controller = std::make_shared(); controller->configure(node, "test", tf, costmap_ros); controller->activate(); + nav2_controller::SimplePathHandler path_handler; + path_handler.initialize(node, node->get_logger(), "path_handler", costmap_ros, tf); // Create the robot pose geometry_msgs::msg::PoseStamped robot_pose; @@ -591,6 +597,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { plan.poses[2].pose.position.y = 0.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); controller->newPathReceived(plan); + path_handler.setPlan(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -600,7 +607,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path transformed_global_plan; + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(robot_pose); geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan, goal); @@ -638,6 +645,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { auto controller = std::make_shared(); controller->configure(node, "test", tf, costmap_ros); controller->activate(); + nav2_controller::SimplePathHandler path_handler; + path_handler.initialize(node, node->get_logger(), "path_handler", costmap_ros, tf); // Create the robot pose geometry_msgs::msg::PoseStamped robot_pose; @@ -674,6 +683,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { plan.poses[2].pose.position.y = 0.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); controller->newPathReceived(plan); + path_handler.setPlan(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -683,7 +693,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { // Set the goal checker nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path transformed_global_plan; + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(robot_pose); geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan, goal); @@ -717,6 +727,8 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { auto controller = std::make_shared(); controller->configure(node, "test", tf, costmap_ros); controller->activate(); + nav2_controller::SimplePathHandler path_handler; + path_handler.initialize(node, node->get_logger(), "path_handler", costmap_ros, tf); // Create the robot pose geometry_msgs::msg::PoseStamped robot_pose; @@ -761,6 +773,7 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { plan.poses[4].pose.position.y = 0.0; plan.poses[4].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); controller->newPathReceived(plan); + path_handler.setPlan(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -771,7 +784,7 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); - nav_msgs::msg::Path transformed_global_plan; + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(robot_pose); geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan, goal); diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 75629aef773..4b37a383f59 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -68,14 +68,6 @@ This process is then repeated a number of times and returns a converged solution | trajectory_step | int | Default: 5. The step between trajectories to visualize to downsample candidate trajectory pool. | | time_step | int | Default: 3. The step between points on trajectories to visualize to downsample trajectory density. | -#### Path Handler - | Parameter | Type | Definition | - | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | - | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. In addition, these cusping points will also be treated by the critics as local goals that the robot will attempt to reach. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | - | inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. | - | inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. | - #### Ackermann Motion Model | Parameter | Type | Definition | | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -203,7 +195,6 @@ controller_server: vy_max: 0.5 wz_max: 1.9 iteration_count: 1 - prune_distance: 1.7 temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" diff --git a/nav2_rotation_shim_controller/test/CMakeLists.txt b/nav2_rotation_shim_controller/test/CMakeLists.txt index e2d79806a31..51c2b3e8877 100644 --- a/nav2_rotation_shim_controller/test/CMakeLists.txt +++ b/nav2_rotation_shim_controller/test/CMakeLists.txt @@ -9,6 +9,7 @@ target_link_libraries(test_shim_controller rclcpp::rclcpp nav2_costmap_2d::nav2_costmap_2d_core nav2_controller::simple_goal_checker + nav2_controller::simple_path_handler nav2_util::nav2_util_core tf2_ros::tf2_ros ) diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 61a6ad492c6..a0b78fbd43d 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -23,6 +23,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/simple_path_handler.hpp" #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" #include "tf2_ros/transform_broadcaster.hpp" @@ -253,10 +254,11 @@ TEST(RotationShimControllerTest, computeVelocityTests) auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); controller->activate(); + nav2_controller::SimplePathHandler path_handler; + path_handler.initialize(node, node->get_logger(), "path_handler", costmap, tf); // Test state update and path setting nav_msgs::msg::Path path; - path.header.frame_id = "fake_frame"; path.poses.resize(10); geometry_msgs::msg::PoseStamped pose; @@ -265,20 +267,6 @@ TEST(RotationShimControllerTest, computeVelocityTests) nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap); - // send without setting a path - should go to RPP immediately - // then it should throw an exception because the path is empty and invalid - nav_msgs::msg::Path transformed_global_plan; - geometry_msgs::msg::Pose goal; - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, - transformed_global_plan, goal), - std::runtime_error); - - // Set with a path -- should attempt to find a sampled point but throw exception - // because it cannot be found, then go to RPP and throw exception because it cannot be transformed - controller->newPathReceived(path); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, - transformed_global_plan, goal), - std::runtime_error); path.header.frame_id = "base_link"; path.poses[1].pose.position.x = 0.1; @@ -293,7 +281,10 @@ TEST(RotationShimControllerTest, computeVelocityTests) // validly because we setup the TF for it. The -1.0 should be selected since default min // is 0.5 and that should cause a rotation in place controller->newPathReceived(path); + path_handler.setPlan(path); tf_broadcaster->sendTransform(transform); + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); + geometry_msgs::msg::Pose goal; auto effort = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); @@ -312,7 +303,9 @@ TEST(RotationShimControllerTest, computeVelocityTests) // is 0.5 and that should cause a pass off to the RPP controller which will throw // and exception because it is off of the costmap controller->newPathReceived(path); + path_handler.setPlan(path); tf_broadcaster->sendTransform(transform); + transformed_global_plan = path_handler.transformGlobalPlan(pose); EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal), std::runtime_error); @@ -355,6 +348,8 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); controller->activate(); + nav2_controller::SimplePathHandler path_handler; + path_handler.initialize(node, node->get_logger(), "path_handler", costmap, tf); // Test state update and path setting nav_msgs::msg::Path path; @@ -387,7 +382,8 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { // Calculate first velocity command controller->newPathReceived(path); - nav_msgs::msg::Path transformed_global_plan; + path_handler.setPlan(path); + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); @@ -431,6 +427,8 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); controller->activate(); + nav2_controller::SimplePathHandler path_handler; + path_handler.initialize(node, node->get_logger(), "path_handler", costmap, tf); // Test state update and path setting nav_msgs::msg::Path path; @@ -462,7 +460,8 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[3].header.frame_id = "base_link"; controller->newPathReceived(path); - nav_msgs::msg::Path transformed_global_plan; + path_handler.setPlan(path); + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); @@ -472,6 +471,8 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[3].pose.orientation.z = 0.3826834; path.poses[3].pose.orientation.w = 0.9238795; controller->newPathReceived(path); + path_handler.setPlan(path); + transformed_global_plan = path_handler.transformGlobalPlan(pose); cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); @@ -514,6 +515,8 @@ TEST(RotationShimControllerTest, accelerationTests) { auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); controller->activate(); + nav2_controller::SimplePathHandler path_handler; + path_handler.initialize(node, node->get_logger(), "path_handler", costmap, tf); // Test state update and path setting nav_msgs::msg::Path path; @@ -546,7 +549,8 @@ TEST(RotationShimControllerTest, accelerationTests) { // Test acceleration limits controller->newPathReceived(path); - nav_msgs::msg::Path transformed_global_plan; + path_handler.setPlan(path); + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); geometry_msgs::msg::Pose goal; auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index c763808912f..4d38e792fca 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -55,11 +55,13 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["goal_checker"] + path_handler_plugins: ["path_handler"] controller_plugins: ["FollowPath"] # Progress checker parameters @@ -73,12 +75,13 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True + path_handler: + plugin: "nav2_controller::MPPIPathHandler" + prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True - prune_distance: 1.0 - forward_prune_distance: 1.0 min_vel_x: 0.0 min_vel_y: 0.0 max_vel_x: 0.26 @@ -101,7 +104,6 @@ controller_server: sim_time: 1.7 linear_granularity: 0.05 angular_granularity: 0.025 - transform_tolerance: 0.2 xy_goal_tolerance: 0.25 trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: 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 a7593657cee..70066e06314 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -55,12 +55,14 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 speed_limit_topic: "/speed_limit" progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["goal_checker"] + path_handler_plugins: ["path_handler"] controller_plugins: ["FollowPath"] # Progress checker parameters @@ -74,6 +76,9 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True + path_handler: + plugin: "nav2_controller::MPPIPathHandler" + prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -100,7 +105,6 @@ controller_server: sim_time: 1.7 linear_granularity: 0.05 angular_granularity: 0.025 - transform_tolerance: 0.2 xy_goal_tolerance: 0.25 trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: 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 aef69d68074..81fc902bbad 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -55,12 +55,14 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 speed_limit_topic: "/speed_limit" progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["goal_checker"] + path_handler_plugins: ["path_handler"] controller_plugins: ["FollowPath"] # Progress checker parameters @@ -74,6 +76,9 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True + path_handler: + plugin: "nav2_controller::MPPIPathHandler" + prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -100,7 +105,6 @@ controller_server: sim_time: 1.7 linear_granularity: 0.05 angular_granularity: 0.025 - transform_tolerance: 0.2 xy_goal_tolerance: 0.25 trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: True diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 7886fe66866..3d8e54987fd 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -1,12 +1,14 @@ controller_server: ros__parameters: controller_frequency: 20.0 + transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.2 failure_tolerance: -0.1 progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + path_handler_plugins: ["path_handler"] controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded","failed_to_make_progress", "no_valid_control"] unknown: plugin: "nav2_system_tests::UnknownErrorController" @@ -38,6 +40,9 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_handler: + plugin: "nav2_controller::MPPIPathHandler" + prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -64,7 +69,6 @@ controller_server: sim_time: 1.7 linear_granularity: 0.05 angular_granularity: 0.025 - transform_tolerance: 0.2 xy_goal_tolerance: 0.25 trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: 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 01fe9656d5b..8ef51ae22f7 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 @@ -28,12 +28,14 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + path_handler_plugins: ["path_handler"] controller_plugins: ["FollowPath"] # Progress checker parameters @@ -52,7 +54,10 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters + path_handler: + plugin: "nav2_controller::MPPIPathHandler" + prune_distance: 1.7 + # MPPI parameters FollowPath: plugin: "nav2_mppi_controller::MPPIController" time_steps: 56 @@ -71,8 +76,6 @@ controller_server: vy_max: 0.5 wz_max: 1.9 iteration_count: 1 - prune_distance: 1.7 - transform_tolerance: 0.1 temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" diff --git a/nav2_system_tests/src/route/CMakeLists.txt b/nav2_system_tests/src/route/CMakeLists.txt index a69bbac70a4..f28023d89f8 100644 --- a/nav2_system_tests/src/route/CMakeLists.txt +++ b/nav2_system_tests/src/route/CMakeLists.txt @@ -8,6 +8,7 @@ ament_add_test(test_route BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=tester_node.py ASTAR=True + PATH_HANDLER=nav2_controller::SimplePathHandler CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController PLANNER=nav2_navfn_planner::NavfnPlanner ) diff --git a/nav2_system_tests/src/route/test_route_launch.py b/nav2_system_tests/src/route/test_route_launch.py index 6a454563894..008d5859d6f 100755 --- a/nav2_system_tests/src/route/test_route_launch.py +++ b/nav2_system_tests/src/route/test_route_launch.py @@ -62,6 +62,9 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} ) + param_substitutions.update( + {'controller_server.ros__parameters.path_handler.plugin': os.getenv('PATH_HANDLER', '')} + ) param_substitutions.update( {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} ) diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 3db27c6398e..b8e3a2ab610 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -8,6 +8,7 @@ ament_add_test(test_bt_navigator BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=True + PATH_HANDLER=nav2_controller::SimplePathHandler CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -22,6 +23,7 @@ ament_add_test(test_bt_navigator_with_wrong_init_pose BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=True + PATH_HANDLER=nav2_controller::SimplePathHandler CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -36,6 +38,7 @@ ament_add_test(test_bt_navigator_with_dijkstra BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False + PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -51,6 +54,7 @@ ament_add_test(test_bt_navigator_with_groot_monitoring TESTER=nav_to_pose_tester_node.py ASTAR=False GROOT_MONITORING=True + PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -66,6 +70,7 @@ ament_add_test(test_dynamic_obstacle BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False + PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -81,6 +86,7 @@ ament_add_test(test_nav_through_poses BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml TESTER=nav_through_poses_tester_node.py ASTAR=False + PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -96,6 +102,7 @@ ament_add_test(test_nav_through_poses_error_msg BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml TESTER=nav_through_poses_tester_error_msg_node.py ASTAR=False + PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 46d678e08c3..f2f0a0cb84b 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -71,12 +71,14 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + path_handler_plugins: ["path_handler"] controller_plugins: ["FollowPath"] use_realtime_priority: false @@ -96,6 +98,9 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_handler: + plugin: "nav2_controller::MPPIPathHandler" + prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -122,7 +127,6 @@ controller_server: sim_time: 1.7 linear_granularity: 0.05 angular_granularity: 0.025 - transform_tolerance: 0.2 xy_goal_tolerance: 0.25 trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: True diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index a1b13bcbaef..2fea7871a8a 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -67,6 +67,9 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} ) + param_substitutions.update( + {'controller_server.ros__parameters.path_handler.plugin': os.getenv('PATH_HANDLER', '')} + ) param_substitutions.update( {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} ) diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index 095066ea739..e793e4748f5 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -65,6 +65,9 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} ) + param_substitutions.update( + {'controller_server.ros__parameters.path_handler.plugin': os.getenv('PATH_HANDLER', '')} + ) param_substitutions.update( {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} ) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index bef573bca71..864f3fbde00 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -67,6 +67,9 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} ) + param_substitutions.update( + {'controller_server.ros__parameters.path_handler.plugin': os.getenv('PATH_HANDLER', '')} + ) param_substitutions.update( {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} ) diff --git a/nav2_system_tests/src/waypoint_follower/test_case_launch.py b/nav2_system_tests/src/waypoint_follower/test_case_launch.py index ebcd491c2ea..53d0f70f5bd 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_launch.py +++ b/nav2_system_tests/src/waypoint_follower/test_case_launch.py @@ -48,15 +48,10 @@ def generate_launch_description() -> LaunchDescription: ) params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml') - - # Replace the `use_astar` setting on the params file - param_substitutions = { - 'controller_server.ros__parameters.FollowPath.prune_distance': '1.0', - 'controller_server.ros__parameters.FollowPath.forward_prune_distance': '1.0'} configured_params = RewrittenYaml( source_file=params_file, root_key='', - param_rewrites=param_substitutions, + param_rewrites={}, value_rewrites={ 'KEEPOUT_ZONE_ENABLED': 'False', 'SPEED_ZONE_ENABLED': 'False', From ffb5a0e4bf7438a0ac3b095ceab76c62c60f8855 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 10 Oct 2025 15:41:46 +0000 Subject: [PATCH 28/62] Add path length checking in position goal checker Signed-off-by: mini-1235 --- .../plugins/position_goal_checker.hpp | 1 + nav2_controller/plugins/position_goal_checker.cpp | 14 +++++++++++++- nav2_controller/plugins/simple_goal_checker.cpp | 1 + 3 files changed, 15 insertions(+), 1 deletion(-) diff --git a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp index fb1f8241e60..7f0e419c5a8 100644 --- a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp @@ -61,6 +61,7 @@ class PositionGoalChecker : public nav2_core::GoalChecker protected: double xy_goal_tolerance_; double xy_goal_tolerance_sq_; + double path_length_tolerance_; bool stateful_; bool position_reached_; std::string plugin_name_; diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index 4f78118e659..5ae371b910d 100644 --- a/nav2_controller/plugins/position_goal_checker.cpp +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -18,6 +18,7 @@ #include "nav2_controller/plugins/position_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; @@ -28,6 +29,7 @@ namespace nav2_controller PositionGoalChecker::PositionGoalChecker() : xy_goal_tolerance_(0.25), xy_goal_tolerance_sq_(0.0625), + path_length_tolerance_(1.0), stateful_(true), position_reached_(false) { @@ -42,6 +44,8 @@ void PositionGoalChecker::initialize( auto node = parent.lock(); xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25); + path_length_tolerance_ = node->declare_or_get_parameter(plugin_name + ".path_length_tolerance", + 1.0); stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; @@ -58,8 +62,14 @@ void PositionGoalChecker::reset() bool PositionGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &) + const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path & transformed_global_plan) { + // If the local plan is longer than the tolerance, we skip the check + if (nav2_util::geometry_utils::calculate_path_length(transformed_global_plan) > + path_length_tolerance_) + { + return false; + } // If stateful and position was already reached, maintain state if (stateful_ && position_reached_) { return true; @@ -127,6 +137,8 @@ PositionGoalChecker::dynamicParametersCallback(std::vector pa if (param_name == plugin_name_ + ".xy_goal_tolerance") { xy_goal_tolerance_ = parameter.as_double(); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + } else if (param_name == plugin_name_ + ".path_length_tolerance") { + path_length_tolerance_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == plugin_name_ + ".stateful") { diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index ccc470f072e..641d4d4c56b 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -92,6 +92,7 @@ bool SimpleGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path & transformed_global_plan) { + // If the local plan is longer than the tolerance, we skip the check if (nav2_util::geometry_utils::calculate_path_length(transformed_global_plan) > path_length_tolerance_) { From aa0755718d5c0789350f6c416b344e9ebc50517f Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 10 Oct 2025 16:46:45 +0000 Subject: [PATCH 29/62] Remove controller frequency in dynamic parameters Signed-off-by: mini-1235 --- nav2_controller/src/parameter_handler.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 83b1a4b8633..027087b90df 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -206,9 +206,7 @@ ParameterHandler::updateParametersCallback( const auto & param_name = parameter.get_name(); if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == "controller_frequency") { - params_.controller_frequency = parameter.as_double(); - } else if (param_name == "transform_tolerance") { + if (param_name == "transform_tolerance") { params_.transform_tolerance = parameter.as_double(); } else if (param_name == "min_x_velocity_threshold") { params_.min_x_velocity_threshold = parameter.as_double(); From 621ee2a5824deecb9224d97a7c181dd7600e5026 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 12 Oct 2025 10:02:13 +0000 Subject: [PATCH 30/62] Fix as suggested Signed-off-by: mini-1235 --- .../include/nav2_controller/controller_server.hpp | 7 ++++--- .../include/nav2_controller/parameter_handler.hpp | 1 - .../plugins/simple_path_handler.hpp | 4 ++-- nav2_controller/plugins.xml | 4 ++-- nav2_controller/plugins/mppi_path_handler.cpp | 2 +- nav2_controller/plugins/simple_path_handler.cpp | 4 ++-- nav2_controller/src/controller_server.cpp | 13 +++++++------ nav2_controller/src/parameter_handler.cpp | 5 +---- nav2_core/include/nav2_core/controller.hpp | 4 ++-- ...ath_handler.hpp => controller_path_handler.hpp} | 14 +++++++------- .../include/nav2_costmap_2d/costmap_2d_ros.hpp | 6 ++++++ .../include/dwb_core/dwb_local_planner.hpp | 8 ++++---- .../dwb_core/src/dwb_local_planner.cpp | 9 ++++----- .../graceful_controller.hpp | 4 ++-- .../nav2_graceful_controller/parameter_handler.hpp | 1 - .../src/graceful_controller.cpp | 2 +- nav2_graceful_controller/src/parameter_handler.cpp | 1 - .../test/test_graceful_controller.cpp | 8 ++++---- .../benchmark/controller_benchmark.cpp | 2 +- .../include/nav2_mppi_controller/controller.hpp | 5 ++--- nav2_mppi_controller/src/controller.cpp | 5 ++--- .../test/controller_state_transition_test.cpp | 2 +- .../parameter_handler.hpp | 1 - .../regulated_pure_pursuit_controller.hpp | 9 ++------- .../src/parameter_handler.cpp | 1 - .../src/regulated_pure_pursuit_controller.cpp | 2 +- .../nav2_rotation_shim_controller.hpp | 4 ++-- .../src/nav2_rotation_shim_controller.cpp | 4 ++-- .../test/test_shim_controller.cpp | 8 ++++---- .../controller/controller_error_plugins.hpp | 12 ++++++------ 30 files changed, 72 insertions(+), 80 deletions(-) rename nav2_core/include/nav2_core/{path_handler.hpp => controller_path_handler.hpp} (87%) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 73962f0b8ed..5f42b5b5306 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -25,7 +25,7 @@ #include "nav2_core/controller.hpp" #include "nav2_core/progress_checker.hpp" #include "nav2_core/goal_checker.hpp" -#include "nav2_core/path_handler.hpp" +#include "nav2_core/controller_path_handler.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "tf2_ros/transform_listener.hpp" #include "nav2_msgs/action/follow_path.hpp" @@ -54,7 +54,7 @@ class ControllerServer : public nav2::LifecycleNode using ControllerMap = std::unordered_map; using GoalCheckerMap = std::unordered_map; using ProgressCheckerMap = std::unordered_map; - using PathHandlerMap = std::unordered_map; + using PathHandlerMap = std::unordered_map; /** * @brief Constructor for nav2_controller::ControllerServer @@ -264,7 +264,7 @@ class ControllerServer : public nav2::LifecycleNode std::string controller_ids_concat_, current_controller_; // Path Handler Plugins - pluginlib::ClassLoader path_handler_loader_; + pluginlib::ClassLoader path_handler_loader_; PathHandlerMap path_handlers_; std::string path_handler_ids_concat_, current_path_handler_; @@ -282,6 +282,7 @@ class ControllerServer : public nav2::LifecycleNode std::unique_ptr param_handler_; Parameters * params_; nav2::Publisher::SharedPtr transformed_plan_pub_; + double transform_tolerance_; private: /** diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index bbf7942deb6..e0125ce7c2a 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -33,7 +33,6 @@ namespace nav2_controller struct Parameters { double controller_frequency; - double transform_tolerance; double min_x_velocity_threshold; double min_y_velocity_threshold; double min_theta_velocity_threshold; diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index ceb26eed100..6d5cc7d7bbf 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -21,7 +21,7 @@ #include #include #include -#include "nav2_core/path_handler.hpp" +#include "nav2_core/controller_path_handler.hpp" #include "nav2_ros_common/lifecycle_node.hpp" namespace nav2_controller @@ -34,7 +34,7 @@ using PathIterator = std::vector::iterator; * and transforming the resulting path into the robot base frame. */ -class SimplePathHandler : public nav2_core::PathHandler +class SimplePathHandler : public nav2_core::ControllerPathHandler { public: void initialize( diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index ea599b069de..276cdda0242 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -25,12 +25,12 @@ - + Handles the path from planner server and transform it to robot base frame - + Handles the path from planner server and transform it to costmap's global frame diff --git a/nav2_controller/plugins/mppi_path_handler.cpp b/nav2_controller/plugins/mppi_path_handler.cpp index 7b13bde539b..9a1f237644e 100644 --- a/nav2_controller/plugins/mppi_path_handler.cpp +++ b/nav2_controller/plugins/mppi_path_handler.cpp @@ -125,4 +125,4 @@ MPPIPathHandler::dynamicParametersCallback(std::vector parame } // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(nav2_controller::MPPIPathHandler, nav2_core::PathHandler) +PLUGINLIB_EXPORT_CLASS(nav2_controller::MPPIPathHandler, nav2_core::ControllerPathHandler) diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index 5f38a6177ce..070f1fbc67a 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -52,7 +52,7 @@ void SimplePathHandler::initialize( 0.2); inversion_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + ".inversion_yaw_tolerance", 0.4); - node->get_parameter("transform_tolerance", transform_tolerance_); + transform_tolerance_ = costmap_ros_->getTransformTolerance(); if (max_robot_pose_search_dist_ < 0.0) { RCLCPP_WARN( logger_, "Max robot search distance is negative, setting to max to search" @@ -251,4 +251,4 @@ SimplePathHandler::dynamicParametersCallback(std::vector para } // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(nav2_controller::SimplePathHandler, nav2_core::PathHandler) +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimplePathHandler, nav2_core::ControllerPathHandler) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 67e6acb0893..da4e71362e5 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -38,7 +38,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), lp_loader_("nav2_core", "nav2_core::Controller"), - path_handler_loader_("nav2_core", "nav2_core::PathHandler"), + path_handler_loader_("nav2_core", "nav2_core::ControllerPathHandler"), costmap_update_timeout_(300ms) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -68,6 +68,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->configure(); // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); + transform_tolerance_ = costmap_ros_->getTransformTolerance(); try { param_handler_ = std::make_unique( node, get_logger()); @@ -160,7 +161,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) for (size_t i = 0; i != params_->path_handler_ids.size(); i++) { try { - nav2_core::PathHandler::Ptr path_handler = + nav2_core::ControllerPathHandler::Ptr path_handler = path_handler_loader_.createUniqueInstance(params_->path_handler_types[i]); RCLCPP_INFO( get_logger(), "Created path handler : %s of type %s", @@ -422,7 +423,7 @@ geometry_msgs::msg::PoseStamped ControllerServer::getTransformedGoal( } geometry_msgs::msg::PoseStamped transformed_goal; if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *costmap_ros_->getTfBuffer(), - costmap_ros_->getGlobalFrameID(), params_->transform_tolerance)) + costmap_ros_->getGlobalFrameID(), transform_tolerance_)) { throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); } @@ -644,7 +645,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); - geometry_msgs::msg::Pose goal = getTransformedGoal(pose.header.stamp).pose; + geometry_msgs::msg::PoseStamped goal = getTransformedGoal(pose.header.stamp); transformed_global_plan_ = path_handlers_[current_path_handler_]->transformGlobalPlan(pose); auto path = std::make_unique(transformed_global_plan_); transformed_plan_pub_->publish(std::move(path)); @@ -693,7 +694,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; if (!nav2_util::transformPoseInTargetFrame( pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), - current_path_.header.frame_id, params_->transform_tolerance)) + current_path_.header.frame_id, transform_tolerance_)) { throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); } @@ -840,7 +841,7 @@ bool ControllerServer::isGoalReached() end_pose_.header.stamp = pose.header.stamp; nav2_util::transformPoseInTargetFrame( end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), - costmap_ros_->getGlobalFrameID(), params_->transform_tolerance); + costmap_ros_->getGlobalFrameID(), transform_tolerance_); return goal_checkers_[current_goal_checker_]->isGoalReached( pose.pose, transformed_end_pose.pose, diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 027087b90df..436272235db 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -35,7 +35,6 @@ ParameterHandler::ParameterHandler( logger_ = logger; params_.controller_frequency = node->declare_or_get_parameter("controller_frequency", 20.0); - params_.transform_tolerance = node->declare_or_get_parameter("transform_tolerance", 0.1); params_.min_x_velocity_threshold = node->declare_or_get_parameter("min_x_velocity_threshold", 0.0001); params_.min_y_velocity_threshold = node->declare_or_get_parameter("min_y_velocity_threshold", @@ -206,9 +205,7 @@ ParameterHandler::updateParametersCallback( const auto & param_name = parameter.get_name(); if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == "transform_tolerance") { - params_.transform_tolerance = parameter.as_double(); - } else if (param_name == "min_x_velocity_threshold") { + if (param_name == "min_x_velocity_threshold") { params_.min_x_velocity_threshold = parameter.as_double(); } else if (param_name == "min_y_velocity_threshold") { params_.min_y_velocity_threshold = parameter.as_double(); diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 04f2c1b6dc3..b52f42d7259 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -112,7 +112,7 @@ class Controller * @param velocity Current robot velocity * @param goal_checker Pointer to the current goal checker the task is utilizing * @param transformed_global_plan The global plan after being processed by the path handler - * @param goal The last pose of the global plan + * @param global_goal The last pose of the global plan * @return The best command for the robot to drive */ virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( @@ -120,7 +120,7 @@ class Controller const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) = 0; + const geometry_msgs::msg::PoseStamped & global_goal) = 0; /** * @brief Cancel the current control action diff --git a/nav2_core/include/nav2_core/path_handler.hpp b/nav2_core/include/nav2_core/controller_path_handler.hpp similarity index 87% rename from nav2_core/include/nav2_core/path_handler.hpp rename to nav2_core/include/nav2_core/controller_path_handler.hpp index 98a03780c92..87a24b0e747 100644 --- a/nav2_core/include/nav2_core/path_handler.hpp +++ b/nav2_core/include/nav2_core/controller_path_handler.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_CORE__PATH_HANDLER_HPP_ -#define NAV2_CORE__PATH_HANDLER_HPP_ +#ifndef NAV2_CORE__CONTROLLER_PATH_HANDLER_HPP_ +#define NAV2_CORE__CONTROLLER_PATH_HANDLER_HPP_ #include #include @@ -27,7 +27,7 @@ namespace nav2_core { /** - * @class PathHandler + * @class ControllerPathHandler * @brief Function-object for handling the path from Planner Server * * This class defines the plugin interface used by the Controller Server to manage the @@ -35,12 +35,12 @@ namespace nav2_core * path segments the robot has already traversed and transforming the remaining, * relevant portion of the path into the costmap's global or base frame. */ -class PathHandler +class ControllerPathHandler { public: - typedef std::shared_ptr Ptr; + typedef std::shared_ptr Ptr; - virtual ~PathHandler() {} + virtual ~ControllerPathHandler() {} /** * @brief Initialize any parameters from the NodeHandle @@ -71,4 +71,4 @@ class PathHandler } // namespace nav2_core -#endif // NAV2_CORE__PATH_HANDLER_HPP_ +#endif // NAV2_CORE__CONTROLLER_PATH_HANDLER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index d1cda80b174..5b1fa01591b 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 @@ -212,6 +212,12 @@ class Costmap2DROS : public nav2::LifecycleNode return name_; } + /** @brief Returns the delay in transform (tf) data that is tolerable in seconds */ + double getTransformTolerance() const + { + return transform_tolerance_; + } + /** * @brief Return a pointer to the "master" costmap which receives updates from all the layers. * diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index d5468438ee2..467ce558ea3 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -106,7 +106,7 @@ class DWBLocalPlanner : public nav2_core::Controller * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands * @param transformed_global_plan The global plan after being processed by the path handler - * @param goal The last pose of the global plan + * @param global_goal The last pose of the global plan * @return The best command for the robot to drive */ geometry_msgs::msg::TwistStamped computeVelocityCommands( @@ -114,7 +114,7 @@ class DWBLocalPlanner : public nav2_core::Controller const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) override; + const geometry_msgs::msg::PoseStamped & global_goal) override; /** * @brief Score a given command. Can be used for testing. @@ -142,6 +142,7 @@ class DWBLocalPlanner : public nav2_core::Controller * @param velocity Current robot velocity * @param results Output param, if not NULL, will be filled in with full evaluation results * @param transformed_global_plan The global plan after being processed by the path handler + * @param global_goal The last pose of the global plan * @return Best command */ virtual nav_2d_msgs::msg::Twist2DStamped computeVelocityCommands( @@ -149,7 +150,7 @@ class DWBLocalPlanner : public nav2_core::Controller const nav_2d_msgs::msg::Twist2D & velocity, std::shared_ptr & results, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal); + const geometry_msgs::msg::PoseStamped & global_goal); /** * @brief Limits the maximum linear speed of the robot. @@ -175,7 +176,6 @@ class DWBLocalPlanner : public nav2_core::Controller std::shared_ptr & results); bool debug_trajectory_details_; - double transform_tolerance_{0}; /** * @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic" diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 7c090d0c39e..ee80738ce9d 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -95,7 +95,6 @@ void DWBLocalPlanner::configure( std::string traj_generator_name; - node->get_parameter("transform_tolerance", transform_tolerance_); node->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); node->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); node->get_parameter( @@ -219,7 +218,7 @@ DWBLocalPlanner::computeVelocityCommands( const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) + const geometry_msgs::msg::PoseStamped & global_goal) { std::shared_ptr results = nullptr; if (pub_->shouldRecordEvaluation()) { @@ -229,7 +228,7 @@ DWBLocalPlanner::computeVelocityCommands( try { nav_2d_msgs::msg::Twist2DStamped cmd_vel2d = computeVelocityCommands( pose, - nav_2d_utils::twist3Dto2D(velocity), results, transformed_global_plan, goal); + nav_2d_utils::twist3Dto2D(velocity), results, transformed_global_plan, global_goal); pub_->publishEvaluation(results); geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.twist = nav_2d_utils::twist2Dto3D(cmd_vel2d.velocity); @@ -255,7 +254,7 @@ DWBLocalPlanner::computeVelocityCommands( const nav_2d_msgs::msg::Twist2D & velocity, std::shared_ptr & results, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) + const geometry_msgs::msg::PoseStamped & global_goal) { if (results) { results->header.frame_id = pose.header.frame_id; @@ -266,7 +265,7 @@ DWBLocalPlanner::computeVelocityCommands( std::unique_lock lock(*(costmap->getMutex())); for (TrajectoryCritic::Ptr & critic : critics_) { - if (!critic->prepare(pose.pose, velocity, goal, transformed_global_plan)) { + if (!critic->prepare(pose.pose, velocity, global_goal.pose, transformed_global_plan)) { RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare"); } } diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 6241df6cd2d..43f16d8fd8e 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -84,7 +84,7 @@ class GracefulController : public nav2_core::Controller * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands * @param transformed_global_plan The global plan after being processed by the path handler - * @param goal The last pose of the global plan + * @param global_goal The last pose of the global plan * @return Best command */ geometry_msgs::msg::TwistStamped computeVelocityCommands( @@ -92,7 +92,7 @@ class GracefulController : public nav2_core::Controller const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) override; + const geometry_msgs::msg::PoseStamped & global_goal) override; /** * @brief nav2_core newPathReceived - Receives a new plan from the Planner Server diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp index 55026a0d6bd..9c76c6ce39b 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -31,7 +31,6 @@ namespace nav2_graceful_controller struct Parameters { - double transform_tolerance; double min_lookahead; double max_lookahead; double k_phi; diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index c73149b1ab2..5b318adbee0 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -106,7 +106,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( const geometry_msgs::msg::Twist & /*velocity*/, nav2_core::GoalChecker * goal_checker, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & /*goal*/) + const geometry_msgs::msg::PoseStamped & /*global_goal*/) { std::lock_guard param_lock(param_handler_->getMutex()); diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index a74163d1897..a5e8c084d09 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -68,7 +68,6 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); - node->get_parameter("transform_tolerance", params_.transform_tolerance); node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead); node->get_parameter(plugin_name_ + ".max_lookahead", params_.max_lookahead); diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index aff652954dd..e4531a157ee 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -527,7 +527,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(robot_pose); - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan, goal); @@ -608,7 +608,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(robot_pose); - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan, goal); @@ -694,7 +694,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { nav2_controller::SimpleGoalChecker checker; checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(robot_pose); - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan, goal); @@ -785,7 +785,7 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { checker.initialize(node, "checker", costmap_ros); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(robot_pose); - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, transformed_global_plan, goal); diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index daddbbb473b..1760cdd5864 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -110,7 +110,7 @@ void prepareAndRunBenchmark( nav2_core::GoalChecker * dummy_goal_checker{nullptr}; nav_msgs::msg::Path transformed_global_plan; - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; for (auto _ : state) { controller->computeVelocityCommands(pose, velocity, dummy_goal_checker, transformed_global_plan, goal); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index b28375b3c75..67bf60e7dc4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -81,14 +81,14 @@ class MPPIController : public nav2_core::Controller * @param robot_speed Robot speed * @param goal_checker Pointer to the goal checker for awareness if completed task * @param transformed_global_plan The global plan after being processed by the path handler - * @param goal The last pose of the global plan + * @param global_goal The last pose of the global plan */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) override; + const geometry_msgs::msg::PoseStamped & global_goal) override; /** * @brief Receives a new plan from the Planner Server @@ -126,7 +126,6 @@ class MPPIController : public nav2_core::Controller bool visualize_; bool publish_optimal_trajectory_; - double transform_tolerance_; }; } // namespace nav2_mppi_controller diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index e12361e88dd..ed1e8d96978 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -35,7 +35,6 @@ void MPPIController::configure( auto node = parent_.lock(); auto getParentParam = parameters_handler_->getParamGetter(""); - getParentParam(transform_tolerance_, "transform_tolerance", 0.1, ParameterType::Static); // Get high-level controller parameters auto getParam = parameters_handler_->getParamGetter(name_); getParam(visualize_, "visualize", false); @@ -95,7 +94,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) + const geometry_msgs::msg::PoseStamped & global_goal) { #ifdef BENCHMARK_TESTING auto start = std::chrono::system_clock::now(); @@ -107,7 +106,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_global_plan, goal, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_global_plan, global_goal.pose, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index 9a21c299a06..0fa24870ad2 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -58,7 +58,7 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) controller->newPathReceived(path); nav_msgs::msg::Path transformed_global_plan; - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}, transformed_global_plan, goal)); diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 2bb43a392b4..b59826e0a1a 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -60,7 +60,6 @@ struct Parameters bool allow_reversing; bool interpolate_curvature_after_goal; bool use_collision_detection; - double transform_tolerance; bool stateful; }; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index cb3cbf1dabe..3e0ee89afe4 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -81,16 +81,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller /** * @brief Compute the best command given the current pose and velocity, with possible debug information - * - * Same as above computeVelocityCommands, but with debug results. - * If the results pointer is not null, additional information about the twists - * evaluated will be in results after the call. - * * @param pose Current robot pose * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands * @param transformed_global_plan The global plan after being processed by the path handler - * @param goal The last pose of the global plan + * @param global_goal The last pose of the global plan * @return Best command */ geometry_msgs::msg::TwistStamped computeVelocityCommands( @@ -98,7 +93,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) override; + const geometry_msgs::msg::PoseStamped & global_goal) override; bool cancel() override; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 8e859ab6a57..462286f0443 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -111,7 +111,6 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".rotate_to_heading_angular_vel", params_.rotate_to_heading_angular_vel); - node->get_parameter("transform_tolerance", params_.transform_tolerance); node->get_parameter( plugin_name_ + ".use_velocity_scaled_lookahead_dist", params_.use_velocity_scaled_lookahead_dist); 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 faf5b87c646..683135b9f6f 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 @@ -159,7 +159,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity const geometry_msgs::msg::Twist & speed, nav2_core::GoalChecker * goal_checker, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & /*goal*/) + const geometry_msgs::msg::PoseStamped & /*global_goal*/) { std::lock_guard lock_reinit(param_handler_->getMutex()); diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index b9021de9265..94d62b13332 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -83,7 +83,7 @@ class RotationShimController : public nav2_core::Controller * @param velocity Current robot velocity * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands * @param transformed_global_plan The global plan after being processed by the path handler - * @param goal The last pose of the global plan + * @param global_goal The last pose of the global plan * @return Best command */ geometry_msgs::msg::TwistStamped computeVelocityCommands( @@ -91,7 +91,7 @@ class RotationShimController : public nav2_core::Controller const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * /*goal_checker*/, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) override; + const geometry_msgs::msg::PoseStamped & global_goal) override; /** * @brief nav2_core newPathReceived - Receives a new plan from the Planner Server diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 54bfe6a33c7..f77ee6165c4 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -170,7 +170,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker, nav_msgs::msg::Path & transformed_global_plan, - const geometry_msgs::msg::Pose & goal) + const geometry_msgs::msg::PoseStamped & global_goal) { // Rotate to goal heading when in goal xy tolerance if (rotate_to_goal_heading_) { @@ -260,7 +260,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands // If at this point, use the primary controller to path track in_rotation_ = false; auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker, - transformed_global_plan, goal); + transformed_global_plan, global_goal); last_angular_vel_ = cmd_vel.twist.angular.z; return cmd_vel; } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index a0b78fbd43d..142617a35ac 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -284,7 +284,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) path_handler.setPlan(path); tf_broadcaster->sendTransform(transform); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; auto effort = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); @@ -384,7 +384,7 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { controller->newPathReceived(path); path_handler.setPlan(path); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); @@ -462,7 +462,7 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { controller->newPathReceived(path); path_handler.setPlan(path); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); @@ -551,7 +551,7 @@ TEST(RotationShimControllerTest, accelerationTests) { controller->newPathReceived(path); path_handler.setPlan(path); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); - geometry_msgs::msg::Pose goal; + geometry_msgs::msg::PoseStamped goal; auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp index 88bf5c01770..cb0bc8bac4c 100644 --- a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -48,7 +48,7 @@ class UnknownErrorController : public nav2_core::Controller const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, nav_msgs::msg::Path &, - const geometry_msgs::msg::Pose &) + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::ControllerException("Unknown Error"); } @@ -63,7 +63,7 @@ class TFErrorController : public UnknownErrorController const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, nav_msgs::msg::Path &, - const geometry_msgs::msg::Pose &) + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::ControllerTFError("TF error"); } @@ -76,7 +76,7 @@ class FailedToMakeProgressErrorController : public UnknownErrorController const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, nav_msgs::msg::Path &, - const geometry_msgs::msg::Pose &) + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::FailedToMakeProgress("Failed to make progress"); } @@ -89,7 +89,7 @@ class PatienceExceededErrorController : public UnknownErrorController const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, nav_msgs::msg::Path &, - const geometry_msgs::msg::Pose &) + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::PatienceExceeded("Patience exceeded"); } @@ -102,7 +102,7 @@ class InvalidPathErrorController : public UnknownErrorController const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, nav_msgs::msg::Path &, - const geometry_msgs::msg::Pose &) + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::InvalidPath("Invalid path"); } @@ -115,7 +115,7 @@ class NoValidControlErrorController : public UnknownErrorController const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *, nav_msgs::msg::Path &, - const geometry_msgs::msg::Pose &) + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::NoValidControl("No valid control"); } From efc7617fd7f4479ff1d941e174cea4b933292b81 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 12 Oct 2025 14:11:10 +0000 Subject: [PATCH 31/62] Add api, fix dynamic parameters Signed-off-by: mini-1235 --- ...hrough_poses_w_replanning_and_recovery.xml | 3 +- .../nav2_controller/controller_server.hpp | 13 +--- .../plugins/simple_path_handler.hpp | 5 +- nav2_controller/plugins.xml | 4 +- nav2_controller/plugins/mppi_path_handler.cpp | 2 +- .../plugins/simple_path_handler.cpp | 22 ++++++- nav2_controller/src/controller_server.cpp | 25 +------- nav2_controller/src/parameter_handler.cpp | 26 ++------ nav2_controller/test/CMakeLists.txt | 17 +++-- .../test/test_dynamic_parameters.cpp | 64 +++++++++++++++++++ ...ller_path_handler.hpp => path_handler.hpp} | 21 ++++-- nav2_mppi_controller/src/controller.cpp | 3 +- nav2_simple_commander/README.md | 2 +- .../nav2_simple_commander/robot_navigator.py | 5 +- 14 files changed, 132 insertions(+), 80 deletions(-) create mode 100644 nav2_controller/test/test_dynamic_parameters.cpp rename nav2_core/include/nav2_core/{controller_path_handler.hpp => path_handler.hpp} (80%) 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 cb88cbb3eed..58ce11b91a0 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 @@ -9,6 +9,7 @@ + @@ -24,7 +25,7 @@ - + diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 5f42b5b5306..38088e6b942 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -25,7 +25,7 @@ #include "nav2_core/controller.hpp" #include "nav2_core/progress_checker.hpp" #include "nav2_core/goal_checker.hpp" -#include "nav2_core/controller_path_handler.hpp" +#include "nav2_core/path_handler.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "tf2_ros/transform_listener.hpp" #include "nav2_msgs/action/follow_path.hpp" @@ -54,7 +54,7 @@ class ControllerServer : public nav2::LifecycleNode using ControllerMap = std::unordered_map; using GoalCheckerMap = std::unordered_map; using ProgressCheckerMap = std::unordered_map; - using PathHandlerMap = std::unordered_map; + using PathHandlerMap = std::unordered_map; /** * @brief Constructor for nav2_controller::ControllerServer @@ -232,13 +232,6 @@ class ControllerServer : public nav2::LifecycleNode return twist_thresh; } - /** - * @brief Get the global goal pose transformed to the local frame - * @param stamp Time to get the goal pose at - * @return Transformed goal pose - */ - geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); - // The controller needs a costmap node std::shared_ptr costmap_ros_; std::unique_ptr costmap_thread_; @@ -264,7 +257,7 @@ class ControllerServer : public nav2::LifecycleNode std::string controller_ids_concat_, current_controller_; // Path Handler Plugins - pluginlib::ClassLoader path_handler_loader_; + pluginlib::ClassLoader path_handler_loader_; PathHandlerMap path_handlers_; std::string path_handler_ids_concat_, current_path_handler_; diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index 6d5cc7d7bbf..bf02619f4d2 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -21,7 +21,7 @@ #include #include #include -#include "nav2_core/controller_path_handler.hpp" +#include "nav2_core/path_handler.hpp" #include "nav2_ros_common/lifecycle_node.hpp" namespace nav2_controller @@ -34,7 +34,7 @@ using PathIterator = std::vector::iterator; * and transforming the resulting path into the robot base frame. */ -class SimplePathHandler : public nav2_core::ControllerPathHandler +class SimplePathHandler : public nav2_core::PathHandler { public: void initialize( @@ -46,6 +46,7 @@ class SimplePathHandler : public nav2_core::ControllerPathHandler void setPlan(const nav_msgs::msg::Path & path) override; nav_msgs::msg::Path transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) override; + geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp) override; protected: /** diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index 276cdda0242..ea599b069de 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -25,12 +25,12 @@ - + Handles the path from planner server and transform it to robot base frame - + Handles the path from planner server and transform it to costmap's global frame diff --git a/nav2_controller/plugins/mppi_path_handler.cpp b/nav2_controller/plugins/mppi_path_handler.cpp index 9a1f237644e..7b13bde539b 100644 --- a/nav2_controller/plugins/mppi_path_handler.cpp +++ b/nav2_controller/plugins/mppi_path_handler.cpp @@ -125,4 +125,4 @@ MPPIPathHandler::dynamicParametersCallback(std::vector parame } // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(nav2_controller::MPPIPathHandler, nav2_core::ControllerPathHandler) +PLUGINLIB_EXPORT_CLASS(nav2_controller::MPPIPathHandler, nav2_core::PathHandler) diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index 070f1fbc67a..d8f56faa04d 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -42,6 +42,7 @@ void SimplePathHandler::initialize( auto node = parent.lock(); costmap_ros_ = costmap_ros; tf_ = tf; + transform_tolerance_ = costmap_ros_->getTransformTolerance(); interpolate_curvature_after_goal_ = node->declare_or_get_parameter(plugin_name + ".interpolate_curvature_after_goal", false); max_robot_pose_search_dist_ = node->declare_or_get_parameter(plugin_name + @@ -52,7 +53,6 @@ void SimplePathHandler::initialize( 0.2); inversion_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + ".inversion_yaw_tolerance", 0.4); - transform_tolerance_ = costmap_ros_->getTransformTolerance(); if (max_robot_pose_search_dist_ < 0.0) { RCLCPP_WARN( logger_, "Max robot search distance is negative, setting to max to search" @@ -220,6 +220,24 @@ nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( return transformed_plan; } +geometry_msgs::msg::PoseStamped SimplePathHandler::getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) +{ + auto goal = global_plan_.poses.back(); + goal.header.frame_id = global_plan_.header.frame_id; + goal.header.stamp = stamp; + if (goal.header.frame_id.empty()) { + throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); + } + geometry_msgs::msg::PoseStamped transformed_goal; + if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *costmap_ros_->getTfBuffer(), + costmap_ros_->getGlobalFrameID(), transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); + } + return transformed_goal; +} + rcl_interfaces::msg::SetParametersResult SimplePathHandler::dynamicParametersCallback(std::vector parameters) { @@ -251,4 +269,4 @@ SimplePathHandler::dynamicParametersCallback(std::vector para } // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(nav2_controller::SimplePathHandler, nav2_core::ControllerPathHandler) +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimplePathHandler, nav2_core::PathHandler) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index da4e71362e5..ea83119c229 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -38,7 +38,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), lp_loader_("nav2_core", "nav2_core::Controller"), - path_handler_loader_("nav2_core", "nav2_core::ControllerPathHandler"), + path_handler_loader_("nav2_core", "nav2_core::PathHandler"), costmap_update_timeout_(300ms) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -161,7 +161,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) for (size_t i = 0; i != params_->path_handler_ids.size(); i++) { try { - nav2_core::ControllerPathHandler::Ptr path_handler = + nav2_core::PathHandler::Ptr path_handler = path_handler_loader_.createUniqueInstance(params_->path_handler_types[i]); RCLCPP_INFO( get_logger(), "Created path handler : %s of type %s", @@ -412,25 +412,6 @@ bool ControllerServer::findPathHandlerId( return true; } -geometry_msgs::msg::PoseStamped ControllerServer::getTransformedGoal( - const builtin_interfaces::msg::Time & stamp) -{ - auto goal = current_path_.poses.back(); - goal.header.frame_id = current_path_.header.frame_id; - goal.header.stamp = stamp; - if (goal.header.frame_id.empty()) { - throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); - } - geometry_msgs::msg::PoseStamped transformed_goal; - if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *costmap_ros_->getTfBuffer(), - costmap_ros_->getGlobalFrameID(), transform_tolerance_)) - { - throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); - } - return transformed_goal; -} - - void ControllerServer::computeControl() { std::lock_guard lock_reinit(param_handler_->getMutex()); @@ -645,7 +626,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); - geometry_msgs::msg::PoseStamped goal = getTransformedGoal(pose.header.stamp); + geometry_msgs::msg::PoseStamped goal = path_handlers_[current_path_handler_]->getTransformedGoal(pose.header.stamp); transformed_global_plan_ = path_handlers_[current_path_handler_]->transformGlobalPlan(pose); auto path = std::make_unique(transformed_global_plan_); transformed_plan_pub_->publish(std::move(path)); diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 436272235db..f2b73288fd3 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -147,14 +147,14 @@ ParameterHandler::ParameterHandler( } } - post_set_params_handler_ = node->add_post_set_parameters_callback( - std::bind( - &ParameterHandler::updateParametersCallback, - this, std::placeholders::_1)); on_set_params_handler_ = node->add_on_set_parameters_callback( std::bind( &ParameterHandler::validateParameterUpdatesCallback, this, std::placeholders::_1)); + post_set_params_handler_ = node->add_post_set_parameters_callback( + std::bind( + &ParameterHandler::updateParametersCallback, + this, std::placeholders::_1)); } ParameterHandler::~ParameterHandler() @@ -183,7 +183,7 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda continue; } if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (parameter.as_double() < 0.0) { + if (parameter.as_double() < 0.0 && param_name != "failure_tolerance") { RCLCPP_WARN( logger_, "The value of parameter '%s' is incorrectly set to %f, " "it should be >=0. Ignoring parameter update.", @@ -213,22 +213,6 @@ ParameterHandler::updateParametersCallback( params_.min_theta_velocity_threshold = parameter.as_double(); } else if (param_name == "failure_tolerance") { params_.failure_tolerance = parameter.as_double(); - } else if (param_name == "costmap_update_timeout") { - params_.costmap_update_timeout = parameter.as_double(); - } else if (param_name == "odom_duration") { - params_.odom_duration = parameter.as_double(); - } - } else if (param_type == ParameterType::PARAMETER_BOOL) { - if (param_name == "use_realtime_priority") { - params_.use_realtime_priority = parameter.as_bool(); - } else if (param_name == "publish_zero_velocity") { - params_.publish_zero_velocity = parameter.as_bool(); - } - } else if (param_type == ParameterType::PARAMETER_STRING) { - if (param_name == "speed_limit_topic") { - params_.speed_limit_topic = parameter.as_string(); - } else if (param_name == "odom_topic") { - params_.odom_topic = parameter.as_string(); } } } diff --git a/nav2_controller/test/CMakeLists.txt b/nav2_controller/test/CMakeLists.txt index edec8b53292..16d75498b43 100644 --- a/nav2_controller/test/CMakeLists.txt +++ b/nav2_controller/test/CMakeLists.txt @@ -1,10 +1,9 @@ -#TODO: add path handler and parameter handler test # Test dynamic parameters -# ament_add_gtest(path_handler_test -# path_handler_test.cpp -# ) -# target_link_libraries(path_handler_test -# ${library_name} -# nav2_util::nav2_util_core -# rclcpp::rclcpp -# ) +ament_add_gtest(test_dynamic_parameters + test_dynamic_parameters.cpp +) +target_link_libraries(test_dynamic_parameters + ${library_name} + nav2_util::nav2_util_core + rclcpp::rclcpp +) diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp new file mode 100644 index 00000000000..db7c750aa28 --- /dev/null +++ b/nav2_controller/test/test_dynamic_parameters.cpp @@ -0,0 +1,64 @@ +// Copyright (c) 2021, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include + +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_controller/controller_server.hpp" +#include "rclcpp/rclcpp.hpp" + +TEST(ControllerServerTest, test_dynamic_parameters) +{ + auto controller = std::make_shared(); + controller->configure(); + controller->activate(); + + auto rec_param = std::make_shared( + controller->get_node_base_interface(), controller->get_node_topics_interface(), + controller->get_node_graph_interface(), + controller->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("min_x_velocity_threshold", 100.0), + rclcpp::Parameter("min_y_velocity_threshold", 100.0), + rclcpp::Parameter("min_theta_velocity_threshold", 100.0), + rclcpp::Parameter("failure_tolerance", 5.0)}); + + rclcpp::spin_until_future_complete( + controller->get_node_base_interface(), + results); + + EXPECT_EQ(controller->get_parameter("min_x_velocity_threshold").as_double(), 100.0); + EXPECT_EQ(controller->get_parameter("min_y_velocity_threshold").as_double(), 100.0); + EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0); + EXPECT_EQ(controller->get_parameter("failure_tolerance").as_double(), 5.0); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(0, nullptr); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} diff --git a/nav2_core/include/nav2_core/controller_path_handler.hpp b/nav2_core/include/nav2_core/path_handler.hpp similarity index 80% rename from nav2_core/include/nav2_core/controller_path_handler.hpp rename to nav2_core/include/nav2_core/path_handler.hpp index 87a24b0e747..2fba5c46afc 100644 --- a/nav2_core/include/nav2_core/controller_path_handler.hpp +++ b/nav2_core/include/nav2_core/path_handler.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_CORE__CONTROLLER_PATH_HANDLER_HPP_ -#define NAV2_CORE__CONTROLLER_PATH_HANDLER_HPP_ +#ifndef NAV2_CORE__PATH_HANDLER_HPP_ +#define NAV2_CORE__PATH_HANDLER_HPP_ #include #include @@ -27,7 +27,7 @@ namespace nav2_core { /** - * @class ControllerPathHandler + * @class PathHandler * @brief Function-object for handling the path from Planner Server * * This class defines the plugin interface used by the Controller Server to manage the @@ -35,12 +35,12 @@ namespace nav2_core * path segments the robot has already traversed and transforming the remaining, * relevant portion of the path into the costmap's global or base frame. */ -class ControllerPathHandler +class PathHandler { public: - typedef std::shared_ptr Ptr; + typedef std::shared_ptr Ptr; - virtual ~ControllerPathHandler() {} + virtual ~PathHandler() {} /** * @brief Initialize any parameters from the NodeHandle @@ -67,8 +67,15 @@ class ControllerPathHandler */ virtual nav_msgs::msg::Path transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) = 0; + + /** + * @brief Get the global goal pose transformed to the desired frame + * @param stamp Time to get the goal pose at + * @return Transformed goal pose + */ + virtual geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp) = 0; }; } // namespace nav2_core -#endif // NAV2_CORE__CONTROLLER_PATH_HANDLER_HPP_ +#endif // NAV2_CORE__PATH_HANDLER_HPP_ diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index ed1e8d96978..bb8bff70f44 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -106,7 +106,8 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_global_plan, global_goal.pose, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_global_plan, global_goal.pose, + goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 52080849820..34763a48cf4 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -22,7 +22,7 @@ New as of September 2023: the simple navigator constructor will accept a `namesp | goThroughPoses(poses, behavior_tree='') | Requests the robot to drive through a set of poses (list of `PoseStamped`).| | goToPose(pose, behavior_tree='') | Requests the robot to drive to a pose (`PoseStamped`). | | followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. | -| followPath(path, controller_id='', goal_checker_id='') | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | +| followPath(path, controller_id='', goal_checker_id='', progress_checker_id='', path_handler_id='') | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | | spin(spin_dist=1.57, time_allowance=10, disable_collision_checks=False) | Requests the robot to performs an in-place rotation by a given angle. | | backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10, disable_collision_checks=False) | Requests the robot to back up by a given distance. | | cancelTask() | Cancel an ongoing task request.| diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index e907ceb3305..52e8219d164 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -525,7 +525,8 @@ def assistedTeleop(self, time_allowance: int = 30) -> Optional[RunningTask]: return RunningTask.ASSISTED_TELEOP def followPath(self, path: Path, controller_id: str = '', - goal_checker_id: str = '') -> Optional[RunningTask]: + goal_checker_id: str = '', progress_checker_id: str = '', + path_handler_id: str = '') -> Optional[RunningTask]: self.clearTaskError() """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") @@ -536,6 +537,8 @@ def followPath(self, path: Path, controller_id: str = '', goal_msg.path = path goal_msg.controller_id = controller_id goal_msg.goal_checker_id = goal_checker_id + goal_msg.progress_checker_id = progress_checker_id + goal_msg.path_handler_id = path_handler_id self.info('Executing path...') send_goal_future = self.follow_path_client.send_goal_async( From c3bc248ecdc307ba5eef5e688937568796343cae Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 12 Oct 2025 14:21:48 +0000 Subject: [PATCH 32/62] Linting Signed-off-by: mini-1235 --- .../include/nav2_controller/plugins/simple_path_handler.hpp | 3 ++- nav2_controller/src/controller_server.cpp | 3 ++- nav2_core/include/nav2_core/path_handler.hpp | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index bf02619f4d2..b75ab1259c7 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -46,7 +46,8 @@ class SimplePathHandler : public nav2_core::PathHandler void setPlan(const nav_msgs::msg::Path & path) override; nav_msgs::msg::Path transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) override; - geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp) override; + geometry_msgs::msg::PoseStamped getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) override; protected: /** diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ea83119c229..96e20dd1789 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -626,7 +626,8 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); - geometry_msgs::msg::PoseStamped goal = path_handlers_[current_path_handler_]->getTransformedGoal(pose.header.stamp); + geometry_msgs::msg::PoseStamped goal = + path_handlers_[current_path_handler_]->getTransformedGoal(pose.header.stamp); transformed_global_plan_ = path_handlers_[current_path_handler_]->transformGlobalPlan(pose); auto path = std::make_unique(transformed_global_plan_); transformed_plan_pub_->publish(std::move(path)); diff --git a/nav2_core/include/nav2_core/path_handler.hpp b/nav2_core/include/nav2_core/path_handler.hpp index 2fba5c46afc..cea4070662a 100644 --- a/nav2_core/include/nav2_core/path_handler.hpp +++ b/nav2_core/include/nav2_core/path_handler.hpp @@ -73,7 +73,8 @@ class PathHandler * @param stamp Time to get the goal pose at * @return Transformed goal pose */ - virtual geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp) = 0; + virtual geometry_msgs::msg::PoseStamped getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) = 0; }; } // namespace nav2_core From db3a49ec41e27a4105cd9e1280c7481de2fbfa7d Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 12 Oct 2025 17:08:06 +0000 Subject: [PATCH 33/62] Add mppi logic in simple path handler Signed-off-by: mini-1235 --- nav2_controller/CMakeLists.txt | 25 ---- .../plugins/mppi_path_handler.hpp | 68 ---------- .../plugins/simple_path_handler.hpp | 26 +++- nav2_controller/plugins.xml | 5 - nav2_controller/plugins/mppi_path_handler.cpp | 128 ------------------ .../plugins/simple_path_handler.cpp | 38 ++++-- nav2_controller/plugins/test/goal_checker.cpp | 18 +-- .../src/costmap_filters/keepout_params.yaml | 2 +- .../costmap_filters/speed_global_params.yaml | 2 +- .../costmap_filters/speed_local_params.yaml | 2 +- .../src/error_codes/error_code_param.yaml | 2 +- .../gps_navigation/nav2_no_map_params.yaml | 2 +- nav2_system_tests/src/route/CMakeLists.txt | 1 - .../src/route/test_route_launch.py | 3 - nav2_system_tests/src/system/CMakeLists.txt | 7 - .../src/system/nav2_system_params.yaml | 2 +- .../src/system/test_system_launch.py | 3 - .../test_system_with_obstacle_launch.py | 3 - .../src/system/test_wrong_init_pose_launch.py | 3 - 19 files changed, 58 insertions(+), 282 deletions(-) delete mode 100644 nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp delete mode 100644 nav2_controller/plugins/mppi_path_handler.cpp diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 7e33619f341..598c9959171 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -189,29 +189,6 @@ target_link_libraries(simple_path_handler PRIVATE pluginlib::pluginlib ) -add_library(mppi_path_handler SHARED plugins/mppi_path_handler.cpp) -target_include_directories(mppi_path_handler - PUBLIC - "$" - "$" -) -target_link_libraries(mppi_path_handler PUBLIC - ${geometry_msgs_TARGETS} - nav2_core::nav2_core - nav2_ros_common::nav2_ros_common - nav2_costmap_2d::nav2_costmap_2d_core - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle - ${rcl_interfaces_TARGETS} - simple_path_handler -) -target_link_libraries(mppi_path_handler PRIVATE - angles::angles - nav2_util::nav2_util_core - pluginlib::pluginlib - tf2::tf2 -) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -235,7 +212,6 @@ install(TARGETS simple_goal_checker stopped_goal_checker simple_path_handler - mppi_path_handler ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib @@ -258,7 +234,6 @@ ament_export_libraries(simple_progress_checker stopped_goal_checker position_goal_checker simple_path_handler - mppi_path_handler ${library_name}) ament_export_dependencies( geometry_msgs diff --git a/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp deleted file mode 100644 index 679549a72fe..00000000000 --- a/nav2_controller/include/nav2_controller/plugins/mppi_path_handler.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright (c) 2025 Maurice Alexander Purnawan -// -// 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_CONTROLLER__PLUGINS__MPPI_PATH_HANDLER_HPP_ -#define NAV2_CONTROLLER__PLUGINS__MPPI_PATH_HANDLER_HPP_ - -#include -#include -#include -#include -#include "nav2_controller/plugins/simple_path_handler.hpp" -#include "nav2_ros_common/lifecycle_node.hpp" - -namespace nav2_controller -{ -/** -* @class MPPIPathHandler -* @brief This plugin manages the global plan by clipping it to the local -* segment relevant to the controller—typically bounded by the local costmap size -* and transforming the resulting path into the costmap's global frame. -*/ - -class MPPIPathHandler : public SimplePathHandler -{ -public: - void initialize( - const nav2::LifecycleNode::WeakPtr & parent, - const rclcpp::Logger & logger, - const std::string & plugin_name, - const std::shared_ptr costmap_ros, - std::shared_ptr tf) override; - -protected: - /** - * @brief Get global plan within window of the local costmap size - * @param global_pose Robot pose - * @return plan transformed in the costmap's global frame and iterator to the first pose of the global - * plan (for pruning) - */ - std::pair getGlobalPlanConsideringBoundsInCostmapFrame( - const geometry_msgs::msg::PoseStamped & global_pose) override; - - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; - rclcpp::Logger logger_ {rclcpp::get_logger("Controller Server")}; - std::string plugin_name_; - double prune_distance_; - - /** - * @brief Callback executed when a parameter change is detected - * @param parameters list of changed parameters - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); -}; -} // namespace nav2_controller - -#endif // NAV2_CONTROLLER__PLUGINS__MPPI_PATH_HANDLER_HPP_ diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index b75ab1259c7..7d60036861f 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -27,6 +27,7 @@ namespace nav2_controller { using PathIterator = std::vector::iterator; +using PathSegment = std::pair; /** * @class SimplePathHandler * @brief This plugin manages the global plan by clipping it to the local @@ -59,13 +60,24 @@ class SimplePathHandler : public nav2_core::PathHandler const geometry_msgs::msg::PoseStamped & pose); /** - * @brief Get global plan within window of the local costmap size - * @param global_pose Robot pose - * @return plan transformed in the robot base frame frame and iterator to the first pose of the global - * plan + * @brief Finds the start and end iterators defining the segment of the global plan + * that should be used for local control. + * @param global_pose Robot's current pose in map frame + * @return PathSegment A pair of iterators representing the [start, end) range of the local plan segment. */ - virtual std::pair getGlobalPlanConsideringBoundsInCostmapFrame( - const geometry_msgs::msg::PoseStamped & global_pose); + PathSegment findPlanSegmentIterators(const geometry_msgs::msg::PoseStamped & global_pose); + + /** + * @brief Transforms a predefined segment of the global plan into the desired frame. + * @param global_pose Robot's current pose + * @param closest_point Iterator to the starting pose of the path segment. + * @param last_point Iterator to the ending pose of the path segment. + * @return nav_msgs::msg::Path The transformed local plan segment in the desired frame. + */ + nav_msgs::msg::Path transformLocalPlan( + const geometry_msgs::msg::PoseStamped & global_pose, + const PathIterator & closest_point, + const PathIterator & last_point); /** * Get the greatest extent of the costmap in meters from the center. @@ -97,7 +109,7 @@ class SimplePathHandler : public nav2_core::PathHandler nav_msgs::msg::Path global_plan_up_to_inversion_; unsigned int inversion_locale_{0u}; bool interpolate_curvature_after_goal_, enforce_path_inversion_; - double max_robot_pose_search_dist_, transform_tolerance_; + double max_robot_pose_search_dist_, transform_tolerance_, prune_distance_; float inversion_xy_tolerance_, inversion_yaw_tolerance_; /** diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index ea599b069de..e64a7b6a836 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -29,9 +29,4 @@ Handles the path from planner server and transform it to robot base frame - - - Handles the path from planner server and transform it to costmap's global frame - - diff --git a/nav2_controller/plugins/mppi_path_handler.cpp b/nav2_controller/plugins/mppi_path_handler.cpp deleted file mode 100644 index 7b13bde539b..00000000000 --- a/nav2_controller/plugins/mppi_path_handler.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// Copyright (c) 2025 Maurice Alexander Purnawan -// -// 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 "angles/angles.h" -#include "pluginlib/class_list_macros.hpp" -#include "nav2_controller/plugins/mppi_path_handler.hpp" -#include "nav2_util/controller_utils.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav2_core/controller_exceptions.hpp" - - -using rcl_interfaces::msg::ParameterType; -using std::placeholders::_1; - -namespace nav2_controller -{ -using nav2_util::geometry_utils::euclidean_distance; - -void MPPIPathHandler::initialize( - const nav2::LifecycleNode::WeakPtr & parent, - const rclcpp::Logger & logger, - const std::string & plugin_name, - const std::shared_ptr costmap_ros, - std::shared_ptr tf) -{ - logger_ = logger; - plugin_name_ = plugin_name; - SimplePathHandler::initialize(parent, logger, plugin_name, costmap_ros, tf); - auto node = parent.lock(); - prune_distance_ = node->declare_or_get_parameter(plugin_name + ".prune_distance", - 2.0); - - // Add callback for dynamic parameters - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&MPPIPathHandler::dynamicParametersCallback, this, _1)); -} - -std::pair -MPPIPathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( - const geometry_msgs::msg::PoseStamped & global_pose) -{ - auto begin = global_plan_up_to_inversion_.poses.begin(); - - // Limit the search for the closest pose up to max_robot_pose_search_dist on the path - auto closest_pose_upper_bound = - nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), - max_robot_pose_search_dist_); - - // Find closest point to the robot - auto closest_point = nav2_util::geometry_utils::min_by( - begin, closest_pose_upper_bound, - [&global_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(global_pose, ps); - }); - - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); - transformed_plan.header.stamp = global_pose.header.stamp; - - auto pruned_plan_end = - nav2_util::geometry_utils::first_after_integrated_distance( - closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); - - unsigned int mx, my; - // Find the furthest relevant pose on the path to consider within costmap - // bounds - // Transforming it to the costmap frame in the same loop - for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; - ++global_plan_pose) - { - // Transform from global plan frame to costmap frame - geometry_msgs::msg::PoseStamped costmap_plan_pose; - global_plan_pose->header.stamp = global_pose.header.stamp; - global_plan_pose->header.frame_id = global_plan_.header.frame_id; - nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_, - costmap_ros_->getGlobalFrameID(), transform_tolerance_); - - // Check if pose is inside the costmap - if (!costmap_ros_->getCostmap()->worldToMap( - costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) - { - return {transformed_plan, closest_point}; - } - - // Filling the transformed plan to return with the transformed pose - transformed_plan.poses.push_back(costmap_plan_pose); - } - - return {transformed_plan, closest_point}; -} - -rcl_interfaces::msg::SetParametersResult -MPPIPathHandler::dynamicParametersCallback(std::vector parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - for (auto parameter : parameters) { - const auto & param_type = parameter.get_type(); - const auto & param_name = parameter.get_name(); - if (param_name.find(plugin_name_ + ".") != 0) { - continue; - } - - if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == "prune_distance") { - prune_distance_ = parameter.as_double(); - } - } - } - result.successful = true; - return result; -} - -} // namespace nav2_controller - -PLUGINLIB_EXPORT_CLASS(nav2_controller::MPPIPathHandler, nav2_core::PathHandler) diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index d8f56faa04d..d2bf9031a0a 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -47,6 +47,8 @@ void SimplePathHandler::initialize( ".interpolate_curvature_after_goal", false); max_robot_pose_search_dist_ = node->declare_or_get_parameter(plugin_name + ".max_robot_pose_search_dist", getCostmapMaxExtent()); + prune_distance_ = node->declare_or_get_parameter(plugin_name + ".prune_distance", + 2.0); enforce_path_inversion_ = node->declare_or_get_parameter(plugin_name + ".enforce_path_inversion", false); inversion_xy_tolerance_ = node->declare_or_get_parameter(plugin_name + ".inversion_xy_tolerance", @@ -130,8 +132,7 @@ geometry_msgs::msg::PoseStamped SimplePathHandler::transformToGlobalPlanFrame( return robot_pose; } -std::pair -SimplePathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( +PathSegment SimplePathHandler::findPlanSegmentIterators( const geometry_msgs::msg::PoseStamped & global_pose) { // Limit the search for the closest pose up to max_robot_pose_search_dist on the path @@ -161,12 +162,25 @@ SimplePathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( } const double max_costmap_extent = getCostmapMaxExtent(); - auto pruned_plan_end = std::find_if( - closest_point, global_plan_up_to_inversion_.poses.end(), + auto transformation_end = std::find_if( + closest_point, global_plan_up_to_inversion_.poses.end(), [&](const auto & global_plan_pose) { return euclidean_distance(global_plan_pose, global_pose) > max_costmap_extent; - }); + }); + auto pruned_plan_end = + nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + auto last_point = std::min(transformation_end, pruned_plan_end); + + return {closest_point, last_point}; +} + +nav_msgs::msg::Path SimplePathHandler::transformLocalPlan( + const geometry_msgs::msg::PoseStamped & global_pose, + const PathIterator & closest_point, + const PathIterator & last_point) +{ // Lambda to transform a PoseStamped from global frame to local auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; @@ -174,7 +188,7 @@ SimplePathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( stamped_pose.header.stamp = global_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_, - costmap_ros_->getBaseFrameID(), transform_tolerance_)) + costmap_ros_->getGlobalFrameID(), transform_tolerance_)) { throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); } @@ -185,21 +199,21 @@ SimplePathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( // Transform the near part of the global plan into the robot's frame of reference. nav_msgs::msg::Path transformed_plan; std::transform( - closest_point, pruned_plan_end, + closest_point, last_point, std::back_inserter(transformed_plan.poses), transformGlobalPoseToLocal); - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); transformed_plan.header.stamp = global_pose.header.stamp; - return {transformed_plan, closest_point}; + return transformed_plan; } nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { geometry_msgs::msg::PoseStamped global_pose = transformToGlobalPlanFrame(pose); - auto [transformed_plan, - closest_point] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); + auto [closest_point, last_point] = findPlanSegmentIterators(global_pose); + auto transformed_plan = transformLocalPlan(global_pose, closest_point, last_point); // Remove the portion of the global plan that we've already passed so we don't // process it on the next iteration (this is called path pruning) @@ -256,6 +270,8 @@ SimplePathHandler::dynamicParametersCallback(std::vector para inversion_xy_tolerance_ = parameter.as_double(); } else if (param_name == "inversion_yaw_tolerance") { inversion_yaw_tolerance_ = parameter.as_double(); + } else if (param_name == "prune_distance") { + prune_distance_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == "enforce_path_inversion") { diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index b92545fcd36..5b8a56cb3bd 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -41,7 +41,6 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav_msgs/msg/path.hpp" -#include "eigen3/Eigen/Geometry" using nav2_controller::SimpleGoalChecker; using nav2_controller::StoppedGoalChecker; @@ -313,15 +312,9 @@ TEST(StoppedGoalChecker, is_reached) // Current angular position is tolerance away from goal - auto quat = - (Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs(); - // epsilon for orientation is a lot bigger than double limit, probably from TF getYaw - auto quat_epsilon = - (Eigen::AngleAxisd::Identity() * - Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs(); - - current_pose.orientation.z = quat[2]; - current_pose.orientation.w = quat[3]; + tf2::Quaternion q; + q.setRPY(0, 0, 0.25); + current_pose.orientation = tf2::toMsg(q); velocity.angular.z = 0.25; EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); @@ -336,8 +329,9 @@ TEST(StoppedGoalChecker, is_reached) gc.reset(); // Current angular position is further than tolerance away from goal - current_pose.orientation.z = quat_epsilon[2]; - current_pose.orientation.w = quat_epsilon[3]; + tf2::Quaternion q2; + q2.setRPY(0, 0, 0.25 + 1e-15); // slightly offset yaw + current_pose.orientation = tf2::toMsg(q2); velocity.angular.z = 0.25; EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 4d38e792fca..9b1f2bee5df 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -76,7 +76,7 @@ controller_server: yaw_goal_tolerance: 0.25 stateful: True path_handler: - plugin: "nav2_controller::MPPIPathHandler" + plugin: "nav2_controller::SimplePathHandler" prune_distance: 1.7 # DWB parameters FollowPath: 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 70066e06314..4e2f199d145 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -77,7 +77,7 @@ controller_server: yaw_goal_tolerance: 0.25 stateful: True path_handler: - plugin: "nav2_controller::MPPIPathHandler" + plugin: "nav2_controller::SimplePathHandler" prune_distance: 1.7 # DWB parameters FollowPath: 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 81fc902bbad..a76b8c5e40d 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -77,7 +77,7 @@ controller_server: yaw_goal_tolerance: 0.25 stateful: True path_handler: - plugin: "nav2_controller::MPPIPathHandler" + plugin: "nav2_controller::SimplePathHandler" prune_distance: 1.7 # DWB parameters FollowPath: diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 3d8e54987fd..b0c5b14dca2 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -41,7 +41,7 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 path_handler: - plugin: "nav2_controller::MPPIPathHandler" + plugin: "nav2_controller::SimplePathHandler" prune_distance: 1.7 # DWB parameters FollowPath: 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 8ef51ae22f7..cce98e033b7 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 @@ -55,7 +55,7 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 path_handler: - plugin: "nav2_controller::MPPIPathHandler" + plugin: "nav2_controller::SimplePathHandler" prune_distance: 1.7 # MPPI parameters FollowPath: diff --git a/nav2_system_tests/src/route/CMakeLists.txt b/nav2_system_tests/src/route/CMakeLists.txt index f28023d89f8..a69bbac70a4 100644 --- a/nav2_system_tests/src/route/CMakeLists.txt +++ b/nav2_system_tests/src/route/CMakeLists.txt @@ -8,7 +8,6 @@ ament_add_test(test_route BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=tester_node.py ASTAR=True - PATH_HANDLER=nav2_controller::SimplePathHandler CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController PLANNER=nav2_navfn_planner::NavfnPlanner ) diff --git a/nav2_system_tests/src/route/test_route_launch.py b/nav2_system_tests/src/route/test_route_launch.py index 008d5859d6f..6a454563894 100755 --- a/nav2_system_tests/src/route/test_route_launch.py +++ b/nav2_system_tests/src/route/test_route_launch.py @@ -62,9 +62,6 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} ) - param_substitutions.update( - {'controller_server.ros__parameters.path_handler.plugin': os.getenv('PATH_HANDLER', '')} - ) param_substitutions.update( {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} ) diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index b8e3a2ab610..3db27c6398e 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -8,7 +8,6 @@ ament_add_test(test_bt_navigator BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=True - PATH_HANDLER=nav2_controller::SimplePathHandler CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -23,7 +22,6 @@ ament_add_test(test_bt_navigator_with_wrong_init_pose BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=True - PATH_HANDLER=nav2_controller::SimplePathHandler CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -38,7 +36,6 @@ ament_add_test(test_bt_navigator_with_dijkstra BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False - PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -54,7 +51,6 @@ ament_add_test(test_bt_navigator_with_groot_monitoring TESTER=nav_to_pose_tester_node.py ASTAR=False GROOT_MONITORING=True - PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -70,7 +66,6 @@ ament_add_test(test_dynamic_obstacle BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False - PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -86,7 +81,6 @@ ament_add_test(test_nav_through_poses BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml TESTER=nav_through_poses_tester_node.py ASTAR=False - PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) @@ -102,7 +96,6 @@ ament_add_test(test_nav_through_poses_error_msg BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml TESTER=nav_through_poses_tester_error_msg_node.py ASTAR=False - PATH_HANDLER=nav2_controller::MPPIPathHandler CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index f2f0a0cb84b..83da81da7cf 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -99,7 +99,7 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 path_handler: - plugin: "nav2_controller::MPPIPathHandler" + plugin: "nav2_controller::SimplePathHandler" prune_distance: 1.7 # DWB parameters FollowPath: diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 2fea7871a8a..a1b13bcbaef 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -67,9 +67,6 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} ) - param_substitutions.update( - {'controller_server.ros__parameters.path_handler.plugin': os.getenv('PATH_HANDLER', '')} - ) param_substitutions.update( {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} ) diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index e793e4748f5..095066ea739 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -65,9 +65,6 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} ) - param_substitutions.update( - {'controller_server.ros__parameters.path_handler.plugin': os.getenv('PATH_HANDLER', '')} - ) param_substitutions.update( {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} ) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 864f3fbde00..bef573bca71 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -67,9 +67,6 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} ) - param_substitutions.update( - {'controller_server.ros__parameters.path_handler.plugin': os.getenv('PATH_HANDLER', '')} - ) param_substitutions.update( {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} ) From 9eedef447b94067d10ceceb80af4d7c406260b41 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Mon, 13 Oct 2025 11:17:12 +0000 Subject: [PATCH 34/62] Simplify test logic Signed-off-by: mini-1235 --- nav2_controller/plugins/test/goal_checker.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 5b8a56cb3bd..883125aa5db 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -310,11 +310,8 @@ TEST(StoppedGoalChecker, is_reached) current_pose.position.x = 0.0; velocity.linear.x = 0.0; - // Current angular position is tolerance away from goal - tf2::Quaternion q; - q.setRPY(0, 0, 0.25); - current_pose.orientation = tf2::toMsg(q); + current_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.25); velocity.angular.z = 0.25; EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); @@ -329,9 +326,7 @@ TEST(StoppedGoalChecker, is_reached) gc.reset(); // Current angular position is further than tolerance away from goal - tf2::Quaternion q2; - q2.setRPY(0, 0, 0.25 + 1e-15); // slightly offset yaw - current_pose.orientation = tf2::toMsg(q2); + current_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.25 + 1e-15); velocity.angular.z = 0.25; EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); From 7a2944c35c5b2ff1a39a7c6453ab8c8cf2a31965 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Mon, 13 Oct 2025 14:33:21 +0000 Subject: [PATCH 35/62] Minor fix Signed-off-by: mini-1235 --- .../nav2_controller/parameter_handler.hpp | 2 +- .../plugins/position_goal_checker.cpp | 2 +- .../plugins/simple_goal_checker.cpp | 2 +- nav2_controller/src/controller_server.cpp | 55 +++++++++---------- nav2_mppi_controller/src/controller.cpp | 1 - .../src/costmap_filters/keepout_params.yaml | 2 - .../costmap_filters/speed_global_params.yaml | 2 - .../costmap_filters/speed_local_params.yaml | 2 - .../src/error_codes/error_code_param.yaml | 2 - .../gps_navigation/nav2_no_map_params.yaml | 2 - .../src/system/nav2_system_params.yaml | 2 - 11 files changed, 30 insertions(+), 44 deletions(-) diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index 97e09271d01..4205e9b2ba4 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -93,7 +93,7 @@ class ParameterHandler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_; Parameters params_; std::string plugin_name_; - rclcpp::Logger logger_ {rclcpp::get_logger("Controller Server")}; + rclcpp::Logger logger_ {rclcpp::get_logger("ControllerServer")}; const std::vector default_progress_checker_ids_{"progress_checker"}; const std::vector default_progress_checker_types_{ diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index 5ae371b910d..a4b824ed157 100644 --- a/nav2_controller/plugins/position_goal_checker.cpp +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -64,7 +64,7 @@ bool PositionGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path & transformed_global_plan) { - // If the local plan is longer than the tolerance, we skip the check + // If the local plan length is longer than the tolerance, we skip the check if (nav2_util::geometry_utils::calculate_path_length(transformed_global_plan) > path_length_tolerance_) { diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 641d4d4c56b..1b6eef43fd0 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -92,7 +92,7 @@ bool SimpleGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path & transformed_global_plan) { - // If the local plan is longer than the tolerance, we skip the check + // If the local plan length is longer than the tolerance, we skip the check if (nav2_util::geometry_utils::calculate_path_length(transformed_global_plan) > path_length_tolerance_) { diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 43199d2203b..6bc90aae6e5 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -132,61 +132,61 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO( get_logger(), "Controller Server has %s goal checkers available.", goal_checker_ids_concat_.c_str()); - - for (size_t i = 0; i != params_->controller_ids.size(); i++) { + + for (size_t i = 0; i != params_->path_handler_ids.size(); i++) { try { - nav2_core::Controller::Ptr controller = - lp_loader_.createUniqueInstance(params_->controller_types[i]); + nav2_core::PathHandler::Ptr path_handler = + path_handler_loader_.createUniqueInstance(params_->path_handler_types[i]); RCLCPP_INFO( - get_logger(), "Created controller : %s of type %s", - params_->controller_ids[i].c_str(), params_->controller_types[i].c_str()); - controller->configure( - node, params_->controller_ids[i], - costmap_ros_->getTfBuffer(), costmap_ros_); - controllers_.insert({params_->controller_ids[i], controller}); + get_logger(), "Created path handler : %s of type %s", + params_->path_handler_ids[i].c_str(), params_->path_handler_types[i].c_str()); + path_handler->initialize(node, get_logger(), params_->path_handler_ids[i], costmap_ros_, + costmap_ros_->getTfBuffer()); + path_handlers_.insert({params_->path_handler_ids[i], path_handler}); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( get_logger(), - "Failed to create controller. Exception: %s", ex.what()); + "Failed to create path handler Exception: %s", ex.what()); on_cleanup(state); return nav2::CallbackReturn::FAILURE; } } - for (size_t i = 0; i != params_->controller_ids.size(); i++) { - controller_ids_concat_ += params_->controller_ids[i] + std::string(" "); + for (size_t i = 0; i != params_->path_handler_ids.size(); i++) { + path_handler_ids_concat_ += params_->path_handler_ids[i] + std::string(" "); } RCLCPP_INFO( get_logger(), - "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); + "Controller Server has %s path handlers available.", path_handler_ids_concat_.c_str()); - for (size_t i = 0; i != params_->path_handler_ids.size(); i++) { + for (size_t i = 0; i != params_->controller_ids.size(); i++) { try { - nav2_core::PathHandler::Ptr path_handler = - path_handler_loader_.createUniqueInstance(params_->path_handler_types[i]); + nav2_core::Controller::Ptr controller = + lp_loader_.createUniqueInstance(params_->controller_types[i]); RCLCPP_INFO( - get_logger(), "Created path handler : %s of type %s", - params_->path_handler_ids[i].c_str(), params_->path_handler_types[i].c_str()); - path_handler->initialize(node, get_logger(), params_->path_handler_ids[i], costmap_ros_, - costmap_ros_->getTfBuffer()); - path_handlers_.insert({params_->path_handler_ids[i], path_handler}); + get_logger(), "Created controller : %s of type %s", + params_->controller_ids[i].c_str(), params_->controller_types[i].c_str()); + controller->configure( + node, params_->controller_ids[i], + costmap_ros_->getTfBuffer(), costmap_ros_); + controllers_.insert({params_->controller_ids[i], controller}); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( get_logger(), - "Failed to create path handler Exception: %s", ex.what()); + "Failed to create controller. Exception: %s", ex.what()); on_cleanup(state); return nav2::CallbackReturn::FAILURE; } } - for (size_t i = 0; i != params_->path_handler_ids.size(); i++) { - path_handler_ids_concat_ += params_->path_handler_ids[i] + std::string(" "); + for (size_t i = 0; i != params_->controller_ids.size(); i++) { + controller_ids_concat_ += params_->controller_ids[i] + std::string(" "); } RCLCPP_INFO( get_logger(), - "Controller Server has %s path handlers available.", path_handler_ids_concat_.c_str()); + "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); odom_sub_ = std::make_unique(node, params_->odom_duration, params_->odom_topic); @@ -236,7 +236,6 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) transformed_plan_pub_->on_activate(); tracking_feedback_pub_->on_activate(); action_server_->activate(); - auto node = shared_from_this(); // create bond connection @@ -699,7 +698,7 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; if (!nav2_util::transformPoseInTargetFrame( pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), - current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) + current_path_.header.frame_id, transform_tolerance_)) { throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); } diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index bb8bff70f44..1b9d6da8e62 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -34,7 +34,6 @@ void MPPIController::configure( parameters_handler_ = std::make_unique(parent, name_); auto node = parent_.lock(); - auto getParentParam = parameters_handler_->getParamGetter(""); // Get high-level controller parameters auto getParam = parameters_handler_->getParamGetter(name_); getParam(visualize_, "visualize", false); diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 9b1f2bee5df..06d6e123ed5 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -55,7 +55,6 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 - transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -77,7 +76,6 @@ controller_server: stateful: True path_handler: plugin: "nav2_controller::SimplePathHandler" - prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" 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 4e2f199d145..ff7595e9256 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -55,7 +55,6 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 - transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -78,7 +77,6 @@ controller_server: stateful: True path_handler: plugin: "nav2_controller::SimplePathHandler" - prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" 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 a76b8c5e40d..1b8f39b871f 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -55,7 +55,6 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 - transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -78,7 +77,6 @@ controller_server: stateful: True path_handler: plugin: "nav2_controller::SimplePathHandler" - prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index b0c5b14dca2..1aa7a7229ab 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -1,7 +1,6 @@ controller_server: ros__parameters: controller_frequency: 20.0 - transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.2 @@ -42,7 +41,6 @@ controller_server: yaw_goal_tolerance: 0.25 path_handler: plugin: "nav2_controller::SimplePathHandler" - prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" 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 cce98e033b7..760dfb7d029 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 @@ -28,7 +28,6 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 - transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -56,7 +55,6 @@ controller_server: yaw_goal_tolerance: 0.25 path_handler: plugin: "nav2_controller::SimplePathHandler" - prune_distance: 1.7 # MPPI parameters FollowPath: plugin: "nav2_mppi_controller::MPPIController" diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 83da81da7cf..fa9f655a6a7 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -71,7 +71,6 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 - transform_tolerance: 0.1 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -100,7 +99,6 @@ controller_server: yaw_goal_tolerance: 0.25 path_handler: plugin: "nav2_controller::SimplePathHandler" - prune_distance: 1.7 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" From 6d91c7ed7409da15ee86aad9c06b8f2be4fe3f91 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Mon, 13 Oct 2025 14:39:15 +0000 Subject: [PATCH 36/62] Linting Signed-off-by: mini-1235 --- nav2_controller/src/controller_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 6bc90aae6e5..5d6e3d53aff 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -132,7 +132,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO( get_logger(), "Controller Server has %s goal checkers available.", goal_checker_ids_concat_.c_str()); - + for (size_t i = 0; i != params_->path_handler_ids.size(); i++) { try { nav2_core::PathHandler::Ptr path_handler = From 8a6cd692c9b69a65325e1c1bc358e7a52a2ff8c9 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 15 Oct 2025 11:16:20 +0000 Subject: [PATCH 37/62] Add output port in FollowPath Signed-off-by: mini-1235 --- .../plugins/action/follow_path_action.hpp | 9 ++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 1 + .../plugins/action/follow_path_action.cpp | 17 ++++++++++- .../behavior_trees/follow_point.xml | 2 +- ...replanning_and_if_path_becomes_invalid.xml | 2 +- .../navigate_on_route_graph_w_recovery.xml | 2 +- ...hrough_poses_w_replanning_and_recovery.xml | 2 +- ...gate_to_pose_w_replanning_and_recovery.xml | 2 +- ..._replanning_goal_patience_and_recovery.xml | 2 +- ...eplanning_only_if_path_becomes_invalid.xml | 2 +- .../navigate_w_replanning_distance.xml | 2 +- ...e_w_replanning_only_if_goal_is_updated.xml | 2 +- ...eplanning_only_if_path_becomes_invalid.xml | 2 +- .../navigate_w_replanning_speed.xml | 2 +- .../navigate_w_replanning_time.xml | 2 +- ...global_planning_and_control_w_recovery.xml | 2 +- .../navigators/navigate_through_poses.hpp | 2 ++ .../navigators/navigate_to_pose.hpp | 2 ++ .../src/navigators/navigate_through_poses.cpp | 30 ++++--------------- .../src/navigators/navigate_to_pose.cpp | 30 ++++--------------- .../plugins/simple_path_handler.cpp | 2 +- 21 files changed, 56 insertions(+), 63 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index 492dbd8d8a5..b9980d07cd7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -80,6 +80,11 @@ class FollowPathAction : public BtActionNode */ void on_wait_for_result( std::shared_ptr feedback) override; + + /** + * @brief Function to set all feedbacks and output ports to be null values + */ + void resetFeedbackAndOutputPorts(); /** * @brief Creates list of BT ports @@ -97,12 +102,16 @@ class FollowPathAction : public BtActionNode BT::InputPort("goal_checker_id", ""), BT::InputPort("progress_checker_id", ""), BT::InputPort("path_handler_id", ""), + BT::OutputPort("tracking_feedback", "Tracking feedback from controller server"), BT::OutputPort( "error_code_id", "The follow path error code"), BT::OutputPort( "error_msg", "The follow path error msg"), }); } + +protected: + Action::Feedback feedback_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index c65fc774b90..2c8a0f6fa76 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -183,6 +183,7 @@ Path handler Action server name Server timeout + Tracking feedback from controller server Follow Path error code Follow Path error message diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 4184e68bf4c..4e7181ff95e 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -39,6 +39,7 @@ void FollowPathAction::on_tick() BT::NodeStatus FollowPathAction::on_success() { + resetFeedbackAndOutputPorts(); setOutput("error_code_id", ActionResult::NONE); setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; @@ -46,6 +47,7 @@ BT::NodeStatus FollowPathAction::on_success() BT::NodeStatus FollowPathAction::on_aborted() { + resetFeedbackAndOutputPorts(); setOutput("error_code_id", result_.result->error_code); setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; @@ -53,6 +55,7 @@ BT::NodeStatus FollowPathAction::on_aborted() BT::NodeStatus FollowPathAction::on_cancelled() { + resetFeedbackAndOutputPorts(); // Set empty error code, action was cancelled setOutput("error_code_id", ActionResult::NONE); setOutput("error_msg", ""); @@ -66,7 +69,7 @@ void FollowPathAction::on_timeout() } void FollowPathAction::on_wait_for_result( - std::shared_ptr/*feedback*/) + std::shared_ptr feedback) { // Grab the new path nav_msgs::msg::Path new_path; @@ -110,6 +113,18 @@ void FollowPathAction::on_wait_for_result( goal_.path_handler_id = new_path_handler_id; goal_updated_ = true; } + + if (feedback) { + feedback_ = *feedback; + setOutput("tracking_feedback", feedback_.tracking_feedback); + } +} + +void FollowPathAction::resetFeedbackAndOutputPorts() +{ + nav2_msgs::msg::TrackingFeedback empty_feedback; + feedback_.tracking_feedback = empty_feedback; + setOutput("tracking_feedback", feedback_.tracking_feedback); } } // namespace nav2_behavior_tree diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index 5d5310f79cd..ab0ff781fbe 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -16,7 +16,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index f2b6589d927..54048f1c171 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -27,7 +27,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml index 44662cb4656..6560dc4c441 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml @@ -65,7 +65,7 @@ - + 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 58ce11b91a0..7573a778354 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 @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 3fb6e4e5ff7..a0f1e1741ba 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -20,7 +20,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index f79984b750e..7ddbf6297dd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -27,7 +27,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index c13b2629dea..139da7d9a93 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index fa39ff05283..c500a3ca2e9 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index f64063bbdcb..d3735fa9447 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index 1607057c0e2..ad56f441325 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -17,7 +17,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 9c9342e8bbd..db512b294bd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 2e43016775d..645500fe482 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml index 4692c9e17b0..e8cb406c8df 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml @@ -64,7 +64,7 @@ - + diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 427373ded47..524ce0b0e50 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -23,6 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" +#include "nav2_msgs/msg/tracking_feedback.hpp" #include "nav2_msgs/msg/waypoint_status.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" @@ -110,6 +111,7 @@ class NavigateThroughPosesNavigator rclcpp::Time start_time_; std::string goals_blackboard_id_; std::string path_blackboard_id_; + std::string tracking_feedback_blackboard_id_; std::string waypoint_statuses_blackboard_id_; // Odometry smoother object diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 9b9290095a7..0d7d89de827 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -23,6 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/msg/tracking_feedback.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav_msgs/msg/path.hpp" @@ -127,6 +128,7 @@ class NavigateToPoseNavigator std::string goal_blackboard_id_; std::string path_blackboard_id_; + std::string tracking_feedback_blackboard_id_; // Odometry smoother object std::shared_ptr odom_smoother_; diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 92b6dfb7320..28a5d98acd5 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -34,6 +34,8 @@ NavigateThroughPosesNavigator::configure( node->declare_or_get_parameter(getName() + ".goals_blackboard_id", std::string("goals")); path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path")); + tracking_feedback_blackboard_id_ = + node->declare_or_get_parameter(getName() + ".tracking_feedback_blackboard_id", std::string("tracking_feedback")); waypoint_statuses_blackboard_id_ = node->declare_or_get_parameter(getName() + ".waypoint_statuses_blackboard_id", std::string("waypoint_statuses")); @@ -148,30 +150,10 @@ NavigateThroughPosesNavigator::onLoop() return; } - // Get current path points - nav_msgs::msg::Path current_path; - res = blackboard->get(path_blackboard_id_, current_path); - if (res && current_path.poses.size() > 0u) { - // Find the closest pose to current pose on global path - auto find_closest_pose_idx = - [¤t_pose, ¤t_path]() { - size_t closest_pose_idx = 0; - double curr_min_dist = std::numeric_limits::max(); - for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { - double curr_dist = nav2_util::geometry_utils::euclidean_distance( - current_pose, current_path.poses[curr_idx]); - if (curr_dist < curr_min_dist) { - curr_min_dist = curr_dist; - closest_pose_idx = curr_idx; - } - } - return closest_pose_idx; - }; - - // Calculate distance on the path - double distance_remaining = - nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx()); - + nav2_msgs::msg::TrackingFeedback tracking_feedback; + res = blackboard->get(tracking_feedback_blackboard_id_, tracking_feedback); + if (res) { + double distance_remaining = tracking_feedback.remaining_path_length; // Default value for time remaining rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0); diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index d4643a09ac8..becb5b32244 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -33,6 +33,8 @@ NavigateToPoseNavigator::configure( std::string("goal")); path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path")); + tracking_feedback_blackboard_id_ = + node->declare_or_get_parameter(getName() + ".tracking_feedback_blackboard_id", std::string("tracking_feedback")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; @@ -129,30 +131,10 @@ NavigateToPoseNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); - // Get current path points - nav_msgs::msg::Path current_path; - auto res = blackboard->get(path_blackboard_id_, current_path); - if (res && current_path.poses.size() > 0u) { - // Find the closest pose to current pose on global path - auto find_closest_pose_idx = - [¤t_pose, ¤t_path]() { - size_t closest_pose_idx = 0; - double curr_min_dist = std::numeric_limits::max(); - for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { - double curr_dist = nav2_util::geometry_utils::euclidean_distance( - current_pose, current_path.poses[curr_idx]); - if (curr_dist < curr_min_dist) { - curr_min_dist = curr_dist; - closest_pose_idx = curr_idx; - } - } - return closest_pose_idx; - }; - - // Calculate distance on the path - double distance_remaining = - nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx()); - + nav2_msgs::msg::TrackingFeedback tracking_feedback; + auto res = blackboard->get(tracking_feedback_blackboard_id_, tracking_feedback); + if (res) { + double distance_remaining = tracking_feedback.remaining_path_length; // Default value for time remaining rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0); diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index d2bf9031a0a..2d25ca76717 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -75,7 +75,7 @@ double SimplePathHandler::getCostmapMaxExtent() const const double max_costmap_dim_meters = std::max( costmap_ros_->getCostmap()->getSizeInCellsX(), costmap_ros_->getCostmap()->getSizeInCellsY()); - return max_costmap_dim_meters / 2.0; + return max_costmap_dim_meters * costmap_ros_->getCostmap()->getResolution() / 2.0; } void SimplePathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) From 2261466d6f9795be3b29f1b040fca5417c8d402f Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 15 Oct 2025 11:20:37 +0000 Subject: [PATCH 38/62] Linting Signed-off-by: mini-1235 --- .../nav2_behavior_tree/plugins/action/follow_path_action.hpp | 5 +++-- nav2_bt_navigator/src/navigators/navigate_through_poses.cpp | 5 +++-- nav2_bt_navigator/src/navigators/navigate_to_pose.cpp | 5 +++-- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index b9980d07cd7..378801884a2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -80,7 +80,7 @@ class FollowPathAction : public BtActionNode */ void on_wait_for_result( std::shared_ptr feedback) override; - + /** * @brief Function to set all feedbacks and output ports to be null values */ @@ -102,7 +102,8 @@ class FollowPathAction : public BtActionNode BT::InputPort("goal_checker_id", ""), BT::InputPort("progress_checker_id", ""), BT::InputPort("path_handler_id", ""), - BT::OutputPort("tracking_feedback", "Tracking feedback from controller server"), + BT::OutputPort("tracking_feedback", + "Tracking feedback from controller server"), BT::OutputPort( "error_code_id", "The follow path error code"), BT::OutputPort( diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 28a5d98acd5..f85b884add1 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -34,8 +34,9 @@ NavigateThroughPosesNavigator::configure( node->declare_or_get_parameter(getName() + ".goals_blackboard_id", std::string("goals")); path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path")); - tracking_feedback_blackboard_id_ = - node->declare_or_get_parameter(getName() + ".tracking_feedback_blackboard_id", std::string("tracking_feedback")); + tracking_feedback_blackboard_id_ = + node->declare_or_get_parameter(getName() + ".tracking_feedback_blackboard_id", + std::string("tracking_feedback")); waypoint_statuses_blackboard_id_ = node->declare_or_get_parameter(getName() + ".waypoint_statuses_blackboard_id", std::string("waypoint_statuses")); diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index becb5b32244..df190b88f3f 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -33,8 +33,9 @@ NavigateToPoseNavigator::configure( std::string("goal")); path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path")); - tracking_feedback_blackboard_id_ = - node->declare_or_get_parameter(getName() + ".tracking_feedback_blackboard_id", std::string("tracking_feedback")); + tracking_feedback_blackboard_id_ = + node->declare_or_get_parameter(getName() + ".tracking_feedback_blackboard_id", + std::string("tracking_feedback")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; From 66f91c860b61e6c6e4ffb7589efa45d1dd2864d5 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 15 Oct 2025 14:49:15 +0000 Subject: [PATCH 39/62] Always return local plan in odom frame Signed-off-by: mini-1235 --- .../plugins/simple_path_handler.hpp | 8 +-- .../plugins/simple_path_handler.cpp | 59 +++++++++---------- .../src/graceful_controller.cpp | 44 +++++++++++--- .../src/regulated_pure_pursuit_controller.cpp | 36 +++++++++-- 4 files changed, 100 insertions(+), 47 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index 7d60036861f..9b26349c3ef 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -68,16 +68,16 @@ class SimplePathHandler : public nav2_core::PathHandler PathSegment findPlanSegmentIterators(const geometry_msgs::msg::PoseStamped & global_pose); /** - * @brief Transforms a predefined segment of the global plan into the desired frame. + * @brief Transforms a predefined segment of the global plan into the odom frame. * @param global_pose Robot's current pose * @param closest_point Iterator to the starting pose of the path segment. - * @param last_point Iterator to the ending pose of the path segment. - * @return nav_msgs::msg::Path The transformed local plan segment in the desired frame. + * @param pruned_plan_end Iterator to the ending pose of the path segment. + * @return nav_msgs::msg::Path The transformed local plan segment in the odom frame. */ nav_msgs::msg::Path transformLocalPlan( const geometry_msgs::msg::PoseStamped & global_pose, const PathIterator & closest_point, - const PathIterator & last_point); + const PathIterator & pruned_plan_end); /** * Get the greatest extent of the costmap in meters from the center. diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index 2d25ca76717..327f93b7002 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -161,49 +161,46 @@ PathSegment SimplePathHandler::findPlanSegmentIterators( closest_point = std::prev(std::prev(closest_pose_upper_bound)); } - const double max_costmap_extent = getCostmapMaxExtent(); - auto transformation_end = std::find_if( - closest_point, global_plan_up_to_inversion_.poses.end(), - [&](const auto & global_plan_pose) { - return euclidean_distance(global_plan_pose, global_pose) > max_costmap_extent; - }); auto pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); - auto last_point = std::min(transformation_end, pruned_plan_end); - return {closest_point, last_point}; + return {closest_point, pruned_plan_end}; } nav_msgs::msg::Path SimplePathHandler::transformLocalPlan( const geometry_msgs::msg::PoseStamped & global_pose, const PathIterator & closest_point, - const PathIterator & last_point) + const PathIterator & pruned_plan_end) { - // Lambda to transform a PoseStamped from global frame to local - auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_up_to_inversion_.header.frame_id; - stamped_pose.header.stamp = global_pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_, - costmap_ros_->getGlobalFrameID(), transform_tolerance_)) - { - throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); - } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - // Transform the near part of the global plan into the robot's frame of reference. nav_msgs::msg::Path transformed_plan; - std::transform( - closest_point, last_point, - std::back_inserter(transformed_plan.poses), - transformGlobalPoseToLocal); transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(); transformed_plan.header.stamp = global_pose.header.stamp; + unsigned int mx, my; + // Find the furthest relevant pose on the path to consider within costmap + // bounds + // Transforming it to the costmap frame in the same loop + for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; + ++global_plan_pose) + { + // Transform from global plan frame to costmap frame + geometry_msgs::msg::PoseStamped costmap_plan_pose; + global_plan_pose->header.stamp = global_pose.header.stamp; + global_plan_pose->header.frame_id = global_plan_.header.frame_id; + nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_, + costmap_ros_->getGlobalFrameID(), transform_tolerance_); + + // Check if pose is inside the costmap + if (!costmap_ros_->getCostmap()->worldToMap( + costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) + { + return transformed_plan; + } + + // Filling the transformed plan to return with the transformed pose + transformed_plan.poses.push_back(costmap_plan_pose); + } return transformed_plan; } @@ -212,8 +209,8 @@ nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { geometry_msgs::msg::PoseStamped global_pose = transformToGlobalPlanFrame(pose); - auto [closest_point, last_point] = findPlanSegmentIterators(global_pose); - auto transformed_plan = transformLocalPlan(global_pose, closest_point, last_point); + auto [closest_point, pruned_plan_end] = findPlanSegmentIterators(global_pose); + auto transformed_plan = transformLocalPlan(global_pose, closest_point, pruned_plan_end); // Remove the portion of the global plan that we've already passed so we don't // process it on the next iteration (this is called path pruning) diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 5b318adbee0..2d522c7b569 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -122,13 +122,41 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( goal_dist_tolerance_ = pose_tolerance.position.x; } + // Transform the pruned global plan to robot base frame + auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = transformed_global_plan.header.frame_id; + stamped_pose.header.stamp = pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + + if (!nav2_util::transformPoseInTargetFrame( + stamped_pose, transformed_pose, *tf_buffer_, + costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance())) + { + throw nav2_core::ControllerTFError( + "Unable to transform plan pose into local frame"); + } + + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = pose.header.stamp; + std::transform( + transformed_global_plan.poses.begin(), + transformed_global_plan.poses.end(), + std::back_inserter(transformed_plan.poses), + transformGlobalPlanToLocal); + // Update the smooth control law with the new params control_law_->setCurvatureConstants( params_->k_phi, params_->k_delta, params_->beta, params_->lambda); control_law_->setSlowdownRadius(params_->slowdown_radius); control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); // Add proper orientations to plan, if needed - validateOrientations(transformed_global_plan.poses); + validateOrientations(transformed_plan.poses); // Transform local frame to global frame to use in collision checking geometry_msgs::msg::TransformStamped costmap_transform; @@ -145,12 +173,12 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( } // Compute distance to goal as the path's integrated distance to account for path curvatures - double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(transformed_global_plan); + double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(transformed_plan); // If we've reached the XY goal tolerance, just rotate if (dist_to_goal < goal_dist_tolerance_ || goal_reached_) { goal_reached_ = true; - double angle_to_goal = tf2::getYaw(transformed_global_plan.poses.back().pose.orientation); + double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); // Check for collisions between our current pose and goal pose size_t num_steps = fabs(angle_to_goal) / params_->in_place_collision_resolution; // Need to check at least the end pose @@ -186,10 +214,10 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( double dist_to_target; std::vector target_distances; - computeDistanceAlongPath(transformed_global_plan.poses, target_distances); + computeDistanceAlongPath(transformed_plan.poses, target_distances); bool is_first_iteration = true; - for (int i = transformed_global_plan.poses.size() - 1; i >= 0; --i) { + for (int i = transformed_plan.poses.size() - 1; i >= 0; --i) { if (is_first_iteration) { // Calculate target pose through lookahead interpolation to get most accurate // lookahead point, if possible @@ -197,7 +225,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // Interpolate after goal false for graceful controller // Requires interpolating the orientation which is not yet implemented // Updates dist_to_target for target_pose returned if using the point on the path - target_pose = nav2_util::getLookAheadPoint(dist_to_target, transformed_global_plan, false); + target_pose = nav2_util::getLookAheadPoint(dist_to_target, transformed_plan, false); is_first_iteration = false; } else { // Underlying control law needs a single target pose, which should: @@ -205,7 +233,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // * But no further than the max_lookahed_ distance // * Be feasible to reach in a collision free manner dist_to_target = target_distances[i]; - target_pose = transformed_global_plan.poses[i]; + target_pose = transformed_plan.poses[i]; } // Compute velocity at this moment if valid target pose is found @@ -219,7 +247,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( target_pose, params_->slowdown_radius); slowdown_pub_->publish(slowdown_marker); // Publish the local plan - local_plan.header = transformed_global_plan.header; + local_plan.header = transformed_plan.header; local_plan_pub_->publish(local_plan); // Successfully found velocity command return cmd_vel; 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 683135b9f6f..305c16696ac 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 @@ -175,12 +175,40 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity goal_dist_tol_ = pose_tolerance.position.x; } + // Transform the plan from costmap's global frame to robot base frame + auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = transformed_global_plan.header.frame_id; + stamped_pose.header.stamp = pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + + if (!nav2_util::transformPoseInTargetFrame( + stamped_pose, transformed_pose, *tf_, + costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance())) + { + throw nav2_core::ControllerTFError( + "Unable to transform plan pose into local frame"); + } + + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = pose.header.stamp; + std::transform( + transformed_global_plan.poses.begin(), + transformed_global_plan.poses.end(), + std::back_inserter(transformed_plan.poses), + transformGlobalPlanToLocal); + // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); double curv_lookahead_dist = params_->curvature_lookahead_dist; // Get the particular point on the path at the lookahead distance - auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_global_plan); + auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_plan); auto rotate_to_path_carrot_pose = carrot_pose; carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -192,7 +220,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity if (params_->use_fixed_curvature_lookahead) { auto curvature_lookahead_pose = nav2_util::getLookAheadPoint( curv_lookahead_dist, - transformed_global_plan, params_->interpolate_curvature_after_goal); + transformed_plan, params_->interpolate_curvature_after_goal); rotate_to_path_carrot_pose = curvature_lookahead_pose; regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position); curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose)); @@ -214,7 +242,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity double angle_to_heading; if (shouldRotateToGoalHeading(carrot_pose)) { is_rotating_to_heading_ = true; - double angle_to_goal = tf2::getYaw(transformed_global_plan.poses.back().pose.orientation); + double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); } else if (shouldRotateToPath(rotate_to_path_carrot_pose, angle_to_heading, x_vel_sign)) { is_rotating_to_heading_ = true; @@ -224,7 +252,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity applyConstraints( regulation_curvature, speed, collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), - transformed_global_plan, linear_vel, x_vel_sign); + transformed_plan, linear_vel, x_vel_sign); if (cancelling_) { const double & dt = control_duration_; From bbd24581b28e11042069fc154e9073134cce9087 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 15 Oct 2025 14:49:58 +0000 Subject: [PATCH 40/62] Linting Signed-off-by: mini-1235 --- .../src/graceful_controller.cpp | 22 +++++++++---------- .../src/regulated_pure_pursuit_controller.cpp | 22 +++++++++---------- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 2d522c7b569..9e816ac4824 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -124,22 +124,22 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // Transform the pruned global plan to robot base frame auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = transformed_global_plan.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = transformed_global_plan.header.frame_id; + stamped_pose.header.stamp = pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; - if (!nav2_util::transformPoseInTargetFrame( + if (!nav2_util::transformPoseInTargetFrame( stamped_pose, transformed_pose, *tf_buffer_, costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance())) - { - throw nav2_core::ControllerTFError( + { + throw nav2_core::ControllerTFError( "Unable to transform plan pose into local frame"); - } + } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; nav_msgs::msg::Path transformed_plan; transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); 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 305c16696ac..f10c72682f4 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 @@ -177,22 +177,22 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Transform the plan from costmap's global frame to robot base frame auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = transformed_global_plan.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = transformed_global_plan.header.frame_id; + stamped_pose.header.stamp = pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; - if (!nav2_util::transformPoseInTargetFrame( + if (!nav2_util::transformPoseInTargetFrame( stamped_pose, transformed_pose, *tf_, costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance())) - { - throw nav2_core::ControllerTFError( + { + throw nav2_core::ControllerTFError( "Unable to transform plan pose into local frame"); - } + } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; nav_msgs::msg::Path transformed_plan; transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); From 0357f37fe1359ac4408088e24e67aa473e26bc15 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 15 Oct 2025 18:35:54 +0000 Subject: [PATCH 41/62] Minor fix Signed-off-by: mini-1235 --- .../include/nav2_controller/plugins/simple_path_handler.hpp | 4 ++-- nav2_graceful_controller/src/graceful_controller.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index 9b26349c3ef..f35b65f34fc 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -31,7 +31,7 @@ using PathSegment = std::pair; /** * @class SimplePathHandler * @brief This plugin manages the global plan by clipping it to the local -* segment relevant to the controller—typically bounded by the local costmap size +* segment, typically bounded by the local costmap size * and transforming the resulting path into the robot base frame. */ @@ -101,7 +101,7 @@ class SimplePathHandler : public nav2_core::PathHandler // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; - rclcpp::Logger logger_ {rclcpp::get_logger("Controller Server")}; + rclcpp::Logger logger_ {rclcpp::get_logger("ControllerServer")}; std::string plugin_name_; std::shared_ptr tf_; std::shared_ptr costmap_ros_; diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 9e816ac4824..611c78dc4b0 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -122,7 +122,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( goal_dist_tolerance_ = pose_tolerance.position.x; } - // Transform the pruned global plan to robot base frame + // Transform the plan from costmap's global frame to robot base frame auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; stamped_pose.header.frame_id = transformed_global_plan.header.frame_id; From 7f43fb7a2f27e330736a2fb2e8d34105508f0841 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 16 Oct 2025 11:44:13 +0000 Subject: [PATCH 42/62] Revert tracking feedback output port Signed-off-by: mini-1235 --- .../plugins/action/follow_path_action.hpp | 10 ------ nav2_behavior_tree/nav2_tree_nodes.xml | 1 - .../plugins/action/follow_path_action.cpp | 17 +--------- .../behavior_trees/follow_point.xml | 2 +- ...replanning_and_if_path_becomes_invalid.xml | 2 +- .../navigate_on_route_graph_w_recovery.xml | 2 +- ...hrough_poses_w_replanning_and_recovery.xml | 2 +- ...gate_to_pose_w_replanning_and_recovery.xml | 2 +- ..._replanning_goal_patience_and_recovery.xml | 2 +- ...eplanning_only_if_path_becomes_invalid.xml | 2 +- .../navigate_w_replanning_distance.xml | 2 +- ...e_w_replanning_only_if_goal_is_updated.xml | 2 +- ...eplanning_only_if_path_becomes_invalid.xml | 2 +- .../navigate_w_replanning_speed.xml | 2 +- .../navigate_w_replanning_time.xml | 2 +- ...global_planning_and_control_w_recovery.xml | 2 +- .../navigators/navigate_through_poses.hpp | 2 -- .../navigators/navigate_to_pose.hpp | 2 -- .../src/navigators/navigate_through_poses.cpp | 31 ++++++++++++++----- .../src/navigators/navigate_to_pose.cpp | 31 ++++++++++++++----- 20 files changed, 62 insertions(+), 58 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index 378801884a2..492dbd8d8a5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -81,11 +81,6 @@ class FollowPathAction : public BtActionNode void on_wait_for_result( std::shared_ptr feedback) override; - /** - * @brief Function to set all feedbacks and output ports to be null values - */ - void resetFeedbackAndOutputPorts(); - /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -102,17 +97,12 @@ class FollowPathAction : public BtActionNode BT::InputPort("goal_checker_id", ""), BT::InputPort("progress_checker_id", ""), BT::InputPort("path_handler_id", ""), - BT::OutputPort("tracking_feedback", - "Tracking feedback from controller server"), BT::OutputPort( "error_code_id", "The follow path error code"), BT::OutputPort( "error_msg", "The follow path error msg"), }); } - -protected: - Action::Feedback feedback_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 2c8a0f6fa76..c65fc774b90 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -183,7 +183,6 @@ Path handler Action server name Server timeout - Tracking feedback from controller server Follow Path error code Follow Path error message diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 4e7181ff95e..4184e68bf4c 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -39,7 +39,6 @@ void FollowPathAction::on_tick() BT::NodeStatus FollowPathAction::on_success() { - resetFeedbackAndOutputPorts(); setOutput("error_code_id", ActionResult::NONE); setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; @@ -47,7 +46,6 @@ BT::NodeStatus FollowPathAction::on_success() BT::NodeStatus FollowPathAction::on_aborted() { - resetFeedbackAndOutputPorts(); setOutput("error_code_id", result_.result->error_code); setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; @@ -55,7 +53,6 @@ BT::NodeStatus FollowPathAction::on_aborted() BT::NodeStatus FollowPathAction::on_cancelled() { - resetFeedbackAndOutputPorts(); // Set empty error code, action was cancelled setOutput("error_code_id", ActionResult::NONE); setOutput("error_msg", ""); @@ -69,7 +66,7 @@ void FollowPathAction::on_timeout() } void FollowPathAction::on_wait_for_result( - std::shared_ptr feedback) + std::shared_ptr/*feedback*/) { // Grab the new path nav_msgs::msg::Path new_path; @@ -113,18 +110,6 @@ void FollowPathAction::on_wait_for_result( goal_.path_handler_id = new_path_handler_id; goal_updated_ = true; } - - if (feedback) { - feedback_ = *feedback; - setOutput("tracking_feedback", feedback_.tracking_feedback); - } -} - -void FollowPathAction::resetFeedbackAndOutputPorts() -{ - nav2_msgs::msg::TrackingFeedback empty_feedback; - feedback_.tracking_feedback = empty_feedback; - setOutput("tracking_feedback", feedback_.tracking_feedback); } } // namespace nav2_behavior_tree diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index ab0ff781fbe..5d5310f79cd 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -16,7 +16,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index 54048f1c171..f2b6589d927 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -27,7 +27,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml index 6560dc4c441..44662cb4656 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml @@ -65,7 +65,7 @@ - + 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 7573a778354..58ce11b91a0 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 @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index a0f1e1741ba..3fb6e4e5ff7 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -20,7 +20,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index 7ddbf6297dd..f79984b750e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -27,7 +27,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index 139da7d9a93..c13b2629dea 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index c500a3ca2e9..fa39ff05283 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index d3735fa9447..f64063bbdcb 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index ad56f441325..1607057c0e2 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -17,7 +17,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index db512b294bd..9c9342e8bbd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 645500fe482..2e43016775d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml index e8cb406c8df..4692c9e17b0 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml @@ -64,7 +64,7 @@ - + diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 524ce0b0e50..427373ded47 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -23,7 +23,6 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" -#include "nav2_msgs/msg/tracking_feedback.hpp" #include "nav2_msgs/msg/waypoint_status.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" @@ -111,7 +110,6 @@ class NavigateThroughPosesNavigator rclcpp::Time start_time_; std::string goals_blackboard_id_; std::string path_blackboard_id_; - std::string tracking_feedback_blackboard_id_; std::string waypoint_statuses_blackboard_id_; // Odometry smoother object diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 0d7d89de827..9b9290095a7 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -23,7 +23,6 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" -#include "nav2_msgs/msg/tracking_feedback.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav_msgs/msg/path.hpp" @@ -128,7 +127,6 @@ class NavigateToPoseNavigator std::string goal_blackboard_id_; std::string path_blackboard_id_; - std::string tracking_feedback_blackboard_id_; // Odometry smoother object std::shared_ptr odom_smoother_; diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index f85b884add1..92b6dfb7320 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -34,9 +34,6 @@ NavigateThroughPosesNavigator::configure( node->declare_or_get_parameter(getName() + ".goals_blackboard_id", std::string("goals")); path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path")); - tracking_feedback_blackboard_id_ = - node->declare_or_get_parameter(getName() + ".tracking_feedback_blackboard_id", - std::string("tracking_feedback")); waypoint_statuses_blackboard_id_ = node->declare_or_get_parameter(getName() + ".waypoint_statuses_blackboard_id", std::string("waypoint_statuses")); @@ -151,10 +148,30 @@ NavigateThroughPosesNavigator::onLoop() return; } - nav2_msgs::msg::TrackingFeedback tracking_feedback; - res = blackboard->get(tracking_feedback_blackboard_id_, tracking_feedback); - if (res) { - double distance_remaining = tracking_feedback.remaining_path_length; + // Get current path points + nav_msgs::msg::Path current_path; + res = blackboard->get(path_blackboard_id_, current_path); + if (res && current_path.poses.size() > 0u) { + // Find the closest pose to current pose on global path + auto find_closest_pose_idx = + [¤t_pose, ¤t_path]() { + size_t closest_pose_idx = 0; + double curr_min_dist = std::numeric_limits::max(); + for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { + double curr_dist = nav2_util::geometry_utils::euclidean_distance( + current_pose, current_path.poses[curr_idx]); + if (curr_dist < curr_min_dist) { + curr_min_dist = curr_dist; + closest_pose_idx = curr_idx; + } + } + return closest_pose_idx; + }; + + // Calculate distance on the path + double distance_remaining = + nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx()); + // Default value for time remaining rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0); diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index df190b88f3f..d4643a09ac8 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -33,9 +33,6 @@ NavigateToPoseNavigator::configure( std::string("goal")); path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path")); - tracking_feedback_blackboard_id_ = - node->declare_or_get_parameter(getName() + ".tracking_feedback_blackboard_id", - std::string("tracking_feedback")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; @@ -132,10 +129,30 @@ NavigateToPoseNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); - nav2_msgs::msg::TrackingFeedback tracking_feedback; - auto res = blackboard->get(tracking_feedback_blackboard_id_, tracking_feedback); - if (res) { - double distance_remaining = tracking_feedback.remaining_path_length; + // Get current path points + nav_msgs::msg::Path current_path; + auto res = blackboard->get(path_blackboard_id_, current_path); + if (res && current_path.poses.size() > 0u) { + // Find the closest pose to current pose on global path + auto find_closest_pose_idx = + [¤t_pose, ¤t_path]() { + size_t closest_pose_idx = 0; + double curr_min_dist = std::numeric_limits::max(); + for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { + double curr_dist = nav2_util::geometry_utils::euclidean_distance( + current_pose, current_path.poses[curr_idx]); + if (curr_dist < curr_min_dist) { + curr_min_dist = curr_dist; + closest_pose_idx = curr_idx; + } + } + return closest_pose_idx; + }; + + // Calculate distance on the path + double distance_remaining = + nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx()); + // Default value for time remaining rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0); From e8590d21d78957227b91d1ace0240e325f3ad202 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 16 Oct 2025 11:48:02 +0000 Subject: [PATCH 43/62] Create post callback first Signed-off-by: mini-1235 --- nav2_controller/src/parameter_handler.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 9a5eeaca51a..e30e63fc352 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -148,14 +148,15 @@ ParameterHandler::ParameterHandler( } } - on_set_params_handler_ = node->add_on_set_parameters_callback( - std::bind( - &ParameterHandler::validateParameterUpdatesCallback, - this, std::placeholders::_1)); post_set_params_handler_ = node->add_post_set_parameters_callback( std::bind( &ParameterHandler::updateParametersCallback, this, std::placeholders::_1)); + + on_set_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &ParameterHandler::validateParameterUpdatesCallback, + this, std::placeholders::_1)); } ParameterHandler::~ParameterHandler() From 7281f03009f0c9c8f3e74238e8974ba90724f1ea Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 17 Oct 2025 16:49:44 +0000 Subject: [PATCH 44/62] Minor fix Signed-off-by: mini-1235 --- ...hrough_poses_w_replanning_and_recovery.xml | 2 +- .../nav2_controller/parameter_handler.hpp | 26 ++++++++++++++----- .../plugins/simple_path_handler.hpp | 2 +- nav2_controller/plugins.xml | 2 +- .../src/parameter_handler.cpp | 2 ++ .../src/regulated_pure_pursuit_controller.cpp | 4 +-- 6 files changed, 26 insertions(+), 12 deletions(-) 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 58ce11b91a0..56beafd2848 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 @@ -9,7 +9,7 @@ - + diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index 4205e9b2ba4..98cc4a93626 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -56,7 +56,7 @@ struct Parameters /** * @class nav2_controller::ParameterHandler - * @brief Handles parameters and dynamic parameters for RPP + * @brief Handles parameters and dynamic parameters for Controller Server */ class ParameterHandler { @@ -79,14 +79,26 @@ class ParameterHandler protected: nav2::LifecycleNode::WeakPtr node_; + /** - * @brief Callback executed when a parameter change is detected - * @param event ParameterEvent message + * @brief Validate incoming parameter updates before applying them. + * This callback is triggered when one or more parameters are about to be updated. + * It checks the validity of parameter values and rejects updates that would lead + * to invalid or inconsistent configurations + * @param parameters List of parameters that are being updated. + * @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted. */ - void - updateParametersCallback(std::vector parameters); - rcl_interfaces::msg::SetParametersResult - validateParameterUpdatesCallback(std::vector parameters); + rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback( + std::vector parameters); + + /** + * @brief Apply parameter updates after validation + * This callback is executed when parameters have been successfully updated. + * It updates the internal configuration of the node with the new parameter values. + * @param parameters List of parameters that have been updated. + */ + void updateParametersCallback(std::vector parameters); + // Dynamic parameters handler std::mutex mutex_; rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_; diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index f35b65f34fc..5b6e140e3d3 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -32,7 +32,7 @@ using PathSegment = std::pair; * @class SimplePathHandler * @brief This plugin manages the global plan by clipping it to the local * segment, typically bounded by the local costmap size -* and transforming the resulting path into the robot base frame. +* and transforming the resulting path into the odom frame. */ class SimplePathHandler : public nav2_core::PathHandler diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index e64a7b6a836..bcffd891e50 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -26,7 +26,7 @@ - Handles the path from planner server and transform it to robot base frame + Manages the global plan by clipping it to the local costmap and transforming it into the odom frame diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 462286f0443..391a818762e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -205,6 +205,7 @@ ParameterHandler::~ParameterHandler() } on_set_params_handler_.reset(); } + rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback( std::vector parameters) { @@ -244,6 +245,7 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda } return result; } + void ParameterHandler::updateParametersCallback( std::vector parameters) 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 f10c72682f4..3bd24c60021 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 @@ -251,8 +251,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity is_rotating_to_heading_ = false; applyConstraints( regulation_curvature, speed, - collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), - transformed_plan, linear_vel, x_vel_sign); + collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, + linear_vel, x_vel_sign); if (cancelling_) { const double & dt = control_duration_; From 0e107ea4558aeb79014a6a6285e9f94c452a694d Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 17 Oct 2025 17:32:06 +0000 Subject: [PATCH 45/62] Add path length tolerance in dwb critic Signed-off-by: mini-1235 --- .../dwb_critics/include/dwb_critics/rotate_to_goal.hpp | 1 + nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp | 8 ++++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp index 00814689aee..37989bc50b5 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp @@ -92,6 +92,7 @@ class RotateToGoalCritic : public dwb_core::TrajectoryCritic double goal_yaw_; double xy_goal_tolerance_; double xy_goal_tolerance_sq_; ///< Cached squared tolerance + double path_length_tolerance_; double current_xy_speed_sq_, stopped_xy_velocity_sq_; double slowing_factor_; double lookahead_time_; diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp index 34f2ae95335..9da959c2f20 100644 --- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp +++ b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp @@ -38,6 +38,7 @@ #include "dwb_core/exceptions.hpp" #include "pluginlib/class_list_macros.hpp" #include "dwb_core/trajectory_utils.hpp" +#include "nav2_util/geometry_utils.hpp" #include "angles/angles.h" PLUGINLIB_EXPORT_CLASS(dwb_critics::RotateToGoalCritic, dwb_core::TrajectoryCritic) @@ -60,6 +61,8 @@ void RotateToGoalCritic::onInit() xy_goal_tolerance_ = node->declare_or_get_parameter( dwb_plugin_name_ + ".xy_goal_tolerance", 0.25); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + path_length_tolerance_ = node->declare_or_get_parameter( + dwb_plugin_name_ + "path_length_tolerance", 1.0); double stopped_xy_velocity = node->declare_or_get_parameter( dwb_plugin_name_ + ".trans_stopped_velocity", 0.25); stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity; @@ -79,10 +82,11 @@ void RotateToGoalCritic::reset() bool RotateToGoalCritic::prepare( const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel, const geometry_msgs::msg::Pose & goal, - const nav_msgs::msg::Path &) + const nav_msgs::msg::Path & transformed_global_plan) { double dxy_sq = hypot_sq(pose.position.x - goal.position.x, pose.position.y - goal.position.y); - in_window_ = in_window_ || dxy_sq <= xy_goal_tolerance_sq_; + double path_length = nav2_util::geometry_utils::calculate_path_length(transformed_global_plan); + in_window_ = in_window_ || dxy_sq <= (xy_goal_tolerance_sq_ && path_length <= path_length_tolerance_); current_xy_speed_sq_ = hypot_sq(vel.x, vel.y); rotating_ = rotating_ || (in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_); goal_yaw_ = tf2::getYaw(goal.orientation); From 689204145f18dcc09aed66207f74cbb61b987b75 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 17 Oct 2025 17:32:45 +0000 Subject: [PATCH 46/62] Linting Signed-off-by: mini-1235 --- nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp index 9da959c2f20..3f45ed25eaa 100644 --- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp +++ b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp @@ -86,7 +86,8 @@ bool RotateToGoalCritic::prepare( { double dxy_sq = hypot_sq(pose.position.x - goal.position.x, pose.position.y - goal.position.y); double path_length = nav2_util::geometry_utils::calculate_path_length(transformed_global_plan); - in_window_ = in_window_ || dxy_sq <= (xy_goal_tolerance_sq_ && path_length <= path_length_tolerance_); + in_window_ = in_window_ || + dxy_sq <= (xy_goal_tolerance_sq_ && path_length <= path_length_tolerance_); current_xy_speed_sq_ = hypot_sq(vel.x, vel.y); rotating_ = rotating_ || (in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_); goal_yaw_ = tf2::getYaw(goal.orientation); From dd76468b0da714660d39cd2e5b4e71a1421fe00d Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 17 Oct 2025 18:28:56 +0000 Subject: [PATCH 47/62] Format Signed-off-by: mini-1235 --- .../include/nav2_controller/controller_server.hpp | 6 +++--- nav2_controller/plugins/pose_progress_checker.cpp | 2 +- nav2_controller/plugins/position_goal_checker.cpp | 2 +- nav2_controller/plugins/simple_goal_checker.cpp | 2 +- nav2_controller/plugins/simple_path_handler.cpp | 12 ++++++------ nav2_controller/plugins/simple_progress_checker.cpp | 2 +- nav2_controller/plugins/stopped_goal_checker.cpp | 4 ++-- nav2_controller/src/parameter_handler.cpp | 10 +++++----- 8 files changed, 20 insertions(+), 20 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 7a4484234c3..f49e850f6a4 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -225,11 +225,11 @@ class ControllerServer : public nav2::LifecycleNode { geometry_msgs::msg::Twist twist_thresh; twist_thresh.linear.x = getThresholdedVelocity(twist.linear.x, - params_->min_x_velocity_threshold); + params_->min_x_velocity_threshold); twist_thresh.linear.y = getThresholdedVelocity(twist.linear.y, - params_->min_y_velocity_threshold); + params_->min_y_velocity_threshold); twist_thresh.angular.z = getThresholdedVelocity(twist.angular.z, - params_->min_theta_velocity_threshold); + params_->min_theta_velocity_threshold); return twist_thresh; } diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index f5a5f828a6d..2ed479f0d74 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -40,7 +40,7 @@ void PoseProgressChecker::initialize( auto node = parent.lock(); required_movement_angle_ = node->declare_or_get_parameter(plugin_name + - ".required_movement_angle", 0.5); + ".required_movement_angle", 0.5); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index a4b824ed157..95086a33908 100644 --- a/nav2_controller/plugins/position_goal_checker.cpp +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -45,7 +45,7 @@ void PositionGoalChecker::initialize( xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25); path_length_tolerance_ = node->declare_or_get_parameter(plugin_name + ".path_length_tolerance", - 1.0); + 1.0); stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 1b6eef43fd0..b0c46509226 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -73,7 +73,7 @@ void SimpleGoalChecker::initialize( xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25); yaw_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".yaw_goal_tolerance", 0.25); path_length_tolerance_ = node->declare_or_get_parameter(plugin_name + ".path_length_tolerance", - 1.0); + 1.0); stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index 327f93b7002..627484c9db8 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -44,17 +44,17 @@ void SimplePathHandler::initialize( tf_ = tf; transform_tolerance_ = costmap_ros_->getTransformTolerance(); interpolate_curvature_after_goal_ = node->declare_or_get_parameter(plugin_name + - ".interpolate_curvature_after_goal", false); + ".interpolate_curvature_after_goal", false); max_robot_pose_search_dist_ = node->declare_or_get_parameter(plugin_name + - ".max_robot_pose_search_dist", getCostmapMaxExtent()); + ".max_robot_pose_search_dist", getCostmapMaxExtent()); prune_distance_ = node->declare_or_get_parameter(plugin_name + ".prune_distance", - 2.0); + 2.0); enforce_path_inversion_ = node->declare_or_get_parameter(plugin_name + ".enforce_path_inversion", - false); + false); inversion_xy_tolerance_ = node->declare_or_get_parameter(plugin_name + ".inversion_xy_tolerance", - 0.2); + 0.2); inversion_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + - ".inversion_yaw_tolerance", 0.4); + ".inversion_yaw_tolerance", 0.4); if (max_robot_pose_search_dist_ < 0.0) { RCLCPP_WARN( logger_, "Max robot search distance is negative, setting to max to search" diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 900c7bf584a..9635bb3712d 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -38,7 +38,7 @@ void SimpleProgressChecker::initialize( radius_ = node->declare_or_get_parameter(plugin_name + ".required_movement_radius", 0.5); time_allowance_ = rclcpp::Duration::from_seconds(node->declare_or_get_parameter(plugin_name + - ".movement_time_allowance", 10.0)); + ".movement_time_allowance", 10.0)); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index ee1f4f9941d..5d66428d967 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -66,9 +66,9 @@ void StoppedGoalChecker::initialize( auto node = parent.lock(); rot_stopped_velocity_ = node->declare_or_get_parameter(plugin_name + ".rot_stopped_velocity", - 0.25); + 0.25); trans_stopped_velocity_ = node->declare_or_get_parameter(plugin_name + ".trans_stopped_velocity", - 0.25); + 0.25); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index e30e63fc352..20fc91b4ba5 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -58,7 +58,7 @@ ParameterHandler::ParameterHandler( RCLCPP_INFO(logger_, "getting progress checker plugins.."); params_.progress_checker_ids = node->declare_or_get_parameter("progress_checker_plugins", - default_progress_checker_ids_); + default_progress_checker_ids_); if (params_.progress_checker_ids == default_progress_checker_ids_) { for (size_t i = 0; i < default_progress_checker_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( @@ -69,7 +69,7 @@ ParameterHandler::ParameterHandler( RCLCPP_INFO(logger_, "getting goal checker plugins.."); params_.goal_checker_ids = node->declare_or_get_parameter("goal_checker_plugins", - default_goal_checker_ids_); + default_goal_checker_ids_); if (params_.goal_checker_ids == default_goal_checker_ids_) { for (size_t i = 0; i < default_goal_checker_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( @@ -80,7 +80,7 @@ ParameterHandler::ParameterHandler( RCLCPP_INFO(logger_, "getting controller plugins.."); params_.controller_ids = node->declare_or_get_parameter("controller_plugins", - default_controller_ids_); + default_controller_ids_); if (params_.controller_ids == default_controller_ids_) { for (size_t i = 0; i < default_controller_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( @@ -91,7 +91,7 @@ ParameterHandler::ParameterHandler( RCLCPP_INFO(logger_, "getting path handler plugins.."); params_.path_handler_ids = node->declare_or_get_parameter("path_handler_plugins", - default_path_handler_ids_); + default_path_handler_ids_); if (params_.path_handler_ids == default_path_handler_ids_) { for (size_t i = 0; i < default_path_handler_ids_.size(); ++i) { nav2::declare_parameter_if_not_declared( @@ -140,7 +140,7 @@ ParameterHandler::ParameterHandler( for (size_t i = 0; i != params_.path_handler_ids.size(); i++) { try { params_.path_handler_types[i] = nav2::get_plugin_type_param(node, - params_.path_handler_ids[i]); + params_.path_handler_ids[i]); } catch (const std::exception & ex) { throw std::runtime_error( std::string("Failed to get type for path handler plugins '") + From 3d1e857e40918d49e35d49a81e7eaa3d2c8fe84f Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 22 Oct 2025 15:26:35 +0000 Subject: [PATCH 48/62] Activate dynamic parameters callback in Avtivate stage Signed-off-by: mini-1235 --- .../include/nav2_controller/parameter_handler.hpp | 10 ++++++++++ nav2_controller/src/controller_server.cpp | 2 ++ nav2_controller/src/parameter_handler.cpp | 10 +++++++++- .../src/parameter_handler.cpp | 4 +++- 4 files changed, 24 insertions(+), 2 deletions(-) diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index 98cc4a93626..c06408d23d6 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -77,6 +77,16 @@ class ParameterHandler Parameters * getParams() {return ¶ms_;} + /** + * @brief Registers callbacks for dynamic parameter handling. + */ + void activate(); + + /** + * @brief Resets callbacks for dynamic parameter handling. + */ + void deactivate(); + protected: nav2::LifecycleNode::WeakPtr node_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 5d6e3d53aff..3da6b51a104 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -236,6 +236,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) transformed_plan_pub_->on_activate(); tracking_feedback_pub_->on_activate(); action_server_->activate(); + param_handler_->activate(); auto node = shared_from_this(); // create bond connection @@ -268,6 +269,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) vel_publisher_->on_deactivate(); transformed_plan_pub_->on_deactivate(); tracking_feedback_pub_->on_deactivate(); + param_handler_->deactivate(); // destroy bond connection destroyBond(); diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 20fc91b4ba5..454e53a3a34 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -147,7 +147,11 @@ ParameterHandler::ParameterHandler( params_.path_handler_types[i] + "': " + ex.what()); } } +} +void ParameterHandler::activate() +{ + auto node = node_.lock(); post_set_params_handler_ = node->add_post_set_parameters_callback( std::bind( &ParameterHandler::updateParametersCallback, @@ -159,7 +163,7 @@ ParameterHandler::ParameterHandler( this, std::placeholders::_1)); } -ParameterHandler::~ParameterHandler() +void ParameterHandler::deactivate() { auto node = node_.lock(); if (post_set_params_handler_ && node) { @@ -171,6 +175,10 @@ ParameterHandler::~ParameterHandler() } on_set_params_handler_.reset(); } + +ParameterHandler::~ParameterHandler() +{ +} rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback( std::vector parameters) { diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 391a818762e..db72f95971e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -255,7 +255,9 @@ ParameterHandler::updateParametersCallback( for (const auto & parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); - + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") { params_.inflation_cost_scaling_factor = parameter.as_double(); From 84d3497c3adbf8f863b8256c21e03fbea8c7b002 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 22 Oct 2025 15:56:12 +0000 Subject: [PATCH 49/62] Fix tests for dynamic parameters Signed-off-by: mini-1235 --- .../test/test_dynamic_parameters.cpp | 47 ++++++++++++++----- 1 file changed, 35 insertions(+), 12 deletions(-) diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp index db7c750aa28..8cedf73ab9e 100644 --- a/nav2_controller/test/test_dynamic_parameters.cpp +++ b/nav2_controller/test/test_dynamic_parameters.cpp @@ -25,29 +25,52 @@ TEST(ControllerServerTest, test_dynamic_parameters) { - auto controller = std::make_shared(); - controller->configure(); - controller->activate(); + std::string nodeName = "test_node"; + auto node = std::make_shared(nodeName); + auto param_handler_ = std::make_unique( + node, node->get_logger()); + param_handler_->activate(); + auto params_ = param_handler_->getParams(); auto rec_param = std::make_shared( - controller->get_node_base_interface(), controller->get_node_topics_interface(), - controller->get_node_graph_interface(), - controller->get_node_services_interface()); + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("min_x_velocity_threshold", 100.0), rclcpp::Parameter("min_y_velocity_threshold", 100.0), rclcpp::Parameter("min_theta_velocity_threshold", 100.0), - rclcpp::Parameter("failure_tolerance", 5.0)}); + rclcpp::Parameter("failure_tolerance", 5.0), + rclcpp::Parameter("search_window", 10.0)}); rclcpp::spin_until_future_complete( - controller->get_node_base_interface(), + node->get_node_base_interface(), results); - EXPECT_EQ(controller->get_parameter("min_x_velocity_threshold").as_double(), 100.0); - EXPECT_EQ(controller->get_parameter("min_y_velocity_threshold").as_double(), 100.0); - EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0); - EXPECT_EQ(controller->get_parameter("failure_tolerance").as_double(), 5.0); + EXPECT_EQ(params_->min_x_velocity_threshold, 100.0); + EXPECT_EQ(params_->min_y_velocity_threshold, 100.0); + EXPECT_EQ(params_->min_theta_velocity_threshold, 100.0); + EXPECT_EQ(params_->failure_tolerance, 5.0); + EXPECT_EQ(params_->search_window, 10.0); + + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("min_x_velocity_threshold", -1.0)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(params_->min_x_velocity_threshold, 100.0); + + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("search_window", -0.1)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(params_->search_window, 10.0); } int main(int argc, char **argv) From 8ad9f8f7335876d699498b3dcc47f3087dc27bd6 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 22 Oct 2025 15:57:34 +0000 Subject: [PATCH 50/62] Lint Signed-off-by: mini-1235 --- nav2_controller/test/test_dynamic_parameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp index 8cedf73ab9e..97ade1a35e3 100644 --- a/nav2_controller/test/test_dynamic_parameters.cpp +++ b/nav2_controller/test/test_dynamic_parameters.cpp @@ -60,7 +60,7 @@ TEST(ControllerServerTest, test_dynamic_parameters) rclcpp::spin_until_future_complete( node->get_node_base_interface(), results); - + EXPECT_EQ(params_->min_x_velocity_threshold, 100.0); results = rec_param->set_parameters_atomically( @@ -69,7 +69,7 @@ TEST(ControllerServerTest, test_dynamic_parameters) rclcpp::spin_until_future_complete( node->get_node_base_interface(), results); - + EXPECT_EQ(params_->search_window, 10.0); } From f1f0539f52f0cecaf7a40ad6f2f17c941f4c1334 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 22 Oct 2025 19:38:02 +0000 Subject: [PATCH 51/62] Skip Signed-off-by: mini-1235 --- .../src/parameter_handler.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index b0552823216..ee68075d56f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -222,6 +222,9 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == plugin_name_ + ".inflation_cost_scaling_factor" && parameter.as_double() <= 0.0) From fb9e294aecf9dce078059d51b6c1d70d64411ca3 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 25 Oct 2025 13:09:42 +0000 Subject: [PATCH 52/62] Fix as suggested Signed-off-by: mini-1235 --- .../plugins/simple_path_handler.cpp | 6 +-- .../src/graceful_controller.cpp | 33 ++++------------ .../src/regulated_pure_pursuit_controller.cpp | 33 ++++------------ .../nav2_rotation_shim_controller.hpp | 7 ---- .../src/nav2_rotation_shim_controller.cpp | 25 +----------- nav2_util/include/nav2_util/path_utils.hpp | 17 ++++++++ nav2_util/src/path_utils.cpp | 39 +++++++++++++++++++ 7 files changed, 77 insertions(+), 83 deletions(-) diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index 627484c9db8..7fd6e0f12a8 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -73,9 +73,9 @@ void SimplePathHandler::initialize( double SimplePathHandler::getCostmapMaxExtent() const { const double max_costmap_dim_meters = std::max( - costmap_ros_->getCostmap()->getSizeInCellsX(), - costmap_ros_->getCostmap()->getSizeInCellsY()); - return max_costmap_dim_meters * costmap_ros_->getCostmap()->getResolution() / 2.0; + costmap_ros_->getCostmap()->getSizeInMetersX(), + costmap_ros_->getCostmap()->getSizeInMetersY()); + return max_costmap_dim_meters / 2.0; } void SimplePathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 0bfb7044133..bfa9f8feef5 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -19,6 +19,7 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/controller_utils.hpp" +#include "nav2_util/path_utils.hpp" #include "nav2_graceful_controller/graceful_controller.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -125,32 +126,14 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( } // Transform the plan from costmap's global frame to robot base frame - auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = transformed_global_plan.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - - if (!nav2_util::transformPoseInTargetFrame( - stamped_pose, transformed_pose, *tf_buffer_, - costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance())) - { - throw nav2_core::ControllerTFError( - "Unable to transform plan pose into local frame"); - } - - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = pose.header.stamp; - std::transform( - transformed_global_plan.poses.begin(), - transformed_global_plan.poses.end(), - std::back_inserter(transformed_plan.poses), - transformGlobalPlanToLocal); + if (!nav2_util::transformPathInTargetFrame( + transformed_global_plan, transformed_plan, *tf_buffer_, + costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance())) + { + throw nav2_core::ControllerTFError( + "Unable to transform plan pose into local frame"); + } // Update the smooth control law with the new params control_law_->setCurvatureConstants( 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 5c5f41d3c8f..68195197d70 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 @@ -26,6 +26,7 @@ #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/controller_utils.hpp" +#include "nav2_util/path_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" using std::hypot; @@ -178,32 +179,14 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Transform the plan from costmap's global frame to robot base frame - auto transformGlobalPlanToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = transformed_global_plan.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - - if (!nav2_util::transformPoseInTargetFrame( - stamped_pose, transformed_pose, *tf_, - costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance())) - { - throw nav2_core::ControllerTFError( - "Unable to transform plan pose into local frame"); - } - - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = pose.header.stamp; - std::transform( - transformed_global_plan.poses.begin(), - transformed_global_plan.poses.end(), - std::back_inserter(transformed_plan.poses), - transformGlobalPlanToLocal); + if (!nav2_util::transformPathInTargetFrame( + transformed_global_plan, transformed_plan, *tf_, + costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance())) + { + throw nav2_core::ControllerTFError( + "Unable to transform plan pose into local frame"); + } // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index 94d62b13332..fca8f907bc4 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -122,13 +122,6 @@ class RotationShimController : public nav2_core::Controller */ geometry_msgs::msg::PoseStamped getSampledPathPt(); - /** - * @brief Find the goal point in path - * May throw exception if the path is empty - * @return pt location of the output point - */ - geometry_msgs::msg::PoseStamped getSampledPathGoal(); - /** * @brief Uses TF to find the location of the sampled path point in base frame * @param pt location of the sampled path point diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index f77ee6165c4..f783a2e2d67 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -177,25 +177,16 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands std::lock_guard lock_reinit(mutex_); try { - geometry_msgs::msg::PoseStamped sampled_pt_goal = getSampledPathGoal(); - - if (!nav2_util::transformPoseInTargetFrame( - sampled_pt_goal, sampled_pt_goal, *tf_, - pose.header.frame_id)) - { - throw nav2_core::ControllerTFError("Failed to transform pose to base frame!"); - } - geometry_msgs::msg::Pose pose_tolerance; geometry_msgs::msg::Twist vel_tolerance; goal_checker->getTolerances(pose_tolerance, vel_tolerance); position_goal_checker_->setXYGoalTolerance(pose_tolerance.position.x); - if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity, + if (position_goal_checker_->isGoalReached(pose.pose, global_goal.pose, velocity, transformed_global_plan)) { double pose_yaw = tf2::getYaw(pose.pose.orientation); - double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation); + double goal_yaw = tf2::getYaw(global_goal.pose.orientation); double angular_distance_to_heading = angles::shortest_angular_distance(pose_yaw, goal_yaw); @@ -292,18 +283,6 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() return goal; } -geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() -{ - if (current_path_.poses.empty()) { - throw nav2_core::InvalidPath("Path is empty - cannot find a goal point"); - } - - auto goal = current_path_.poses.back(); - goal.header.frame_id = current_path_.header.frame_id; - goal.header.stamp = clock_->now(); - return goal; -} - geometry_msgs::msg::Pose RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt) { diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index e6c1f2ee642..d169a9eb822 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -24,7 +24,9 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "nav_msgs/msg/path.hpp" +#include "tf2_ros/buffer.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_util { @@ -56,6 +58,21 @@ PathSearchResult distance_from_path( const size_t start_index = 0, const double search_window_length = std::numeric_limits::max()); +/** + * @brief get an arbitrary path in a target frame + * @param input_path Path to transform + * @param transformed_path Output transformation + * @param tf_buffer TF buffer to use for the transformation + * @param target_frame Frame to transform into + * @param transform_timeout TF Timeout to use for transformation + * @return bool Whether it could be transformed successfully + */ +bool transformPathInTargetFrame( + const nav_msgs::msg::Path & input_path, + nav_msgs::msg::Path & transformed_path, + tf2_ros::Buffer & tf_buffer, const std::string target_frame, + const double transform_timeout = 0.1); + } // namespace nav2_util #endif // NAV2_UTIL__PATH_UTILS_HPP_ diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp index 9d5975b65eb..e44f28a7ecd 100644 --- a/nav2_util/src/path_utils.cpp +++ b/nav2_util/src/path_utils.cpp @@ -82,4 +82,43 @@ PathSearchResult distance_from_path( return result; } +bool transformPathInTargetFrame( + const nav_msgs::msg::Path & input_path, + nav_msgs::msg::Path & transformed_path, + tf2_ros::Buffer & tf_buffer, const std::string target_frame, + const double transform_timeout) +{ + static rclcpp::Logger logger = rclcpp::get_logger("transformPathInTargetFrame"); + + if(input_path.header.frame_id == target_frame) { + transformed_path = input_path; + return true; + } + + transformed_path.header.frame_id = target_frame; + transformed_path.header.stamp = input_path.header.stamp; + + for (const auto & input_pose : input_path.poses) { + geometry_msgs::msg::PoseStamped source_pose, transformed_pose; + source_pose.header.frame_id = input_path.header.frame_id; + source_pose.header.stamp = input_path.header.stamp; + source_pose.pose = input_pose.pose; + + if (!nav2_util::transformPoseInTargetFrame( + source_pose, transformed_pose, tf_buffer, target_frame, transform_timeout)) + { + RCLCPP_ERROR( + logger, + "Failed to transform path from '%s' to '%s'.", + input_path.header.frame_id.c_str(), target_frame.c_str()); + return false; + } + + transformed_pose.pose.position.z = 0.0; + transformed_path.poses.push_back(std::move(transformed_pose)); + } + + return true; +} + } // namespace nav2_util From 70f55cd5ea27e521182f56adfa1fbb5cf0827aed Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 25 Oct 2025 16:35:15 +0000 Subject: [PATCH 53/62] Fix tests Signed-off-by: mini-1235 --- nav2_controller/plugins/test/goal_checker.cpp | 21 +++++ nav2_controller/src/parameter_handler.cpp | 6 +- .../dwb_critics/src/rotate_to_goal.cpp | 2 +- .../test/test_graceful_controller.cpp | 9 ++- .../test/test_shim_controller.cpp | 34 ++------ nav2_util/include/nav2_util/path_utils.hpp | 1 + nav2_util/test/test_path_utils.cpp | 81 +++++++++++++++++++ 7 files changed, 119 insertions(+), 35 deletions(-) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 883125aa5db..8e6fcb60369 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -218,6 +218,7 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0), rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0), + rclcpp::Parameter("test2.path_length_tolerance", 200.0), rclcpp::Parameter("test2.stateful", true)}); rclcpp::spin_until_future_complete( @@ -226,6 +227,7 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0); EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0); + EXPECT_EQ(x->get_parameter("test2.path_length_tolerance").as_double(), 200.0); EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true); // Test the dynamic parameters impacted the tolerances @@ -330,6 +332,25 @@ TEST(StoppedGoalChecker, is_reached) velocity.angular.z = 0.25; EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + sgc.reset(); + gc.reset(); + + // Looping path, xy yaw tolerance reached but path longer than path_length_tolerance + current_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.25); + geometry_msgs::msg::PoseStamped first_pose, second_pose, last_pose; + first_pose.pose.position.x = 9.55; + first_pose.pose.position.y = 7.77; + second_pose.pose.position.x = 13.7; + second_pose.pose.position.y = 7.84; + last_pose.pose.position.x = 9.54; + last_pose.pose.position.y = 7.77; + transformed_global_plan_.poses.push_back(first_pose); + transformed_global_plan_.poses.push_back(second_pose); + transformed_global_plan_.poses.push_back(last_pose); + EXPECT_FALSE(sgc.isGoalReached(first_pose.pose, last_pose.pose, velocity, + transformed_global_plan_)); + EXPECT_FALSE(gc.isGoalReached(first_pose.pose, last_pose.pose, velocity, + transformed_global_plan_)); } int main(int argc, char ** argv) diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index 9f9e99a889f..c7b8a725ab3 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -36,13 +36,13 @@ ParameterHandler::ParameterHandler( params_.controller_frequency = node->declare_or_get_parameter("controller_frequency", 20.0); params_.min_x_velocity_threshold = node->declare_or_get_parameter("min_x_velocity_threshold", - 0.0001); + 0.0001); params_.min_y_velocity_threshold = node->declare_or_get_parameter("min_y_velocity_threshold", - 0.0001); + 0.0001); params_.min_theta_velocity_threshold = node->declare_or_get_parameter("min_theta_velocity_threshold", 0.0001); params_.speed_limit_topic = node->declare_or_get_parameter("speed_limit_topic", - std::string("speed_limit")); + std::string("speed_limit")); params_.failure_tolerance = node->declare_or_get_parameter("failure_tolerance", 0.0); params_.use_realtime_priority = node->declare_or_get_parameter("use_realtime_priority", false); params_.publish_zero_velocity = node->declare_or_get_parameter("publish_zero_velocity", true); diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp index 3f45ed25eaa..91008decb08 100644 --- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp +++ b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp @@ -87,7 +87,7 @@ bool RotateToGoalCritic::prepare( double dxy_sq = hypot_sq(pose.position.x - goal.position.x, pose.position.y - goal.position.y); double path_length = nav2_util::geometry_utils::calculate_path_length(transformed_global_plan); in_window_ = in_window_ || - dxy_sq <= (xy_goal_tolerance_sq_ && path_length <= path_length_tolerance_); + (dxy_sq <= xy_goal_tolerance_sq_ && path_length <= path_length_tolerance_); current_xy_speed_sq_ = hypot_sq(vel.x, vel.y); rotating_ = rotating_ || (in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_); goal_yaw_ = tf2::getYaw(goal.orientation); diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 61b3cb1a1d2..24ffd4002fc 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -628,6 +628,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // Set initial rotation false and allow backward to true nav2::declare_parameter_if_not_declared( @@ -658,7 +659,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { // Create the robot pose geometry_msgs::msg::PoseStamped robot_pose; robot_pose.header.frame_id = "test_robot_frame"; - robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.x = 2.0; robot_pose.pose.position.y = 0.0; robot_pose.pose.position.z = 0.0; robot_pose.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); @@ -678,15 +679,15 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { plan.header.frame_id = "test_global_frame"; plan.poses.resize(3); plan.poses[0].header.frame_id = "test_global_frame"; - plan.poses[0].pose.position.x = 0.0; + plan.poses[0].pose.position.x = 2.0; plan.poses[0].pose.position.y = 0.0; plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); plan.poses[1].header.frame_id = "test_global_frame"; - plan.poses[1].pose.position.x = -1.0; + plan.poses[1].pose.position.x = 1.0; plan.poses[1].pose.position.y = 0.0; plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); plan.poses[2].header.frame_id = "test_global_frame"; - plan.poses[2].pose.position.x = -2.0; + plan.poses[2].pose.position.x = 0.0; plan.poses[2].pose.position.y = 0.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); controller->newPathReceived(plan); diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 142617a35ac..b44d98d0f5f 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -231,8 +231,8 @@ TEST(RotationShimControllerTest, computeVelocityTests) auto tf = std::make_shared(node->get_clock()); auto listener = std::make_shared(*tf, node, true); auto costmap = std::make_shared("fake_costmap"); - costmap->set_parameter(rclcpp::Parameter("origin_x", -25.0)); - costmap->set_parameter(rclcpp::Parameter("origin_y", -25.0)); + costmap->set_parameter(rclcpp::Parameter("origin_x", 0.0)); + costmap->set_parameter(rclcpp::Parameter("origin_y", 0.0)); costmap->configure(); auto tf_broadcaster = std::make_shared(node); @@ -284,31 +284,10 @@ TEST(RotationShimControllerTest, computeVelocityTests) path_handler.setPlan(path); tf_broadcaster->sendTransform(transform); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); - geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::PoseStamped goal = path.poses.back(); auto effort = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); - - path.header.frame_id = "base_link"; - path.poses[1].pose.position.x = 0.1; - path.poses[1].pose.position.y = 0.1; - path.poses[2].pose.position.x = 1.0; - path.poses[2].pose.position.y = 0.0; - path.poses[2].header.frame_id = "base_link"; - path.poses[3].pose.position.x = 10.0; - path.poses[3].pose.position.y = 10.0; - - // this should allow it to find the sampled point, then transform to base_link - // validly because we setup the TF for it. The 1.0 should be selected since default min - // is 0.5 and that should cause a pass off to the RPP controller which will throw - // and exception because it is off of the costmap - controller->newPathReceived(path); - path_handler.setPlan(path); - tf_broadcaster->sendTransform(transform); - transformed_global_plan = path_handler.transformGlobalPlan(pose); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker, - transformed_global_plan, goal), - std::runtime_error); } TEST(RotationShimControllerTest, openLoopRotationTests) { @@ -384,7 +363,7 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { controller->newPathReceived(path); path_handler.setPlan(path); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); - geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::PoseStamped goal = path.poses.back(); auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); @@ -462,7 +441,7 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { controller->newPathReceived(path); path_handler.setPlan(path); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); - geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::PoseStamped goal = path.poses.back(); auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); @@ -473,6 +452,7 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { controller->newPathReceived(path); path_handler.setPlan(path); transformed_global_plan = path_handler.transformGlobalPlan(pose); + goal = path.poses.back(); cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); @@ -551,7 +531,7 @@ TEST(RotationShimControllerTest, accelerationTests) { controller->newPathReceived(path); path_handler.setPlan(path); nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); - geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::PoseStamped goal = path.poses.back(); auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker, transformed_global_plan, goal); EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index d169a9eb822..6bc19942522 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index 30a16bfb30c..2e199e2aaa4 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -21,6 +21,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/path_utils.hpp" +#include "tf2_ros/transform_listener.hpp" geometry_msgs::msg::PoseStamped createPoseStamped(double x, double y) { @@ -369,3 +370,83 @@ TEST(PathUtilsTest, FourArgThrowsOnStartIndexOutOfBounds) nav2_util::distance_from_path(path, robot_pose, 100, 5.0), // 100 >= 3 std::runtime_error); } + +TEST(TransformPathTest, SuccessfulTransform) +{ + rclcpp::init(0, nullptr); + rclcpp::Node::SharedPtr node_ = std::make_shared("test_transform_path"); + std::shared_ptr tf_buffer_ = std::make_shared(node_->get_clock()); + std::shared_ptr tf_listener_ = std::make_shared(*tf_buffer_); + + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = rclcpp::Time(0.0); + tf.header.frame_id = "map"; + tf.child_frame_id = "base_link"; + tf.transform.translation.x = 1.0; + tf.transform.translation.y = 2.0; + tf.transform.translation.z = 0.0; + tf.transform.rotation.w = 1.0; + + tf_buffer_->setTransform(tf, "test_authority"); + nav_msgs::msg::Path input_path, transformed_path; + input_path.header.frame_id = "map"; + input_path.header.stamp = rclcpp::Time(0.0); + + geometry_msgs::msg::PoseStamped pose; + pose.header = input_path.header; + pose.pose.position.x = 3.0; + pose.pose.position.y = 4.0; + input_path.poses.push_back(pose); + + EXPECT_TRUE(nav2_util::transformPathInTargetFrame( + input_path, transformed_path, *tf_buffer_, "base_link", 1.0)); + + EXPECT_EQ(transformed_path.header.frame_id, "base_link"); + EXPECT_EQ(transformed_path.poses.size(), 1u); + EXPECT_NEAR(transformed_path.poses.front().pose.position.x, 2.0, 1e-3); + EXPECT_NEAR(transformed_path.poses.front().pose.position.y, 2.0, 1e-3); + rclcpp::shutdown(); +} + +TEST(TransformPathTest, PathAlreadyInTargetFrame) +{ + rclcpp::init(0, nullptr); + rclcpp::Node::SharedPtr node_ = std::make_shared("test_transform_path"); + std::shared_ptr tf_buffer_ = std::make_shared(node_->get_clock()); + std::shared_ptr tf_listener_ = std::make_shared(*tf_buffer_); + nav_msgs::msg::Path input_path, transformed_path; + input_path.header.frame_id = "map"; + + geometry_msgs::msg::PoseStamped pose; + pose.header = input_path.header; + pose.pose.position.x = 1.0; + pose.pose.position.y = 2.0; + input_path.poses.push_back(pose); + + EXPECT_TRUE(nav2_util::transformPathInTargetFrame( + input_path, transformed_path, *tf_buffer_, "map", 0.1)); + + EXPECT_EQ(transformed_path.header.frame_id, "map"); + EXPECT_EQ(transformed_path.poses.size(), 1u); + rclcpp::shutdown(); +} + +TEST(TransformPathTest, MissingTransform) +{ + rclcpp::init(0, nullptr); + rclcpp::Node::SharedPtr node_ = std::make_shared("test_transform_path"); + std::shared_ptr tf_buffer_ = std::make_shared(node_->get_clock()); + std::shared_ptr tf_listener_ = std::make_shared(*tf_buffer_); + nav_msgs::msg::Path input_path, transformed_path; + input_path.header.frame_id = "odom"; + + geometry_msgs::msg::PoseStamped pose; + pose.header = input_path.header; + pose.pose.position.x = 1.0; + pose.pose.position.y = 1.0; + input_path.poses.push_back(pose); + + EXPECT_FALSE(nav2_util::transformPathInTargetFrame( + input_path, transformed_path, *tf_buffer_, "base_link", 0.1)); + rclcpp::shutdown(); +} \ No newline at end of file From 15f48a42f42c84dc6012b56c4cb3e1103643a201 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 25 Oct 2025 18:06:45 +0000 Subject: [PATCH 54/62] Fix tests Signed-off-by: mini-1235 --- .../src/error_codes/error_code_param.yaml | 2 ++ .../src/error_codes/test_error_codes.py | 1 + nav2_util/test/test_path_utils.cpp | 20 ++++++++++++------- 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 1aa7a7229ab..3815b765267 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -93,6 +93,8 @@ local_costmap: rolling_window: true width: 3 height: 3 + origin_x: -2.0 + origin_y: -2.0 resolution: 0.05 robot_radius: 0.22 plugins: ["voxel_layer", "inflation_layer"] diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index 4b2d6b2919b..a99778b2e30 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -49,6 +49,7 @@ def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] goal_pose1 = goal_pose goal_pose1.pose.position.x += 0.25 + path.header.frame_id = 'map' path.poses.append(initial_pose) path.poses.append(goal_pose) path.poses.append(goal_pose1) diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index 2e199e2aaa4..c0eb13b33ea 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -375,8 +375,10 @@ TEST(TransformPathTest, SuccessfulTransform) { rclcpp::init(0, nullptr); rclcpp::Node::SharedPtr node_ = std::make_shared("test_transform_path"); - std::shared_ptr tf_buffer_ = std::make_shared(node_->get_clock()); - std::shared_ptr tf_listener_ = std::make_shared(*tf_buffer_); + std::shared_ptr tf_buffer_ = + std::make_shared(node_->get_clock()); + std::shared_ptr tf_listener_ = + std::make_shared(*tf_buffer_); geometry_msgs::msg::TransformStamped tf; tf.header.stamp = rclcpp::Time(0.0); @@ -412,8 +414,10 @@ TEST(TransformPathTest, PathAlreadyInTargetFrame) { rclcpp::init(0, nullptr); rclcpp::Node::SharedPtr node_ = std::make_shared("test_transform_path"); - std::shared_ptr tf_buffer_ = std::make_shared(node_->get_clock()); - std::shared_ptr tf_listener_ = std::make_shared(*tf_buffer_); + std::shared_ptr tf_buffer_ = + std::make_shared(node_->get_clock()); + std::shared_ptr tf_listener_ = + std::make_shared(*tf_buffer_); nav_msgs::msg::Path input_path, transformed_path; input_path.header.frame_id = "map"; @@ -435,8 +439,10 @@ TEST(TransformPathTest, MissingTransform) { rclcpp::init(0, nullptr); rclcpp::Node::SharedPtr node_ = std::make_shared("test_transform_path"); - std::shared_ptr tf_buffer_ = std::make_shared(node_->get_clock()); - std::shared_ptr tf_listener_ = std::make_shared(*tf_buffer_); + std::shared_ptr tf_buffer_ = + std::make_shared(node_->get_clock()); + std::shared_ptr tf_listener_ = + std::make_shared(*tf_buffer_); nav_msgs::msg::Path input_path, transformed_path; input_path.header.frame_id = "odom"; @@ -449,4 +455,4 @@ TEST(TransformPathTest, MissingTransform) EXPECT_FALSE(nav2_util::transformPathInTargetFrame( input_path, transformed_path, *tf_buffer_, "base_link", 0.1)); rclcpp::shutdown(); -} \ No newline at end of file +} From 4b7deb21da0ac157cef69100ccfa4e975dd74ae5 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 25 Oct 2025 18:09:12 +0000 Subject: [PATCH 55/62] Update CI key for codecov Signed-off-by: mini-1235 --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 4dd0c904636..aa871e92116 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v40\ + - "<< parameters.key >>-v41\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v40\ + - "<< parameters.key >>-v41\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v40\ + key: "<< parameters.key >>-v41\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ From a8f5ad72ffd95c187c0f712b70541757e838e22d Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 26 Oct 2025 10:30:02 +0000 Subject: [PATCH 56/62] Increase code coverage Signed-off-by: mini-1235 --- .../plugins/simple_path_handler.hpp | 1 + nav2_controller/plugins/test/CMakeLists.txt | 5 +- nav2_controller/plugins/test/goal_checker.cpp | 55 +++- nav2_controller/plugins/test/path_handler.cpp | 265 ++++++++++++++++++ 4 files changed, 324 insertions(+), 2 deletions(-) create mode 100644 nav2_controller/plugins/test/path_handler.cpp diff --git a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp index 5b6e140e3d3..86279e82c63 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -49,6 +49,7 @@ class SimplePathHandler : public nav2_core::PathHandler const geometry_msgs::msg::PoseStamped & pose) override; geometry_msgs::msg::PoseStamped getTransformedGoal( const builtin_interfaces::msg::Time & stamp) override; + nav_msgs::msg::Path getPlan() {return global_plan_;} protected: /** diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index ba0ce9eb2d5..383e6e75971 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -2,4 +2,7 @@ ament_add_gtest(pctest progress_checker.cpp) target_link_libraries(pctest simple_progress_checker pose_progress_checker) ament_add_gtest(gctest goal_checker.cpp) -target_link_libraries(gctest simple_goal_checker stopped_goal_checker) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker position_goal_checker) + +ament_add_gtest(phtest path_handler.cpp) +target_link_libraries(phtest simple_path_handler) \ No newline at end of file diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 8e6fcb60369..257b775e929 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -38,12 +38,14 @@ #include "gtest/gtest.h" #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_controller/plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/position_goal_checker.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav_msgs/msg/path.hpp" using nav2_controller::SimpleGoalChecker; using nav2_controller::StoppedGoalChecker; +using nav2_controller::PositionGoalChecker; void checkMacro( nav2_core::GoalChecker & gc, @@ -157,6 +159,16 @@ TEST(VelocityIterator, stopped_goal_checker_reset) EXPECT_TRUE(true); } +TEST(VelocityIterator, position_goal_checker_reset) +{ + auto x = std::make_shared("position_goal_checker"); + + nav2_core::GoalChecker * pgc = new PositionGoalChecker; + pgc->reset(); + delete pgc; + EXPECT_TRUE(true); +} + TEST(VelocityIterator, two_checks) { auto x = std::make_shared("goal_checker"); @@ -184,10 +196,12 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) SimpleGoalChecker gc; StoppedGoalChecker sgc; + PositionGoalChecker pgc; auto costmap = std::make_shared("test_costmap"); sgc.initialize(x, "test", costmap); gc.initialize(x, "test2", costmap); + pgc.initialize(x, "test3", costmap); geometry_msgs::msg::Pose pose_tol; geometry_msgs::msg::Twist vel_tol; @@ -239,6 +253,24 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); EXPECT_EQ(pose_tol.position.x, 200.0); EXPECT_EQ(pose_tol.position.y, 200.0); + + // Test position goal checker's dynamic parameters + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test3.xy_goal_tolerance", 200.0), + rclcpp::Parameter("test3.path_length_tolerance", 200.0), + rclcpp::Parameter("test3.stateful", true)}); + + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + + EXPECT_EQ(x->get_parameter("test3.xy_goal_tolerance").as_double(), 200.0); + EXPECT_EQ(x->get_parameter("test3.path_length_tolerance").as_double(), 200.0); + EXPECT_EQ(x->get_parameter("test3.stateful").as_bool(), true); + + EXPECT_TRUE(pgc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(pose_tol.position.x, 200.0); + EXPECT_EQ(pose_tol.position.y, 200.0); } TEST(StoppedGoalChecker, is_reached) @@ -247,10 +279,12 @@ TEST(StoppedGoalChecker, is_reached) SimpleGoalChecker gc; StoppedGoalChecker sgc; + PositionGoalChecker pgc; auto costmap = std::make_shared("test_costmap"); sgc.initialize(x, "test", costmap); gc.initialize(x, "test2", costmap); + pgc.initialize(x, "test3", costmap); geometry_msgs::msg::Pose goal_pose; geometry_msgs::msg::Twist velocity; geometry_msgs::msg::Pose current_pose; @@ -261,23 +295,28 @@ TEST(StoppedGoalChecker, is_reached) nav_msgs::msg::Path transformed_global_plan_; EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); + pgc.reset(); // Current linear x speed exceeds tolerance velocity.linear.x = 0.25 + std::numeric_limits::epsilon(); EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); + pgc.reset(); // Current linear x position is further than tolerance away from goal current_pose.position.x = 0.25 + std::numeric_limits::epsilon(); velocity.linear.x = 0.25; EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_FALSE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); - gc.reset(); + pgc.reset(); current_pose.position.x = 0.0; velocity.linear.x = 0.0; @@ -288,16 +327,20 @@ TEST(StoppedGoalChecker, is_reached) velocity.linear.y = 0.25 / std::sqrt(2); EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); + pgc.reset(); // Current linear speed exceeds tolerance velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); + pgc.reset(); // Current linear position is further than tolerance away from goal current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); @@ -306,8 +349,10 @@ TEST(StoppedGoalChecker, is_reached) velocity.linear.y = 0.25 / std::sqrt(2); EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_FALSE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); + pgc.reset(); current_pose.position.x = 0.0; velocity.linear.x = 0.0; @@ -317,23 +362,29 @@ TEST(StoppedGoalChecker, is_reached) velocity.angular.z = 0.25; EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); + pgc.reset(); // Current angular speed exceeds tolerance velocity.angular.z = 0.25 + std::numeric_limits::epsilon(); EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); + pgc.reset(); // Current angular position is further than tolerance away from goal current_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.25 + 1e-15); velocity.angular.z = 0.25; EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); + EXPECT_TRUE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan_)); sgc.reset(); gc.reset(); + pgc.reset(); // Looping path, xy yaw tolerance reached but path longer than path_length_tolerance current_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.25); @@ -351,6 +402,8 @@ TEST(StoppedGoalChecker, is_reached) transformed_global_plan_)); EXPECT_FALSE(gc.isGoalReached(first_pose.pose, last_pose.pose, velocity, transformed_global_plan_)); + EXPECT_FALSE(pgc.isGoalReached(first_pose.pose, last_pose.pose, velocity, + transformed_global_plan_)); } int main(int argc, char ** argv) diff --git a/nav2_controller/plugins/test/path_handler.cpp b/nav2_controller/plugins/test/path_handler.cpp new file mode 100644 index 00000000000..34aab1a8a82 --- /dev/null +++ b/nav2_controller/plugins/test/path_handler.cpp @@ -0,0 +1,265 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_controller/plugins/simple_path_handler.hpp" +#include "tf2_ros/transform_broadcaster.hpp" + +using nav2_controller::PathIterator; +using PathSegment = std::pair; + +class PathHandlerWrapper : public nav2_controller::SimplePathHandler +{ +public: + PathHandlerWrapper() + : SimplePathHandler() {} + + void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end) + { + return prunePlan(path, end); + } + + double getCostmapMaxExtentWrapper() + { + return getCostmapMaxExtent(); + } + + PathSegment + findPlanSegmentIteratorsWrapper(const geometry_msgs::msg::PoseStamped & pose) + { + return findPlanSegmentIterators(pose); + } + + geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper( + const geometry_msgs::msg::PoseStamped & pose) + { + return transformToGlobalPlanFrame(pose); + } + nav_msgs::msg::Path transformLocalPlanWrapper( + const geometry_msgs::msg::PoseStamped & pose, + const PathIterator & closest, + const PathIterator & end) + { + return transformLocalPlan(pose, closest, end); + } + + void setGlobalPlanUpToInversion(const nav_msgs::msg::Path & path) + { + global_plan_up_to_inversion_ = path; + } + + bool isWithinInversionTolerancesWrapper(const geometry_msgs::msg::PoseStamped & robot_pose) + { + return isWithinInversionTolerances(robot_pose); + } + + nav_msgs::msg::Path & getInvertedPath() + { + return global_plan_up_to_inversion_; + } +}; + +TEST(PathHandlerTests, GetAndPrunePath) +{ + nav_msgs::msg::Path path; + PathHandlerWrapper handler; + + path.header.frame_id = "fkframe"; + path.poses.resize(11); + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); + + handler.initialize(node, node->get_logger(), "dummy", costmap_ros, costmap_ros->getTfBuffer()); + handler.setPlan(path); + auto rtn_path = handler.getPlan(); + EXPECT_EQ(path.header.frame_id, rtn_path.header.frame_id); + EXPECT_EQ(path.poses.size(), rtn_path.poses.size()); + + PathIterator it = rtn_path.poses.begin() + 5; + handler.pruneGlobalPlanWrapper(rtn_path, it); + EXPECT_EQ(rtn_path.poses.size(), 6u); +} + +TEST(PathHandlerTests, TestBounds) +{ + PathHandlerWrapper handler; + auto node = std::make_shared("my_node"); + node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + auto results = costmap_ros->set_parameters_atomically( + {rclcpp::Parameter("global_frame", "odom"), + rclcpp::Parameter("robot_base_frame", "base_link")}); + std::string name = "test"; + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); + + // Test initialization and getting costmap basic metadata + handler.initialize(node, node->get_logger(), "dummy", costmap_ros, costmap_ros->getTfBuffer()); + EXPECT_EQ(handler.getCostmapMaxExtentWrapper(), 2.475); + + // Set tf between map odom and base_link + std::unique_ptr tf_broadcaster_ = + std::make_unique(node); + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = "map"; + t.child_frame_id = "base_link"; + tf_broadcaster_->sendTransform(t); + t.header.frame_id = "map"; + t.child_frame_id = "odom"; + tf_broadcaster_->sendTransform(t); + + // Test getting the global plans within a bounds window + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.poses.resize(100); + for (unsigned int i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = i; + path.poses[i].header.frame_id = "map"; + } + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "odom"; + robot_pose.pose.position.x = 25.0; + + handler.setPlan(path); + auto [closest, pruned_plan_end] = handler.findPlanSegmentIteratorsWrapper(robot_pose); + auto transformed_plan = handler.transformLocalPlanWrapper(robot_pose, closest, pruned_plan_end); + auto & path_inverted = handler.getInvertedPath(); + EXPECT_EQ(closest - path_inverted.poses.begin(), 25); + handler.pruneGlobalPlanWrapper(path_inverted, closest); + auto & path_pruned = handler.getInvertedPath(); + EXPECT_EQ(path_pruned.poses.size(), 75u); +} + +TEST(PathHandlerTests, TestTransforms) +{ + PathHandlerWrapper handler; + auto node = std::make_shared("my_node"); + node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); + + // Test basic transformations and path handling + handler.initialize(node, node->get_logger(), "dummy", costmap_ros, costmap_ros->getTfBuffer()); + + // Set tf between map odom and base_link + std::unique_ptr tf_broadcaster_ = + std::make_unique(node); + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = "map"; + t.child_frame_id = "base_link"; + tf_broadcaster_->sendTransform(t); + t.header.frame_id = "map"; + t.child_frame_id = "odom"; + tf_broadcaster_->sendTransform(t); + + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.poses.resize(100); + for (unsigned int i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = i; + path.poses[i].header.frame_id = "map"; + } + + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "odom"; + robot_pose.pose.position.x = 2.5; + + EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error); + handler.setPlan(path); + EXPECT_NO_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose)); + + auto [closest_point, pruned_plan_end] = handler.findPlanSegmentIteratorsWrapper(robot_pose); + auto path_out = handler.transformLocalPlanWrapper(robot_pose, closest_point, pruned_plan_end); + + // Put it all together + auto final_path = handler.transformGlobalPlan(robot_pose); + EXPECT_EQ(final_path.poses.size(), path_out.poses.size()); +} + +TEST(PathHandlerTests, TestInversionToleranceChecks) +{ + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = static_cast(i); + path.poses.push_back(pose); + } + path.poses.back().pose.orientation.w = 1; + + PathHandlerWrapper handler; + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); + + handler.initialize(node, node->get_logger(), "dummy", costmap_ros, costmap_ros->getTfBuffer()); + handler.setGlobalPlanUpToInversion(path); + + // Not near (0,0) + geometry_msgs::msg::PoseStamped robot_pose; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Exactly on top of it + robot_pose.pose.position.x = 9; + robot_pose.pose.orientation.w = 1.0; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Laterally of it + robot_pose.pose.position.y = 9; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.z = 0.8509035; + robot_pose.pose.orientation.w = 0.525322; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled within tolerances + std::cout <<"111" << std::endl; + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.w = 0.9961947; + robot_pose.pose.orientation.z = 0.0871558; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Offset spatially + off angled but both within tolerances + robot_pose.pose.position.x = 9.10; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(0, nullptr); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} From 487f1ed33d6c77a1a99669608ece77016de2c68e Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 26 Oct 2025 10:32:38 +0000 Subject: [PATCH 57/62] Linting Signed-off-by: mini-1235 --- nav2_controller/plugins/test/CMakeLists.txt | 2 +- nav2_controller/plugins/test/path_handler.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index 383e6e75971..42475699bae 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -5,4 +5,4 @@ ament_add_gtest(gctest goal_checker.cpp) target_link_libraries(gctest simple_goal_checker stopped_goal_checker position_goal_checker) ament_add_gtest(phtest path_handler.cpp) -target_link_libraries(phtest simple_path_handler) \ No newline at end of file +target_link_libraries(phtest simple_path_handler) diff --git a/nav2_controller/plugins/test/path_handler.cpp b/nav2_controller/plugins/test/path_handler.cpp index 34aab1a8a82..c318ff011c9 100644 --- a/nav2_controller/plugins/test/path_handler.cpp +++ b/nav2_controller/plugins/test/path_handler.cpp @@ -240,7 +240,6 @@ TEST(PathHandlerTests, TestInversionToleranceChecks) EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); // On top but off angled within tolerances - std::cout <<"111" << std::endl; robot_pose.pose.position.y = 0; robot_pose.pose.orientation.w = 0.9961947; robot_pose.pose.orientation.z = 0.0871558; From ae8b5995850370af31d52c8e61a69c58ad11add0 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 26 Oct 2025 11:57:27 +0000 Subject: [PATCH 58/62] Increase code coverage Signed-off-by: mini-1235 --- nav2_controller/plugins/test/path_handler.cpp | 39 +++++++++++++++++-- nav2_controller/src/controller_server.cpp | 2 +- 2 files changed, 36 insertions(+), 5 deletions(-) diff --git a/nav2_controller/plugins/test/path_handler.cpp b/nav2_controller/plugins/test/path_handler.cpp index c318ff011c9..b81994cc661 100644 --- a/nav2_controller/plugins/test/path_handler.cpp +++ b/nav2_controller/plugins/test/path_handler.cpp @@ -84,7 +84,6 @@ TEST(PathHandlerTests, GetAndPrunePath) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - std::string name = "test"; rclcpp_lifecycle::State state; costmap_ros->on_configure(state); @@ -109,7 +108,6 @@ TEST(PathHandlerTests, TestBounds) auto results = costmap_ros->set_parameters_atomically( {rclcpp::Parameter("global_frame", "odom"), rclcpp::Parameter("robot_base_frame", "base_link")}); - std::string name = "test"; rclcpp_lifecycle::State state; costmap_ros->on_configure(state); @@ -157,7 +155,6 @@ TEST(PathHandlerTests, TestTransforms) node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - std::string name = "test"; rclcpp_lifecycle::State state; costmap_ros->on_configure(state); @@ -213,7 +210,6 @@ TEST(PathHandlerTests, TestInversionToleranceChecks) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - std::string name = "test"; rclcpp_lifecycle::State state; costmap_ros->on_configure(state); @@ -250,6 +246,41 @@ TEST(PathHandlerTests, TestInversionToleranceChecks) EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); } +TEST(PathHandlerTests, TestDynamicParams) +{ + PathHandlerWrapper handler; + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); + + handler.initialize(node, node->get_logger(), "dummy", costmap_ros, costmap_ros->getTfBuffer()); + + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically({ + rclcpp::Parameter("dummy.max_robot_pose_search_dist", 100.0), + rclcpp::Parameter("dummy.inversion_xy_tolerance", 200.0), + rclcpp::Parameter("dummy.inversion_yaw_tolerance", 300.0), + rclcpp::Parameter("dummy.prune_distance", 400.0), + rclcpp::Parameter("dummy.enforce_path_inversion", true), + }); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(node->get_parameter("dummy.max_robot_pose_search_dist").as_double(), 100.0); + EXPECT_EQ(node->get_parameter("dummy.inversion_xy_tolerance").as_double(), 200.0); + EXPECT_EQ(node->get_parameter("dummy.inversion_yaw_tolerance").as_double(), 300.0); + EXPECT_EQ(node->get_parameter("dummy.prune_distance").as_double(), 400.0); + EXPECT_EQ(node->get_parameter("dummy.enforce_path_inversion").as_bool(), true); +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 3da6b51a104..1e8e98b4267 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -74,12 +74,12 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) try { param_handler_ = std::make_unique( node, get_logger()); - params_ = param_handler_->getParams(); } catch (const std::exception & ex) { RCLCPP_FATAL(get_logger(), "%s", ex.what()); on_cleanup(state); return nav2::CallbackReturn::FAILURE; } + params_ = param_handler_->getParams(); for (size_t i = 0; i != params_->progress_checker_ids.size(); i++) { try { From 9431ee49d5e2769b079a7dea7215aaf5dfb7869e Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 26 Oct 2025 15:40:22 +0000 Subject: [PATCH 59/62] Move from controller_utils to path_utils Signed-off-by: mini-1235 --- .../plugins/simple_path_handler.cpp | 2 +- .../include/nav2_util/controller_utils.hpp | 67 --------------- nav2_util/include/nav2_util/path_utils.hpp | 67 +++++++++++++++ nav2_util/test/test_controller_utils.cpp | 63 -------------- nav2_util/test/test_path_utils.cpp | 83 +++++++++++++++++++ 5 files changed, 151 insertions(+), 131 deletions(-) diff --git a/nav2_controller/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp index 7fd6e0f12a8..4df0ea1c262 100644 --- a/nav2_controller/plugins/simple_path_handler.cpp +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -18,7 +18,7 @@ #include "angles/angles.h" #include "pluginlib/class_list_macros.hpp" #include "nav2_controller/plugins/simple_path_handler.hpp" -#include "nav2_util/controller_utils.hpp" +#include "nav2_util/path_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_core/controller_exceptions.hpp" diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp index 8b19f984e0d..be8c88c0013 100644 --- a/nav2_util/include/nav2_util/controller_utils.hpp +++ b/nav2_util/include/nav2_util/controller_utils.hpp @@ -62,73 +62,6 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint( double &, const nav_msgs::msg::Path &, const bool interpolate_after_goal = false); -/** - * @brief Find the iterator of the first pose at which there is an inversion on the path, - * @param path to check for inversion - * @return the first point after the inversion found in the path - */ -inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) -{ - // At least 3 poses for a possible inversion - if (path.poses.size() < 3) { - return path.poses.size(); - } - - // Iterating through the path to determine the position of the path inversion - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - float oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - float oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - float ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - float ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existence of cusp, in the path, using the dot product. - float dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0f) { - return idx + 1; - } - - if ( - (hypot(oa_x, oa_y) == 0.0 && - path.poses[idx - 1].pose.orientation != - path.poses[idx].pose.orientation) - || - (hypot(ab_x, ab_y) == 0.0 && - path.poses[idx].pose.orientation != - path.poses[idx + 1].pose.orientation)) - { - // returning the distance since the points overlap - // but are not simply duplicate points (e.g. in place rotation) - return idx + 1; - } - } - - return path.poses.size(); -} - -/** - * @brief Find and remove poses after the first inversion in the path - * @param path to check for inversion - * @return The location of the inversion, return 0 if none exist - */ -inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) -{ - nav_msgs::msg::Path cropped_path = path; - const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); - if (first_after_inversion == path.poses.size()) { - return 0u; - } - - cropped_path.poses.erase( - cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); - path = cropped_path; - return first_after_inversion; -} - } // namespace nav2_util #endif // NAV2_UTIL__CONTROLLER_UTILS_HPP_ diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index 6bc19942522..cf099af87e2 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -74,6 +74,73 @@ bool transformPathInTargetFrame( tf2_ros::Buffer & tf_buffer, const std::string target_frame, const double transform_timeout = 0.1); +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ +inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existence of cusp, in the path, using the dot product. + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0f) { + return idx + 1; + } + + if ( + (hypot(oa_x, oa_y) == 0.0 && + path.poses[idx - 1].pose.orientation != + path.poses[idx].pose.orientation) + || + (hypot(ab_x, ab_y) == 0.0 && + path.poses[idx].pose.orientation != + path.poses[idx + 1].pose.orientation)) + { + // returning the distance since the points overlap + // but are not simply duplicate points (e.g. in place rotation) + return idx + 1; + } + } + + return path.poses.size(); +} + +/** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; +} + } // namespace nav2_util #endif // NAV2_UTIL__PATH_UTILS_HPP_ diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp index 57a206fce35..e6117db8e11 100644 --- a/nav2_util/test/test_controller_utils.cpp +++ b/nav2_util/test/test_controller_utils.cpp @@ -387,66 +387,3 @@ TEST(InterpolationUtils, lookaheadExtrapolation) EXPECT_NEAR(pt.pose.position.x, 2.0 + cos(-45.0 * M_PI / 180) * 10.0, EPSILON); EXPECT_NEAR(pt.pose.position.y, -2.0 + sin(-45.0 * M_PI / 180) * 10.0, EPSILON); } - -TEST(UtilsTests, FindPathInversionTest) -{ - // Straight path, no inversions to be found - nav_msgs::msg::Path path; - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - EXPECT_EQ(nav2_util::findFirstPathInversion(path), 10u); - - // To short to process - path.poses.erase(path.poses.begin(), path.poses.begin() + 7); - EXPECT_EQ(nav2_util::findFirstPathInversion(path), 3u); - - // Has inversion at index 10, so should return 11 for the first point afterwards - // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 - path.poses.clear(); - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = 10 - i; - path.poses.push_back(pose); - } - EXPECT_EQ(nav2_util::findFirstPathInversion(path), 11u); -} - -TEST(UtilsTests, RemovePosesAfterPathInversionTest) -{ - nav_msgs::msg::Path path; - // straight path - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u); - - // try empty path - path.poses.clear(); - EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u); - - // cusping path - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = 10 - i; - path.poses.push_back(pose); - } - EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 11u); - // Check to see if removed - EXPECT_EQ(path.poses.size(), 11u); - EXPECT_EQ(path.poses.back().pose.position.x, 10); -} diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index c0eb13b33ea..a17d8648891 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -456,3 +456,86 @@ TEST(TransformPathTest, MissingTransform) input_path, transformed_path, *tf_buffer_, "base_link", 0.1)); rclcpp::shutdown(); } + +TEST(UtilsTests, FindPathInversionTest) +{ + // Straight path, no inversions to be found + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 10u); + + // To short to process + path.poses.erase(path.poses.begin(), path.poses.begin() + 7); + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 3u); + + // Has inversion at index 10, so should return 11 for the first point afterwards + // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 + path.poses.clear(); + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 11u); + + // In place rotation + path.poses.clear(); + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + geometry_msgs::msg::PoseStamped last_pose; + last_pose = path.poses.back(); + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); // rotate 90 degrees + last_pose.pose.orientation.x = q.x(); + last_pose.pose.orientation.y = q.y(); + last_pose.pose.orientation.z = q.z(); + last_pose.pose.orientation.w = q.w(); + path.poses.push_back(last_pose); + last_pose.pose.position.x = 11.0; + path.poses.push_back(last_pose); + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 10u); +} + +TEST(UtilsTests, RemovePosesAfterPathInversionTest) +{ + nav_msgs::msg::Path path; + // straight path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u); + + // try empty path + path.poses.clear(); + EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u); + + // cusping path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 11u); + // Check to see if removed + EXPECT_EQ(path.poses.size(), 11u); + EXPECT_EQ(path.poses.back().pose.position.x, 10); +} From c669b68b847530e0ebe31954d7e00f0c9f00312d Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sun, 26 Oct 2025 17:01:43 +0000 Subject: [PATCH 60/62] Update params and rviz Signed-off-by: mini-1235 --- nav2_bringup/params/nav2_params.yaml | 11 +++++++++-- nav2_bringup/rviz/nav2_default_view.rviz | 14 +++++++------- .../src/costmap_filters/keepout_params.yaml | 6 ++++++ .../src/costmap_filters/speed_global_params.yaml | 6 ++++++ .../src/costmap_filters/speed_local_params.yaml | 6 ++++++ .../src/error_codes/error_code_param.yaml | 6 ++++++ .../src/gps_navigation/nav2_no_map_params.yaml | 6 ++++++ .../src/system/nav2_system_params.yaml | 6 ++++++ 8 files changed, 52 insertions(+), 9 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index e6521258ffc..f463a329521 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -96,6 +96,7 @@ controller_server: progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + path_handler_plugins: ["path_handler"] use_realtime_priority: false speed_limit_topic: "speed_limit" @@ -115,6 +116,14 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_length_tolerance: 1.0 + path_handler: + plugin: "nav2_controller::SimplePathHandler" + prune_distance: 2.0 + enforce_path_inversion: False + inversion_xy_tolerance: 0.2 + inversion_yaw_tolerance: 0.4 + interpolate_curvature_after_goal: False FollowPath: plugin: "nav2_mppi_controller::MPPIController" time_steps: 56 @@ -133,8 +142,6 @@ controller_server: vy_max: 0.5 wz_max: 1.9 iteration_count: 1 - prune_distance: 1.7 - transform_tolerance: 0.1 temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index eb8852ffff7..6ee36dca002 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -403,8 +403,8 @@ Visualization Manager: Class: rviz_default_plugins/Path Color: 0; 12; 255 Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 @@ -413,18 +413,18 @@ Visualization Manager: X: 0 Y: 0 Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None + Pose Color: 0; 0; 255 + Pose Style: Arrows Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: local_plan + Value: transformed_global_plan Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 06d6e123ed5..f7ae9284042 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -73,9 +73,15 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_length_tolerance: 1.0 stateful: True path_handler: plugin: "nav2_controller::SimplePathHandler" + prune_distance: 2.0 + enforce_path_inversion: False + inversion_xy_tolerance: 0.2 + inversion_yaw_tolerance: 0.4 + interpolate_curvature_after_goal: False # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" 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 ff7595e9256..7f4635cef40 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -74,9 +74,15 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_length_tolerance: 1.0 stateful: True path_handler: plugin: "nav2_controller::SimplePathHandler" + prune_distance: 2.0 + enforce_path_inversion: False + inversion_xy_tolerance: 0.2 + inversion_yaw_tolerance: 0.4 + interpolate_curvature_after_goal: False # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" 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 1b8f39b871f..16449c2c2c5 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -74,9 +74,15 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_length_tolerance: 1.0 stateful: True path_handler: plugin: "nav2_controller::SimplePathHandler" + prune_distance: 2.0 + enforce_path_inversion: False + inversion_xy_tolerance: 0.2 + inversion_yaw_tolerance: 0.4 + interpolate_curvature_after_goal: False # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 3815b765267..e83b8f0db7d 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -39,8 +39,14 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_length_tolerance: 1.0 path_handler: plugin: "nav2_controller::SimplePathHandler" + prune_distance: 2.0 + enforce_path_inversion: False + inversion_xy_tolerance: 0.2 + inversion_yaw_tolerance: 0.4 + interpolate_curvature_after_goal: False # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" 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 760dfb7d029..61007ed6d3f 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 @@ -53,8 +53,14 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_length_tolerance: 1.0 path_handler: plugin: "nav2_controller::SimplePathHandler" + prune_distance: 2.0 + enforce_path_inversion: False + inversion_xy_tolerance: 0.2 + inversion_yaw_tolerance: 0.4 + interpolate_curvature_after_goal: False # MPPI parameters FollowPath: plugin: "nav2_mppi_controller::MPPIController" diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index fa9f655a6a7..d0f74f1ee9d 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -97,8 +97,14 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_length_tolerance: 1.0 path_handler: plugin: "nav2_controller::SimplePathHandler" + prune_distance: 2.0 + enforce_path_inversion: False + inversion_xy_tolerance: 0.2 + inversion_yaw_tolerance: 0.4 + interpolate_curvature_after_goal: False # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" From 25cd69918a0d108e96570a079f0f5e8df76ac4cb Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Tue, 28 Oct 2025 14:30:04 +0000 Subject: [PATCH 61/62] Update costmap update timeout Signed-off-by: mini-1235 --- .../include/nav2_controller/controller_server.hpp | 1 - .../include/nav2_controller/parameter_handler.hpp | 2 +- nav2_controller/src/controller_server.cpp | 7 ++----- nav2_controller/src/parameter_handler.cpp | 3 ++- 4 files changed, 5 insertions(+), 8 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index f49e850f6a4..6bcc0183466 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -264,7 +264,6 @@ class ControllerServer : public nav2::LifecycleNode std::string path_handler_ids_concat_, current_path_handler_; size_t start_index_; - rclcpp::Duration costmap_update_timeout_; geometry_msgs::msg::PoseStamped end_pose_; geometry_msgs::msg::PoseStamped transformed_end_pose_; diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index c06408d23d6..ca405032cd2 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -40,7 +40,7 @@ struct Parameters double failure_tolerance; bool use_realtime_priority; bool publish_zero_velocity; - double costmap_update_timeout; + rclcpp::Duration costmap_update_timeout{0,0}; std::string odom_topic; double odom_duration; double search_window; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 1e8e98b4267..0ac4549c6a8 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -40,8 +40,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), lp_loader_("nav2_core", "nav2_core::Controller"), path_handler_loader_("nav2_core", "nav2_core::PathHandler"), - start_index_(0), - costmap_update_timeout_(300ms) + start_index_(0) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -194,8 +193,6 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) transformed_plan_pub_ = create_publisher("transformed_global_plan"); tracking_feedback_pub_ = create_publisher("tracking_feedback"); - costmap_update_timeout_ = rclcpp::Duration::from_seconds(params_->costmap_update_timeout); - // Create the action server that we implement with our followPath method // This may throw due to real-time prioritization if user doesn't have real-time permissions try { @@ -492,7 +489,7 @@ void ControllerServer::computeControl() rclcpp::Rate r(100); auto waiting_start = now(); while (!costmap_ros_->isCurrent()) { - if (now() - waiting_start > costmap_update_timeout_) { + if (now() - waiting_start > params_->costmap_update_timeout) { throw nav2_core::ControllerTimedOut("Costmap timed out waiting for update"); } r.sleep(); diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index c7b8a725ab3..b9be9337161 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -46,7 +46,8 @@ ParameterHandler::ParameterHandler( params_.failure_tolerance = node->declare_or_get_parameter("failure_tolerance", 0.0); params_.use_realtime_priority = node->declare_or_get_parameter("use_realtime_priority", false); params_.publish_zero_velocity = node->declare_or_get_parameter("publish_zero_velocity", true); - params_.costmap_update_timeout = node->declare_or_get_parameter("costmap_update_timeout", 0.30); + double costmap_update_timeout_dbl = node->declare_or_get_parameter("costmap_update_timeout", 0.30); + params_.costmap_update_timeout = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); params_.odom_topic = node->declare_or_get_parameter("odom_topic", std::string("odom")); params_.odom_duration = node->declare_or_get_parameter("odom_duration", 0.3); params_.search_window = node->declare_or_get_parameter("search_window", 2.0); From 4529858c047dc584f2f6a2b40411a35e4f12d616 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Tue, 28 Oct 2025 16:53:17 +0000 Subject: [PATCH 62/62] Lint Signed-off-by: mini-1235 --- nav2_controller/include/nav2_controller/parameter_handler.hpp | 2 +- nav2_controller/src/parameter_handler.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index ca405032cd2..27635f7ec84 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -40,7 +40,7 @@ struct Parameters double failure_tolerance; bool use_realtime_priority; bool publish_zero_velocity; - rclcpp::Duration costmap_update_timeout{0,0}; + rclcpp::Duration costmap_update_timeout{0, 0}; std::string odom_topic; double odom_duration; double search_window; diff --git a/nav2_controller/src/parameter_handler.cpp b/nav2_controller/src/parameter_handler.cpp index b9be9337161..0490295cf3e 100644 --- a/nav2_controller/src/parameter_handler.cpp +++ b/nav2_controller/src/parameter_handler.cpp @@ -46,7 +46,8 @@ ParameterHandler::ParameterHandler( params_.failure_tolerance = node->declare_or_get_parameter("failure_tolerance", 0.0); params_.use_realtime_priority = node->declare_or_get_parameter("use_realtime_priority", false); params_.publish_zero_velocity = node->declare_or_get_parameter("publish_zero_velocity", true); - double costmap_update_timeout_dbl = node->declare_or_get_parameter("costmap_update_timeout", 0.30); + double costmap_update_timeout_dbl = node->declare_or_get_parameter("costmap_update_timeout", + 0.30); params_.costmap_update_timeout = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); params_.odom_topic = node->declare_or_get_parameter("odom_topic", std::string("odom")); params_.odom_duration = node->declare_or_get_parameter("odom_duration", 0.3);