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 379c7ad225d..88818282ffe 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -181,6 +181,7 @@ Path to follow Goal checker Progress checker + Path handler Action server name Server timeout Follow Path error code @@ -258,6 +259,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_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 270b26540c9..e2c24fc7d2e 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_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index b9c75a413de..7ce7213454a 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_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..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,6 +9,7 @@ + @@ -24,7 +25,7 @@ - + diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index c8a655f4340..598c9959171 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/controller_server.cpp ) target_include_directories(${library_name} @@ -45,6 +46,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} @@ -167,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 @@ -183,7 +205,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 + simple_path_handler + ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -204,6 +233,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 a4e33a4249b..6bcc0183466 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" @@ -37,6 +38,7 @@ #include "nav2_util/twist_publisher.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" +#include "nav2_controller/parameter_handler.hpp" namespace nav2_controller { @@ -53,6 +55,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 @@ -154,6 +157,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 @@ -212,23 +224,15 @@ 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_; - // The controller needs a costmap node std::shared_ptr costmap_ros_; std::unique_ptr costmap_thread_; @@ -242,45 +246,25 @@ 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_; - double controller_frequency_; - double min_x_velocity_threshold_; - double min_y_velocity_threshold_; - double min_theta_velocity_threshold_; - double search_window_; - size_t start_index_; - - double failure_tolerance_; - bool use_realtime_priority_; - bool publish_zero_velocity_; - rclcpp::Duration costmap_update_timeout_; + // Path Handler Plugins + pluginlib::ClassLoader path_handler_loader_; + PathHandlerMap path_handlers_; + std::string path_handler_ids_concat_, current_path_handler_; - // Whether we've published the single controller warning yet + size_t start_index_; geometry_msgs::msg::PoseStamped end_pose_; - geometry_msgs::msg::PoseStamped transformed_end_pose_; // Last time the controller generated a valid command @@ -288,6 +272,11 @@ class ControllerServer : public nav2::LifecycleNode // Current path container nav_msgs::msg::Path current_path_; + nav_msgs::msg::Path transformed_global_plan_; + 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 new file mode 100644 index 00000000000..27635f7ec84 --- /dev/null +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -0,0 +1,133 @@ +// 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 controller_frequency; + 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; + rclcpp::Duration costmap_update_timeout{0, 0}; + std::string odom_topic; + double odom_duration; + double search_window; + 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; +}; + +/** + * @class nav2_controller::ParameterHandler + * @brief Handles parameters and dynamic parameters for Controller Server + */ +class ParameterHandler +{ +public: + /** + * @brief Constructor for nav2_controller::ParameterHandler + */ + ParameterHandler( + nav2::LifecycleNode::SharedPtr node, + const rclcpp::Logger & logger); + + /** + * @brief Destrructor for nav2_controller::ParameterHandler + */ + ~ParameterHandler(); + + std::mutex & getMutex() {return mutex_;} + + 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_; + + /** + * @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. + */ + 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_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_; + Parameters params_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("ControllerServer")}; + + 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::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 + +#endif // NAV2_CONTROLLER__PARAMETER_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 04eb7623ecb..7f0e419c5a8 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: @@ -45,30 +45,32 @@ 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 & transformed_global_plan) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, 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: double xy_goal_tolerance_; double xy_goal_tolerance_sq_; + double path_length_tolerance_; bool stateful_; bool position_reached_; std::string plugin_name_; 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 76feabf256f..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,13 +66,14 @@ 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) 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; 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/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..a1073d6d672 --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/simple_path_handler.hpp @@ -0,0 +1,125 @@ +// 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_CONTROLLER__PLUGINS__SIMPLE_PATH_HANDLER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PATH_HANDLER_HPP_ + +#include +#include +#include +#include +#include "nav2_core/path_handler.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +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 +* segment, typically bounded by the local costmap size +* and transforming the resulting path into the odom frame. +*/ + +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; + 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; + nav_msgs::msg::Path getPlan() {return global_plan_;} + +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 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. + */ + PathSegment findPlanSegmentIterators(const geometry_msgs::msg::PoseStamped & global_pose); + + /** + * @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 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 & pruned_plan_end); + + /** + * 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_; + rclcpp::Logger logger_ {rclcpp::get_logger("ControllerServer")}; + 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_constraint_; + unsigned int constraint_locale_{0u}; + bool interpolate_curvature_after_goal_, enforce_path_constraint_; + double max_robot_pose_search_dist_, transform_tolerance_, prune_distance_; + float inversion_xy_tolerance_, inversion_yaw_tolerance_, minimum_rotation_angle_; + + /** + * @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 e0094cf6f4d..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) 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..bcffd891e50 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 + + + Manages the global plan by clipping it to the local costmap and transforming it into the odom frame + + diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index 5d9dcf93dc1..95086a33908 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 geometry_msgs::msg::Twist &, const nav_msgs::msg::Path & transformed_global_plan) { + // 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_) + { + 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 ebdc8665c08..b0c46509226 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) @@ -71,6 +72,8 @@ 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); stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; @@ -87,8 +90,14 @@ 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 & transformed_global_plan) { + // 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_) + { + return false; + } if (check_xy_) { double dx = query_pose.position.x - goal_pose.position.x, dy = query_pose.position.y - goal_pose.position.y; @@ -146,6 +155,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/plugins/simple_path_handler.cpp b/nav2_controller/plugins/simple_path_handler.cpp new file mode 100644 index 00000000000..6fefe7649b4 --- /dev/null +++ b/nav2_controller/plugins/simple_path_handler.cpp @@ -0,0 +1,290 @@ +// 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 +#include "angles/angles.h" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_controller/plugins/simple_path_handler.hpp" +#include "nav2_util/path_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 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; + 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 + + ".max_robot_pose_search_dist", getCostmapMaxExtent()); + prune_distance_ = node->declare_or_get_parameter(plugin_name + ".prune_distance", + 2.0); + enforce_path_constraint_ = node->declare_or_get_parameter(plugin_name + + ".enforce_path_constraint", 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); + minimum_rotation_angle_ = node->declare_or_get_parameter(plugin_name + ".minimum_rotation_angle", + 0.785); + 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_constraint_) { + constraint_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_constraint_.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_constraint_ = global_plan_; + if(enforce_path_constraint_) { + constraint_locale_ = nav2_util::removePosesAfterFirstConstraint(global_plan_up_to_constraint_); + } +} + +geometry_msgs::msg::PoseStamped SimplePathHandler::transformToGlobalPlanFrame( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_up_to_constraint_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + if (interpolate_curvature_after_goal_ && global_plan_up_to_constraint_.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_constraint_.header.frame_id, + transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); + } + + return robot_pose; +} + +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 + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_up_to_constraint_.poses.begin(), global_plan_up_to_constraint_.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_constraint_.poses.begin(), closest_pose_upper_bound, + [&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 + // 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_constraint_.poses.begin() != closest_pose_upper_bound && + global_plan_up_to_constraint_.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 = + nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_constraint_.poses.end(), prune_distance_); + + 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 & pruned_plan_end) +{ + nav_msgs::msg::Path transformed_plan; + 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; +} + +nav_msgs::msg::Path SimplePathHandler::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) +{ + geometry_msgs::msg::PoseStamped global_pose = transformToGlobalPlanFrame(pose); + 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) + prunePlan(global_plan_up_to_constraint_, closest_point); + + if (enforce_path_constraint_ && constraint_locale_ != 0u) { + if (isWithinInversionTolerances(global_pose)) { + prunePlan(global_plan_, global_plan_.poses.begin() + constraint_locale_); + global_plan_up_to_constraint_ = global_plan_; + constraint_locale_ = nav2_util::removePosesAfterFirstConstraint(global_plan_up_to_constraint_, + minimum_rotation_angle_); + } + } + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + 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) +{ + 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_name == "prune_distance") { + prune_distance_ = parameter.as_double(); + } else if (param_name == "minimum_rotation_angle") { + minimum_rotation_angle_ = parameter.as_double(); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == "enforce_path_constraint") { + enforce_path_constraint_ = 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/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index e7e4c4629a4..aa1f273f50f 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -77,9 +77,10 @@ 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 & transformed_global_plan) { - bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity); + bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity, + transformed_global_plan); if (!ret) { return ret; } diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index 8619a6f0703..19bfc47d0d7 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -2,4 +2,7 @@ nav2_add_gtest(pctest progress_checker.cpp) target_link_libraries(pctest simple_progress_checker pose_progress_checker) nav2_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) + +nav2_add_gtest(phtest path_handler.cpp) +target_link_libraries(phtest simple_path_handler) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 1aba1ba80bb..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 "eigen3/Eigen/Geometry" +#include "nav_msgs/msg/path.hpp" using nav2_controller::SimpleGoalChecker; using nav2_controller::StoppedGoalChecker; +using nav2_controller::PositionGoalChecker; void checkMacro( nav2_core::GoalChecker & gc, @@ -70,10 +72,11 @@ void checkMacro( v.linear.y = yv; v.angular.z = thetav; + nav_msgs::msg::Path transformed_global_plan; if (expected_result) { - EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v)); + EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v, transformed_global_plan)); } else { - EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v)); + EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v, transformed_global_plan)); } } @@ -156,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"); @@ -183,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; @@ -217,6 +232,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( @@ -225,6 +241,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 @@ -236,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) @@ -244,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; @@ -255,25 +292,31 @@ TEST(StoppedGoalChecker, is_reached) // 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)); + 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)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + 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)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + 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; @@ -282,62 +325,85 @@ 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)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + 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)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + 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(); 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)); + 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; - // 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]; + current_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.25); velocity.angular.z = 0.25; - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + 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)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + 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.z = quat_epsilon[2]; - current_pose.orientation.w = quat_epsilon[3]; + 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)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + 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); + 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_)); + EXPECT_FALSE(pgc.isGoalReached(first_pose.pose, last_pose.pose, velocity, + transformed_global_plan_)); } int main(int argc, char ** argv) diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_controller/plugins/test/path_handler.cpp similarity index 63% rename from nav2_mppi_controller/test/path_handler_test.cpp rename to nav2_controller/plugins/test/path_handler.cpp index 8d25724fb53..958b868fe41 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_controller/plugins/test/path_handler.cpp @@ -17,33 +17,32 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "nav2_mppi_controller/tools/path_handler.hpp" +#include "nav2_controller/plugins/simple_path_handler.hpp" #include "tf2_ros/transform_broadcaster.hpp" -// Tests path handling +using nav2_controller::PathIterator; +using PathSegment = std::pair; -using namespace mppi; // NOLINT - -class PathHandlerWrapper : public PathHandler +class PathHandlerWrapper : public nav2_controller::SimplePathHandler { public: PathHandlerWrapper() - : PathHandler() {} + : SimplePathHandler() {} void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end) { return prunePlan(path, end); } - double getMaxCostmapDistWrapper() + double getCostmapMaxExtentWrapper() { - return getMaxCostmapDist(); + return getCostmapMaxExtent(); } - std::pair - getGlobalPlanConsideringBoundsInCostmapFrameWrapper(const geometry_msgs::msg::PoseStamped & pose) + PathSegment + findPlanSegmentIteratorsWrapper(const geometry_msgs::msg::PoseStamped & pose) { - return getGlobalPlanConsideringBoundsInCostmapFrame(pose); + return findPlanSegmentIterators(pose); } geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper( @@ -51,10 +50,17 @@ class PathHandlerWrapper : public PathHandler { 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; + global_plan_up_to_constraint_ = path; } bool isWithinInversionTolerancesWrapper(const geometry_msgs::msg::PoseStamped & robot_pose) @@ -64,7 +70,7 @@ class PathHandlerWrapper : public PathHandler nav_msgs::msg::Path & getInvertedPath() { - return global_plan_up_to_inversion_; + return global_plan_up_to_constraint_; } }; @@ -75,16 +81,21 @@ TEST(PathHandlerTests, GetAndPrunePath) 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); + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); - handler.setPath(path); - auto & rtn_path = handler.getPath(); + 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); - auto rtn2_path = handler.getPath(); - EXPECT_EQ(rtn2_path.poses.size(), 6u); + EXPECT_EQ(rtn_path.poses.size(), 6u); } TEST(PathHandlerTests, TestBounds) @@ -97,14 +108,12 @@ 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"; - 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); + 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_ = @@ -129,9 +138,9 @@ TEST(PathHandlerTests, TestBounds) robot_pose.header.frame_id = "odom"; robot_pose.pose.position.x = 25.0; - handler.setPath(path); - auto [transformed_plan, closest] = - handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose); + 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); @@ -146,13 +155,11 @@ 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"; - 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); + 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_ = @@ -177,16 +184,15 @@ TEST(PathHandlerTests, TestTransforms) 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); + handler.setPlan(path); EXPECT_NO_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose)); - auto [path_out, closest] = - handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(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.transformPath(robot_pose); + auto final_path = handler.transformGlobalPlan(robot_pose); EXPECT_EQ(final_path.poses.size(), path_out.poses.size()); } @@ -201,6 +207,13 @@ TEST(PathHandlerTests, TestInversionToleranceChecks) 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); + 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) @@ -233,7 +246,42 @@ TEST(PathHandlerTests, TestInversionToleranceChecks) EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); } -int main(int argc, char ** argv) +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 d0c6b47f22e..4602e828662 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -29,6 +29,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 { @@ -36,16 +37,10 @@ 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::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), - default_ids_{"FollowPath"}, - default_types_{"dwb_core::DWBLocalPlanner"}, - start_index_(0), - costmap_update_timeout_(300ms) + path_handler_loader_("nav2_core", "nav2_core::PathHandler"), + start_index_(0) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -60,6 +55,7 @@ ControllerServer::~ControllerServer() progress_checkers_.clear(); goal_checkers_.clear(); controllers_.clear(); + path_handlers_.clear(); costmap_thread_.reset(); } @@ -70,70 +66,29 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "Configuring controller interface"); - RCLCPP_INFO(get_logger(), "getting progress checker plugins.."); - progress_checker_ids_ = declare_or_get_parameter( - "progress_checker_plugins", default_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.."); - goal_checker_ids_ = declare_or_get_parameter("goal_checker_plugins", default_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])); - } - } - - controller_ids_ = declare_or_get_parameter("controller_plugins", default_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()); - - controller_frequency_ = declare_or_get_parameter("controller_frequency", 20.0); - min_x_velocity_threshold_ = declare_or_get_parameter("min_x_velocity_threshold", 0.0001); - min_y_velocity_threshold_ = declare_or_get_parameter("min_y_velocity_threshold", 0.0001); - min_theta_velocity_threshold_ = declare_or_get_parameter("min_theta_velocity_threshold", 0.0001); - RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); - - std::string speed_limit_topic = declare_or_get_parameter( - "speed_limit_topic", std::string("speed_limit")); - std::string odom_topic = declare_or_get_parameter("odom_topic", std::string("odom")); - double odom_duration = declare_or_get_parameter("odom_duration", 0.3); - failure_tolerance_ = declare_or_get_parameter("failure_tolerance", 0.0); - use_realtime_priority_ = declare_or_get_parameter("use_realtime_priority", false); - publish_zero_velocity_ = declare_or_get_parameter("publish_zero_velocity", true); - search_window_ = declare_or_get_parameter("search_window", 2.0); - 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()); + } 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 != 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(), @@ -143,8 +98,8 @@ 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(" "); } if (progress_checker_ids_concat_.empty()) { progress_checker_ids_concat_ = "(none)"; @@ -154,16 +109,15 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) 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(), @@ -173,26 +127,52 @@ 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_->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, 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 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()); + + 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(), @@ -202,21 +182,20 @@ 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( 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"); + transformed_plan_pub_ = create_publisher("transformed_global_plan"); tracking_feedback_pub_ = create_publisher("tracking_feedback"); - double costmap_update_timeout_dbl = declare_or_get_parameter("costmap_update_timeout", 0.30); - costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); - // 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 { @@ -225,7 +204,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); @@ -234,7 +213,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; @@ -254,12 +233,11 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) it->second->activate(); } vel_publisher_->on_activate(); + transformed_plan_pub_->on_activate(); tracking_feedback_pub_->on_activate(); action_server_->activate(); + param_handler_->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(); @@ -289,10 +267,9 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) publishZeroVelocity(); vel_publisher_->on_deactivate(); + transformed_plan_pub_->on_deactivate(); tracking_feedback_pub_->on_deactivate(); - - remove_on_set_parameters_callback(dyn_params_handler_.get()); - dyn_params_handler_.reset(); + param_handler_->deactivate(); // destroy bond connection destroyBond(); @@ -314,6 +291,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) goal_checkers_.clear(); progress_checkers_.clear(); + path_handlers_.clear(); costmap_ros_->cleanup(); @@ -323,6 +301,8 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) odom_sub_.reset(); costmap_thread_.reset(); vel_publisher_.reset(); + transformed_plan_pub_.reset(); + tracking_feedback_pub_.reset(); speed_limit_sub_.reset(); return nav2::CallbackReturn::SUCCESS; @@ -429,9 +409,35 @@ 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; +} + 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."); @@ -465,13 +471,21 @@ 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); if (!current_progress_checker_.empty()) { 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(); @@ -496,7 +510,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(); @@ -516,7 +530,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()); loop_rate.reset(); } } @@ -610,7 +624,8 @@ 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_handlers_[current_path_handler_]->setPlan(path); end_pose_ = path.poses.back(); end_pose_.header.frame_id = path.header.frame_id; @@ -640,6 +655,12 @@ 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); + 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; try { @@ -647,14 +668,17 @@ void ControllerServer::computeAndPublishVelocity() controllers_[current_controller_]->computeVelocityCommands( pose, twist, - goal_checkers_[current_goal_checker_].get()); + goal_checkers_[current_goal_checker_].get(), + transformed_global_plan_, + goal + ); last_valid_cmd_time_ = now(); cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); cmd_vel_2d.header.stamp = last_valid_cmd_time_; // 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; @@ -664,8 +688,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"); } @@ -684,7 +708,7 @@ void ControllerServer::computeAndPublishVelocity() if (!nav2_util::transformPoseInTargetFrame( end_pose_, transformed_end_pose_, *costmap_ros_->getTfBuffer(), - costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance())) + costmap_ros_->getGlobalFrameID(), transform_tolerance_)) { throw nav2_core::ControllerTFError("Failed to transform end pose to global frame"); } @@ -697,13 +721,13 @@ 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"); } const auto path_search_result = nav2_util::distance_from_path( - current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); + current_path_, robot_pose_in_path_frame.pose, start_index_, params_->search_window); // Create tracking error message auto tracking_feedback_msg = std::make_unique(); @@ -776,6 +800,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); } } @@ -808,7 +848,7 @@ void ControllerServer::publishZeroVelocity() void ControllerServer::onGoalExit(bool force_stop) { - if (publish_zero_velocity_ || force_stop) { + if (params_->publish_zero_velocity || force_stop) { publishZeroVelocity(); } @@ -830,7 +870,7 @@ bool ControllerServer::isGoalReached() return goal_checkers_[current_goal_checker_]->isGoalReached( pose.pose, transformed_end_pose_.pose, - velocity); + velocity, transformed_global_plan_); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) @@ -851,50 +891,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 new file mode 100644 index 00000000000..0490295cf3e --- /dev/null +++ b/nav2_controller/src/parameter_handler.cpp @@ -0,0 +1,237 @@ +// 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, + const rclcpp::Logger & logger) +{ + node_ = node; + logger_ = logger; + + 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); + 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); + 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); + + RCLCPP_INFO( + logger_, + "Controller frequency set to %.4fHz", + params_.controller_frequency); + + RCLCPP_INFO(logger_, "getting progress checker plugins.."); + 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( + node, default_progress_checker_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_progress_checker_types_[i])); + } + } + + RCLCPP_INFO(logger_, "getting goal checker plugins.."); + 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( + node, default_goal_checker_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_goal_checker_types_[i])); + } + } + + 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( + node, default_controller_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_controller_types_[i])); + } + } + + 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 { + 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()); + } + } + + 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()); + } + } +} + +void ParameterHandler::activate() +{ + auto node = node_.lock(); + 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)); +} + +void ParameterHandler::deactivate() +{ + 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(); +} + +ParameterHandler::~ParameterHandler() +{ +} +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 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 && param_name != "failure_tolerance") { + 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::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_name.find('.') != std::string::npos) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + 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 == "search_window") { + params_.search_window = parameter.as_double(); + } + } + } +} + +} // namespace nav2_controller diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp index 551551b4aed..eeb29a1d7e3 100644 --- a/nav2_controller/test/test_dynamic_parameters.cpp +++ b/nav2_controller/test/test_dynamic_parameters.cpp @@ -23,74 +23,54 @@ #include "nav2_controller/controller_server.hpp" #include "rclcpp/rclcpp.hpp" -class ControllerShim : public nav2_controller::ControllerServer +TEST(ControllerServerTest, test_dynamic_parameters) { -public: - ControllerShim() - : nav2_controller::ControllerServer(rclcpp::NodeOptions()) - { - } - - /** - * @brief Declare parameters needed for dynamic parameter testing. - * - * This method mirrors the parameter declarations in the - * on_configure method of ControllerServer. - */ - void declareTestParameters() - { - declare_parameter("min_x_velocity_threshold", 0.0001); - declare_parameter("min_y_velocity_threshold", 0.0001); - declare_parameter("min_theta_velocity_threshold", 0.0001); - declare_parameter("failure_tolerance", 0.0); - } - - // Since we cannot call configure/activate due to costmaps - // requiring TF - void setDynamicCallback() - { - declareTestParameters(); - - 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(); + 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( + node->get_node_base_interface(), + results); + + 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( - 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_->search_window, 10.0); } int main(int argc, char ** argv) 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/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index d24e84d6680..b52f42d7259 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -92,10 +92,13 @@ 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 + * @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 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 @@ -108,12 +111,16 @@ 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 global plan after being processed by the path handler + * @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( 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, + const geometry_msgs::msg::PoseStamped & global_goal) = 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..2fde1fad80a 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" @@ -80,11 +81,14 @@ 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( - 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 & transformed_global_plan) = 0; /** * @brief Get the maximum possible tolerances used for goal checking in the major types. 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..cea4070662a --- /dev/null +++ b/nav2_core/include/nav2_core/path_handler.hpp @@ -0,0 +1,82 @@ +// 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 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 +{ +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 rclcpp::Logger & logger, + 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 transform global plan to local applying constraints, + * then prune global plan + * @param pose pose to transform + * @return Path after pruned + */ + 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__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 63013bd091e..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 @@ -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 @@ -105,12 +105,16 @@ 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 global plan after being processed by the path handler + * @param global_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*/) override; + nav2_core::GoalChecker * /*goal_checker*/, + nav_msgs::msg::Path & transformed_global_plan, + const geometry_msgs::msg::PoseStamped & global_goal) override; /** * @brief Score a given command. Can be used for testing. @@ -137,12 +141,16 @@ 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 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( 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 & transformed_global_plan, + const geometry_msgs::msg::PoseStamped & global_goal); /** * @brief Limits the maximum linear speed of the robot. @@ -159,17 +167,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 */ @@ -178,30 +175,7 @@ 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_; 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..75cdee9ad41 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,10 @@ 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 Full LocalPlanEvaluation + * 3) Markers representing the different trajectories evaluated + * 4) The CostGrid (in the form of a complex PointCloud2) */ class DWBPublisher { @@ -94,8 +92,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); protected: @@ -108,8 +104,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_; bool publish_cost_grid_pc_; @@ -120,8 +114,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_; 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 d0373d98d91..59b98656432 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -83,52 +83,23 @@ 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_ + ".transform_tolerance", - rclcpp::ParameterValue(0.1)); - 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)); 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(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(); @@ -235,23 +206,21 @@ 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(); } - traj_generator_->reset(); - - pub_->publishGlobalPlan(path); - global_plan_ = path; } 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, + const geometry_msgs::msg::PoseStamped & global_goal) { std::shared_ptr results = nullptr; if (pub_->shouldRecordEvaluation()) { @@ -261,7 +230,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, transformed_global_plan, global_goal); pub_->publishEvaluation(results); geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.twist = nav_2d_utils::twist2Dto3D(cmd_vel2d.velocity); @@ -281,45 +250,24 @@ 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) -{ - transformed_plan = transformGlobalPlan(pose); - 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 & transformed_global_plan, + const geometry_msgs::msg::PoseStamped & global_goal) { if (results) { results->header.frame_id = pose.header.frame_id; results->header.stamp = clock_->now(); } - nav_msgs::msg::Path transformed_plan; - geometry_msgs::msg::PoseStamped goal_pose; - - prepareGlobalPlan(pose, transformed_plan, 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, global_goal.pose, transformed_global_plan)) { RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare"); } } @@ -461,115 +409,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_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index 46651d586f6..4626b8f34d9 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -75,12 +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)); declare_parameter_if_not_declared( node, plugin_name_ + ".publish_local_plan", rclcpp::ParameterValue(true)); @@ -95,15 +89,11 @@ 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"); cost_grid_pc_pub_ = node->create_publisher("cost_cloud"); @@ -119,8 +109,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(); cost_grid_pc_pub_->on_activate(); @@ -132,8 +120,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(); cost_grid_pc_pub_->on_deactivate(); @@ -145,8 +131,6 @@ nav2::CallbackReturn DWBPublisher::on_cleanup() { eval_pub_.reset(); - global_pub_.reset(); - transformed_pub_.reset(); local_pub_.reset(); marker_pub_.reset(); cost_grid_pc_pub_.reset(); @@ -337,18 +321,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) -{ - publishGenericPlan(plan, *transformed_pub_, publish_transformed_); -} - void DWBPublisher::publishLocalPlan(const nav_msgs::msg::Path plan) { 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..91008decb08 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,12 @@ 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); 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/README.md b/nav2_graceful_controller/README.md index 331a91352b1..5e5b20462f9 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -15,9 +15,7 @@ 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. | -| `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. | @@ -36,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 28bec7eddd8..43f16d8fd8e 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" @@ -84,18 +83,22 @@ 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 global plan after being processed by the path handler + * @param global_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) override; + nav2_core::GoalChecker * goal_checker, + nav_msgs::msg::Path & transformed_global_plan, + const geometry_msgs::msg::PoseStamped & global_goal) 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. @@ -189,11 +192,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/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp index f5fe81ac462..898d45c4abf 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -30,10 +30,8 @@ namespace nav2_graceful_controller 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; @@ -67,7 +65,7 @@ class ParameterHandler : public nav2_util::ParameterHandler ParameterHandler( const nav2::LifecycleNode::SharedPtr & node, std::string & plugin_name, - rclcpp::Logger & logger, const double costmap_size_x); + rclcpp::Logger & logger); protected: /** 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 8e9504cdd81..332b29bac96 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" @@ -43,14 +44,9 @@ 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 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(); @@ -106,7 +98,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(); @@ -116,7 +107,9 @@ 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_global_plan, + const geometry_msgs::msg::PoseStamped & /*global_goal*/) { std::lock_guard param_lock(param_handler_->getMutex()); @@ -132,22 +125,24 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( goal_dist_tolerance_ = pose_tolerance.position.x; } + // Transform the plan from costmap's global frame to robot base frame + nav_msgs::msg::Path transformed_plan; + 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( 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 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 { @@ -247,9 +242,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::newPathReceived(const nav_msgs::msg::Path & /*raw_global_path*/) { - path_handler_->setPlan(path); goal_reached_ = false; do_initial_rotation_ = true; } diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 2b9cfdfbcab..221f29f6d11 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -29,19 +29,14 @@ using rcl_interfaces::msg::ParameterType; ParameterHandler::ParameterHandler( const nav2::LifecycleNode::SharedPtr & node, std::string & plugin_name, - rclcpp::Logger & logger, const double costmap_size_x) + rclcpp::Logger & logger) : nav2_util::ParameterHandler(node, logger) { plugin_name_ = plugin_name; - 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( 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)); @@ -71,17 +66,8 @@ 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(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); @@ -166,9 +152,7 @@ ParameterHandler::updateParametersCallback( continue; } 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/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_graceful_controller/test/CMakeLists.txt b/nav2_graceful_controller/test/CMakeLists.txt index 93bed7aae27..bb52ec4454e 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 1fe65d341de..24ffd4002fc 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" @@ -57,8 +58,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 +73,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) { @@ -209,40 +202,11 @@ 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()); 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)); @@ -267,8 +231,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), @@ -291,7 +254,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); @@ -497,321 +459,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()); - - // 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); - 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); - - EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::InvalidPath); -} - -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()); - - // 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); - 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); - - EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::ControllerException); -} - -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()); - - // 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); - 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); - - // 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) { - 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()); - - // 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); - 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); - - // Check results: the plan should be pruned - auto transformed_plan = controller->transformGlobalPlan(robot_pose); - EXPECT_EQ(transformed_plan.poses.size(), 3u); -} - -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()); - - // 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); - 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); - - // Check results: the plan should be pruned - auto transformed_plan = controller->transformGlobalPlan(robot_pose); - EXPECT_EQ(transformed_plan.poses.size(), 2u); -} - TEST(GracefulControllerTest, computeVelocityCommandRotate) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -838,6 +485,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; @@ -873,7 +522,8 @@ 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); + path_handler.setPlan(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -883,8 +533,10 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { // 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 = path_handler.transformGlobalPlan(robot_pose); + geometry_msgs::msg::PoseStamped goal; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, + 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. @@ -914,6 +566,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; @@ -949,7 +603,8 @@ 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); + path_handler.setPlan(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -959,8 +614,10 @@ 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 = path_handler.transformGlobalPlan(robot_pose); + geometry_msgs::msg::PoseStamped goal; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, + 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. @@ -971,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( @@ -995,11 +653,13 @@ 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; 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)); @@ -1019,18 +679,19 @@ 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->setPlan(plan); + controller->newPathReceived(plan); + path_handler.setPlan(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -1040,8 +701,10 @@ 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 = path_handler.transformGlobalPlan(robot_pose); + geometry_msgs::msg::PoseStamped goal; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, + transformed_global_plan, goal); // Check results: the robot should go straight to the target. // So, both linear velocity should be some negative values. @@ -1072,6 +735,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; @@ -1115,7 +780,8 @@ 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); + path_handler.setPlan(plan); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -1126,7 +792,10 @@ 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 = path_handler.transformGlobalPlan(robot_pose); + geometry_msgs::msg::PoseStamped goal; + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker, + 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/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/README.md b/nav2_mppi_controller/README.md index 63db3f0aa03..07cff357751 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -68,16 +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 | - | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | 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. | - #### Ackermann Motion Model | Parameter | Type | Definition | | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -205,8 +195,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" @@ -293,7 +281,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 6585fa8cbd8..1760cdd5864 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -106,12 +106,14 @@ 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 transformed_global_plan; + geometry_msgs::msg::PoseStamped goal; for (auto _ : state) { - controller->computeVelocityCommands(pose, velocity, dummy_goal_checker); + 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 1c52df96f68..67bf60e7dc4 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" @@ -81,17 +80,21 @@ 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 global plan after being processed by the path handler + * @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) override; + nav2_core::GoalChecker * goal_checker, + nav_msgs::msg::Path & transformed_global_plan, + const geometry_msgs::msg::PoseStamped & global_goal) 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 @@ -103,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); @@ -121,7 +122,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/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/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 4871169bf52..1356a06a968 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -511,59 +511,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/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index f8c20f4f399..1b9d6da8e62 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, + const geometry_msgs::msg::PoseStamped & global_goal) { #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); 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, global_goal.pose, + goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); @@ -129,25 +128,23 @@ 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::setPlan(const nav_msgs::msg::Path & path) +void MPPIController::newPathReceived(const nav_msgs::msg::Path & /*raw_global_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 04f2dcc58dc..00000000000 --- a/nav2_mppi_controller/src/path_handler.cpp +++ /dev/null @@ -1,208 +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/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/CMakeLists.txt b/nav2_mppi_controller/test/CMakeLists.txt index 083de690ebd..751066471f2 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 5bdfb8a1121..4c8645d4128 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -56,9 +56,11 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) path.header.frame_id = costmap_ros->getGlobalFrameID(); pose.header.frame_id = costmap_ros->getGlobalFrameID(); - controller->setPlan(path); - - EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {})); + controller->newPathReceived(path); + nav_msgs::msg::Path transformed_global_plan; + geometry_msgs::msg::PoseStamped 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_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 27ef0a69762..29e31c09d4c 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_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 7dba563252b..38a98dae9e5 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 & /*transformed_global_plan*/) {return false;} virtual bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, @@ -340,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_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 48c4b59d079..ccf66dd7a5d 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/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/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 2caec97ff32..4a3f632dba5 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. | @@ -89,7 +88,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. | @@ -123,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 @@ -138,7 +135,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 e6626c2244c..190216ffd76 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 @@ -57,10 +57,8 @@ 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; bool stateful; }; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp deleted file mode 100644 index 135a304dc26..00000000000 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ /dev/null @@ -1,88 +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_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ -#define NAV2_REGULATED_PURE_PURSUIT_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 "geometry_msgs/msg/pose.hpp" - -namespace nav2_regulated_pure_pursuit_controller -{ - -/** - * @class nav2_regulated_pure_pursuit_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 - */ - PathHandler( - double transform_tolerance, - std::shared_ptr tf, - std::shared_ptr costmap_ros); - - /** - * @brief Destrructor for nav2_regulated_pure_pursuit_controller::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) - * @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 - */ - nav_msgs::msg::Path transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist, bool reject_unit_path = false); - - void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;} - - nav_msgs::msg::Path getPlan() {return global_plan_;} - -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; - - 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 - -#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ 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..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 @@ -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" @@ -82,28 +81,27 @@ 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 global_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*/) override; + nav2_core::GoalChecker * /*goal_checker*/, + nav_msgs::msg::Path & transformed_global_plan, + const geometry_msgs::msg::PoseStamped & global_goal) override; 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. @@ -174,13 +172,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_; @@ -196,12 +187,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/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 31bc7b7f19d..3bb10cd7563 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -47,8 +47,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)); @@ -94,9 +92,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)); @@ -115,7 +110,6 @@ 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( plugin_name_ + ".use_velocity_scaled_lookahead_dist", params_.use_velocity_scaled_lookahead_dist); @@ -166,15 +160,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", @@ -237,6 +222,7 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda } return result; } + void ParameterHandler::updateParametersCallback( const std::vector & parameters) @@ -287,16 +273,11 @@ 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_name == plugin_name_ + ".max_robot_pose_search_dist") { - params_.max_robot_pose_search_dist = parameter.as_double(); } else if (param_name == plugin_name_ + ".approach_velocity_scaling_dist") { params_.approach_velocity_scaling_dist = 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") { diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp deleted file mode 100644 index 68d138c2ea8..00000000000 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ /dev/null @@ -1,138 +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_regulated_pure_pursuit_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 -{ - -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_(tf), costmap_ros_(costmap_ros) -{ -} - -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; -} - -nav_msgs::msg::Path PathHandler::transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist, - bool reject_unit_path) -{ - if (global_plan_.poses.empty()) { - throw nav2_core::InvalidPath("Received plan with zero length"); - } - - if (reject_unit_path && global_plan_.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_)) - { - 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); - - // 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); - }); - - // 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 && - transformation_begin == std::prev(closest_pose_upper_bound)) - { - transformation_begin = 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_.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_.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; - - // 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; -} - -} // namespace nav2_regulated_pure_pursuit_controller 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 b348bdf7933..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; @@ -60,10 +61,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 +70,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 +84,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 +96,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(); @@ -115,7 +109,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(); @@ -167,7 +160,9 @@ 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_global_plan, + const geometry_msgs::msg::PoseStamped & /*global_goal*/) { std::lock_guard lock_reinit(param_handler_->getMutex()); @@ -183,29 +178,20 @@ 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); + // Transform the plan from costmap's global frame to robot base frame + nav_msgs::msg::Path transformed_plan; + 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); 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; @@ -392,10 +378,10 @@ 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; - path_handler_->setPlan(path); } void RegulatedPurePursuitController::setSpeedLimit( @@ -424,53 +410,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 6e1ad1eec7f..aec04163dc0 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( @@ -79,18 +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); - } - - 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) @@ -108,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); @@ -145,46 +123,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(); @@ -452,7 +390,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), @@ -483,7 +420,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); @@ -522,361 +458,7 @@ 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) +int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); 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 8a556dac40d..c996b4cb9c0 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,18 +83,22 @@ 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 global plan after being processed by the path handler + * @param global_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*/) override; + nav2_core::GoalChecker * /*goal_checker*/, + nav_msgs::msg::Path & transformed_global_plan, + const geometry_msgs::msg::PoseStamped & global_goal) 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. @@ -119,13 +123,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 210e63c6679..dbb623e961f 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -122,30 +122,25 @@ 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, + const geometry_msgs::msg::PoseStamped & global_goal) { // Rotate to goal heading when in goal xy tolerance if (params_->rotate_to_goal_heading) { std::lock_guard lock_reinit(param_handler_->getMutex()); 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); @@ -209,7 +204,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); + auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker, + transformed_global_plan, global_goal); last_angular_vel_ = cmd_vel.twist.angular.z; return cmd_vel; } @@ -241,18 +237,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) { @@ -346,11 +330,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_ = params_->rotate_to_heading_once ? isGoalChanged(path) : true; - current_path_ = path; - primary_controller_->setPlan(path); + path_updated_ = params_->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/CMakeLists.txt b/nav2_rotation_shim_controller/test/CMakeLists.txt index 0ece4014727..e1dd8182dcb 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 3e13a879c1f..93ef808272b 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" @@ -142,7 +143,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 +154,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 +193,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( @@ -230,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); @@ -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,14 +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 - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), 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); path.header.frame_id = "base_link"; path.poses[1].pose.position.x = 0.1; @@ -286,27 +280,14 @@ 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); + path_handler.setPlan(path); tf_broadcaster->sendTransform(transform); - auto effort = controller->computeVelocityCommands(pose, velocity, &checker); + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); + 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->setPlan(path); - tf_broadcaster->sendTransform(transform); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); } TEST(RotationShimControllerTest, openLoopRotationTests) { @@ -346,6 +327,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; @@ -377,13 +360,18 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { path.poses[3].header.frame_id = "base_link"; // Calculate first velocity command - controller->setPlan(path); - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + controller->newPathReceived(path); + path_handler.setPlan(path); + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); + 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); // 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, + goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4); } @@ -420,6 +408,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; @@ -450,15 +440,23 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[3].pose.orientation.w = 0.9238795; path.poses[3].header.frame_id = "base_link"; - controller->setPlan(path); - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + controller->newPathReceived(path); + path_handler.setPlan(path); + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); + 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); // 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); + 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); } @@ -499,6 +497,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; @@ -530,13 +530,18 @@ TEST(RotationShimControllerTest, accelerationTests) { path.poses[3].header.frame_id = "base_link"; // Test acceleration limits - controller->setPlan(path); - auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + controller->newPathReceived(path); + path_handler.setPlan(path); + nav_msgs::msg::Path transformed_global_plan = path_handler.transformGlobalPlan(pose); + 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); // 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, + goal); EXPECT_NEAR(cmd_vel.twist.angular.z, -std::sqrt(2 * 0.5 * M_PI / 4), 1e-4); } @@ -583,7 +588,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_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 c117be8433b..8981f3375ce 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() @@ -149,6 +158,11 @@ void Selector::setProgressChecker() setSelection(progress_checker_, pub_progress_checker_); } +void Selector::setPathHandler() +{ + setSelection(path_handler_, pub_path_handler_); +} + void Selector::loadPlugins() { @@ -169,11 +183,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_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 fd6b7efd1c6..f668c423d15 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( diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index c763808912f..f7ae9284042 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -60,6 +60,7 @@ controller_server: 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 @@ -72,13 +73,19 @@ 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" 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 +108,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..7f4635cef40 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -61,6 +61,7 @@ controller_server: 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 @@ -73,7 +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" @@ -100,7 +109,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..16449c2c2c5 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -61,6 +61,7 @@ controller_server: 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 @@ -73,7 +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" @@ -100,7 +109,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/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp index cb5c626b2b8..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 @@ -41,12 +41,14 @@ 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 &, const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + nav2_core::GoalChecker *, + nav_msgs::msg::Path &, + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::ControllerException("Unknown Error"); } @@ -59,7 +61,9 @@ 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 &, + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::ControllerTFError("TF error"); } @@ -70,7 +74,9 @@ 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 &, + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::FailedToMakeProgress("Failed to make progress"); } @@ -81,7 +87,9 @@ 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 &, + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::PatienceExceeded("Patience exceeded"); } @@ -92,7 +100,9 @@ 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 &, + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::InvalidPath("Invalid path"); } @@ -103,7 +113,9 @@ 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 &, + const geometry_msgs::msg::PoseStamped &) { throw nav2_core::NoValidControl("No valid control"); } 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..e83b8f0db7d 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -7,6 +7,7 @@ controller_server: 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 +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" @@ -64,7 +73,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 @@ -91,6 +99,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_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 01fe9656d5b..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 @@ -34,6 +34,7 @@ controller_server: 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 +53,15 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters + 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" time_steps: 56 @@ -71,8 +80,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/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 46d678e08c3..d0f74f1ee9d 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -77,6 +77,7 @@ controller_server: 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 +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" @@ -122,7 +131,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/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', diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index e6c1f2ee642..293b5c67f0a 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -18,13 +18,18 @@ #include #include #include +#include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "nav_msgs/msg/path.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2/utils.hpp" +#include "angles/angles.h" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_util { @@ -56,6 +61,119 @@ 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); + +/** + * @brief Find the iterator of the first pose at which there is an inversion or in place rotation on the path, + * @param path to check for inversion or rotation + * @param rotation_threshold Minimum rotation angle to consider an in-place rotation + * @return the first point after the inversion or in place rotation found in the path + */ +inline unsigned int findFirstPathConstraint( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + unsigned int rotation_idx = path.poses.size(); + + // Iterating through the path to determine the position of the path inversion or rotation + for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + float oa_x = 0.0f; + float oa_y = 0.0f; + 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; + if (idx > 0) { + oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].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; + } + } + + float translation = hypot(ab_x, ab_y); + if (translation < 1e-4) { + float accumulated_rotation = 0.0f; + unsigned int end_idx = idx; + + // Continue checking while translation remains small + // until accumulated rotation is larger than threshold + while (end_idx < path.poses.size() - 1) { + float current_yaw = tf2::getYaw(path.poses[end_idx].pose.orientation); + float next_yaw = tf2::getYaw(path.poses[end_idx + 1].pose.orientation); + accumulated_rotation += fabs(angles::shortest_angular_distance(current_yaw, next_yaw)); + if (accumulated_rotation > rotation_threshold) { + // We found the constraint here, but there might be smaller index + rotation_idx = std::min(rotation_idx, end_idx + 1); + break; + } + if (end_idx + 2 < path.poses.size()) { + float dx = path.poses[end_idx + 2].pose.position.x - + path.poses[end_idx + 1].pose.position.x; + float dy = path.poses[end_idx + 2].pose.position.y - + path.poses[end_idx + 1].pose.position.y; + // Stop if translation resumes + if (hypot(dx, dy) > 1e-4) { + break; + } + } else { + // We have reached the end of the path + break; + } + end_idx++; + } + } + } + + return rotation_idx; +} + +/** + * @brief Find and remove poses after the first constraint in the path + * @param path to check for inversion or rotation + * * @param rotation_threshold Minimum rotation angle to consider an in-place rotation + * @return The location of the inversion or rotation, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstConstraint( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_constraint = findFirstPathConstraint(cropped_path, + rotation_threshold); + if (first_after_constraint == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_constraint, cropped_path.poses.end()); + path = cropped_path; + return first_after_constraint; +} + } // namespace nav2_util #endif // NAV2_UTIL__PATH_UTILS_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index d1ee2e86a6e..04917518af2 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -27,6 +27,7 @@ target_link_libraries(${library_name} PUBLIC ${tf2_geometry_msgs_TARGETS} pluginlib::pluginlib nav2_ros_common::nav2_ros_common + angles::angles ) target_link_libraries(${library_name} PRIVATE ${bond_TARGETS} diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp index 57e88e9bddd..2ef48c7fe75 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 diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index 82b98a3b16d..3971af09e81 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) { @@ -375,3 +376,172 @@ 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(); +} + +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::findFirstPathConstraint(path), 10u); + + // To short to process + path.poses.erase(path.poses.begin(), path.poses.begin() + 7); + EXPECT_EQ(nav2_util::findFirstPathConstraint(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::findFirstPathConstraint(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::findFirstPathConstraint(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::removePosesAfterFirstConstraint(path), 0u); + + // try empty path + path.poses.clear(); + EXPECT_EQ(nav2_util::removePosesAfterFirstConstraint(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::removePosesAfterFirstConstraint(path), 11u); + // Check to see if removed + EXPECT_EQ(path.poses.size(), 11u); + EXPECT_EQ(path.poses.back().pose.position.x, 10); +}