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 1dc46f19e9..ea49c7acf7 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 @@ -17,6 +17,7 @@ #include #include +#include #include "nav2_msgs/action/follow_path.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -89,6 +90,7 @@ class FollowPathAction : public BtActionNode { // Register JSON definitions for the types used in the ports BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition>(); return providedBasicPorts( { @@ -96,6 +98,8 @@ class FollowPathAction : public BtActionNode BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), BT::InputPort("progress_checker_id", ""), + BT::InputPort>("key_poses", + "Key poses along the path"), BT::OutputPort( "error_code_id", "The follow path error code"), BT::OutputPort( diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index e89a23e282..91bbffc01f 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("key_poses", goal_.key_poses); } 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::vector new_key_poses; + getInput("key_poses", new_key_poses); + + if (goal_.key_poses != new_key_poses) { + goal_.key_poses = new_key_poses; + goal_updated_ = true; + } } } // namespace nav2_behavior_tree diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index a112a3c9d6..b94e20f41e 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -156,8 +156,11 @@ class ControllerServer : public nav2::LifecycleNode /** * @brief Assigns path to controller * @param path Path received from action server + * @param key_poses Key poses along the path that are considered points of interest */ - void setPlannerPath(const nav_msgs::msg::Path & path); + void setPlannerPath( + const nav_msgs::msg::Path & path, + const std::vector & key_poses); /** * @brief Calculates velocity and publishes to "cmd_vel" topic */ diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 26187087ec..eaa1c4ad67 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -457,7 +457,7 @@ void ControllerServer::computeControl() throw nav2_core::ControllerException("Failed to find progress checker name: " + pc_name); } - setPlannerPath(goal->path); + setPlannerPath(goal->path, goal->key_poses); progress_checkers_[current_progress_checker_]->reset(); last_valid_cmd_time_ = now(); @@ -591,7 +591,9 @@ void ControllerServer::computeControl() action_server_->succeeded_current(); } -void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) +void ControllerServer::setPlannerPath( + const nav_msgs::msg::Path & path, + const std::vector & key_poses) { RCLCPP_DEBUG( get_logger(), @@ -599,7 +601,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) if (path.poses.empty()) { throw nav2_core::InvalidPath("Path is empty."); } - controllers_[current_controller_]->setPlan(path); + controllers_[current_controller_]->setPlan(path, key_poses); end_pose_ = path.poses.back(); end_pose_.header.frame_id = path.header.frame_id; @@ -743,7 +745,7 @@ void ControllerServer::updateGlobalPath() action_server_->terminate_current(result); return; } - setPlannerPath(goal->path); + setPlannerPath(goal->path, goal->key_poses); } } diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 261eec2243..9c96391b63 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -38,6 +38,7 @@ #include #include +#include #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_ros_common/lifecycle_node.hpp" @@ -94,8 +95,11 @@ class Controller /** * @brief local setPlan - Sets the global plan * @param path The global plan + * @param key_poses Key poses along the path that are considered points of interest */ - virtual void setPlan(const nav_msgs::msg::Path & path) = 0; + virtual void setPlan( + const nav_msgs::msg::Path & path, + const std::vector & key_poses) = 0; /** * @brief Controller computeVelocityCommands - calculates the best command given the current pose and velocity diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 59b987f9cd..6e61e75056 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 @@ -91,8 +91,11 @@ class DWBLocalPlanner : public nav2_core::Controller /** * @brief nav2_core setPlan - Sets the global plan * @param path The global plan + * @param key_poses Key poses along the path that are considered points of interest */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan( + const nav_msgs::msg::Path & path, + const std::vector & key_poses) override; /** * @brief nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 0af3291202..e74210ac09 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -235,7 +235,9 @@ DWBLocalPlanner::loadCritics() } void -DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) +DWBLocalPlanner::setPlan( + const nav_msgs::msg::Path & path, + const std::vector & /*key_poses*/) { auto path2d = nav_2d_utils::pathToPath2D(path); for (TrajectoryCritic::Ptr & critic : critics_) { 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 96358ae960..dff777e837 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -94,8 +94,11 @@ class GracefulController : public nav2_core::Controller /** * @brief nav2_core setPlan - Sets the global plan. * @param path The global plan + * @param key_poses Key poses along the path that are considered points of interest */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan( + const nav_msgs::msg::Path & path, + const std::vector & key_poses) override; /** * @brief Limits the maximum linear speed of the robot. diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index fb0010698f..f5fb9a9592 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -252,7 +252,9 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( throw nav2_core::NoValidControl("Collision detected in trajectory"); } -void GracefulController::setPlan(const nav_msgs::msg::Path & path) +void GracefulController::setPlan( + const nav_msgs::msg::Path & path, + const std::vector & /*key_poses*/) { path_handler_->setPlan(path); goal_reached_ = false; diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 799daa8d8b..6db00209f3 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -225,7 +225,7 @@ TEST(GracefulControllerTest, configure) { plan.header.frame_id = "map"; plan.poses.resize(3); plan.poses[0].header.frame_id = "map"; - controller->setPlan(plan); + controller->setPlan(plan, {}); EXPECT_EQ(controller->getPlan().poses.size(), 3u); EXPECT_EQ(controller->getPlan().poses[0].header.frame_id, "map"); @@ -536,7 +536,7 @@ TEST(GracefulControllerTest, emptyPlan) { // Set an empty global plan nav_msgs::msg::Path global_plan; global_plan.header.frame_id = "test_global_frame"; - controller->setPlan(global_plan); + controller->setPlan(global_plan, {}); EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::InvalidPath); } @@ -591,7 +591,7 @@ TEST(GracefulControllerTest, poseOutsideCostmap) { 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); + controller->setPlan(global_plan, {}); EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::ControllerException); } @@ -655,7 +655,7 @@ TEST(GracefulControllerTest, noPruningPlan) { 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); + controller->setPlan(global_plan, {}); // Check results: the plan should not be pruned auto transformed_plan = controller->transformGlobalPlan(robot_pose); @@ -732,7 +732,7 @@ TEST(GracefulControllerTest, pruningPlan) { 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); + controller->setPlan(global_plan, {}); // Check results: the plan should be pruned auto transformed_plan = controller->transformGlobalPlan(robot_pose); @@ -798,7 +798,7 @@ TEST(GracefulControllerTest, pruningPlanOutsideCostmap) { 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); + controller->setPlan(global_plan, {}); // Check results: the plan should be pruned auto transformed_plan = controller->transformGlobalPlan(robot_pose); @@ -866,7 +866,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { plan.poses[2].pose.position.x = 1.0; plan.poses[2].pose.position.y = 1.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(plan); + controller->setPlan(plan, {}); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -942,7 +942,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { plan.poses[2].pose.position.x = 2.0; plan.poses[2].pose.position.y = 0.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(plan); + controller->setPlan(plan, {}); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -1023,7 +1023,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { plan.poses[2].pose.position.x = -2.0; plan.poses[2].pose.position.y = 0.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(plan); + controller->setPlan(plan, {}); // Set velocity geometry_msgs::msg::Twist robot_velocity; @@ -1108,7 +1108,7 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { plan.poses[4].pose.position.x = 0.2; plan.poses[4].pose.position.y = 0.0; plan.poses[4].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(plan); + controller->setPlan(plan, {}); // Set velocity geometry_msgs::msg::Twist robot_velocity; diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp index 6585fa8cbd..550aebdbf9 100644 --- a/nav2_mppi_controller/benchmark/controller_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -106,7 +106,7 @@ void prepareAndRunBenchmark( auto velocity = getDummyTwist(); auto path = getIncrementalDummyPath(node, path_settings); - controller->setPlan(path); + controller->setPlan(path, {}); nav2_core::GoalChecker * dummy_goal_checker{nullptr}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 1c52df96f6..2f24fded27 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "nav2_mppi_controller/tools/path_handler.hpp" #include "nav2_mppi_controller/optimizer.hpp" @@ -90,8 +91,11 @@ class MPPIController : public nav2_core::Controller /** * @brief Set new reference path to track * @param path Path to track + * @param key_poses Key poses along the path that are considered points of interest */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan( + const nav_msgs::msg::Path & path, + const std::vector & key_poses) override; /** * @brief Set new speed limit from callback diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index a9fab10c85..011c168879 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -141,7 +141,9 @@ void MPPIController::visualize( trajectory_visualizer_.visualize(std::move(transformed_plan)); } -void MPPIController::setPlan(const nav_msgs::msg::Path & path) +void MPPIController::setPlan( + const nav_msgs::msg::Path & path, + const std::vector & /*key_poses*/) { path_handler_.setPath(path); } diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index ef32775a5b..6a35ccb65a 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -56,7 +56,7 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) path.header.frame_id = costmap_ros->getGlobalFrameID(); pose.header.frame_id = costmap_ros->getGlobalFrameID(); - controller->setPlan(path); + controller->setPlan(path, {}); EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {})); diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 80b0a1d4ef..1de732d0fb 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 +geometry_msgs/PoseStamped[] key_poses --- #result definition 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 42ed663d9b..08a0f9940b 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 @@ -102,8 +102,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller /** * @brief nav2_core setPlan - Sets the global plan * @param path The global plan + * @param key_poses Key poses along the path that are considered points of interest */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan( + const nav_msgs::msg::Path & path, + const std::vector & key_poses) override; /** * @brief Limits the maximum linear speed of the robot. diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 746dccf48d..1f09337bed 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 @@ -477,7 +477,9 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = sign * linear_vel; } -void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +void RegulatedPurePursuitController::setPlan( + const nav_msgs::msg::Path & path, + const std::vector & /*key_poses*/) { has_reached_xy_tolerance_ = false; path_handler_->setPlan(path); 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 7639a7ee8b..bab1ad55a0 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -134,7 +134,7 @@ TEST(RegulatedPurePursuitTest, basicAPI) nav_msgs::msg::Path path; path.poses.resize(2); path.poses[0].header.frame_id = "fake_frame"; - ctrl->setPlan(path); + ctrl->setPlan(path, {}); EXPECT_EQ(ctrl->getPlan().poses.size(), 2ul); EXPECT_EQ(ctrl->getPlan().poses[0].header.frame_id, std::string("fake_frame")); @@ -196,13 +196,13 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) 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); + 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); + ctrl->setPlan(path, {}); rtn = ctrl->findVelocitySignChangeWrapper(path); EXPECT_EQ(rtn, std::numeric_limits::max()); } @@ -914,7 +914,7 @@ TEST_F(TransformGlobalPlanTest, no_pruning_on_large_costmap) std::make_unique(circle_radius) }); - ctrl_->setPlan(global_plan); + ctrl_->setPlan(global_plan, {}); // Transform the plan @@ -958,7 +958,7 @@ TEST_F(TransformGlobalPlanTest, transform_start_selection) std::make_unique(circle_radius) }); - ctrl_->setPlan(global_plan); + ctrl_->setPlan(global_plan, {}); // Transform the plan auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); @@ -1003,7 +1003,7 @@ TEST_F(TransformGlobalPlanTest, all_poses_outside_of_costmap) std::make_unique(circle_radius) }); - ctrl_->setPlan(global_plan); + ctrl_->setPlan(global_plan, {}); // Transform the plan EXPECT_THROW(ctrl_->transformGlobalPlanWrapper(robot_pose), nav2_core::ControllerException); @@ -1045,7 +1045,7 @@ TEST_F(TransformGlobalPlanTest, good_circle_shortcut) std::make_unique(circle_radius) }); - ctrl_->setPlan(global_plan); + ctrl_->setPlan(global_plan, {}); // Transform the plan auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); @@ -1090,7 +1090,7 @@ TEST_F(TransformGlobalPlanTest, costmap_pruning) std::make_unique(path_length) }); - ctrl_->setPlan(global_plan); + ctrl_->setPlan(global_plan, {}); // Transform the plan auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); @@ -1137,7 +1137,7 @@ TEST_F(TransformGlobalPlanTest, prune_after_leaving_costmap) std::make_unique(path_length) }); - ctrl_->setPlan(global_plan); + ctrl_->setPlan(global_plan, {}); // Transform the plan auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); 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 5f12c6dda6..2d876f632a 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -92,8 +92,11 @@ class RotationShimController : public nav2_core::Controller /** * @brief nav2_core setPlan - Sets the global plan * @param path The global plan + * @param key_poses Key poses along the path that are considered points of interest */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan( + const nav_msgs::msg::Path & path, + const std::vector & key_poses) override; /** * @brief Limits the maximum linear speed of the robot. diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5be66f548a..8cabe798b7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -391,11 +391,13 @@ 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::setPlan( + const nav_msgs::msg::Path & path, + const std::vector & key_poses) { path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true; current_path_ = path; - primary_controller_->setPlan(path); + primary_controller_->setPlan(path, key_poses); position_goal_checker_->reset(); } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 0b97876182..918714c5a5 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -142,7 +142,7 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) path.poses[3].pose.position.x = 10.0; path.poses[3].pose.position.y = 10.0; EXPECT_EQ(controller->isPathUpdated(), false); - controller->setPlan(path); + controller->setPlan(path, {}); EXPECT_EQ(controller->getPath().header.frame_id, std::string("hi mate!")); EXPECT_EQ(controller->getPath().poses.size(), 10u); EXPECT_EQ(controller->isPathUpdated(), true); @@ -153,12 +153,12 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) EXPECT_EQ(pose.pose.position.y, 1.0); nav_msgs::msg::Path path_invalid_leng; - controller->setPlan(path_invalid_leng); + controller->setPlan(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->setPlan(path_invalid_dists, {}); EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error); } @@ -192,7 +192,7 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) path.poses[2].pose.position.y = 1.0; path.poses[3].pose.position.x = 10.0; path.poses[3].pose.position.y = 10.0; - controller->setPlan(path); + controller->setPlan(path, {}); const geometry_msgs::msg::Twist velocity; EXPECT_EQ( @@ -271,7 +271,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // Set with a path -- should attempt to find a sampled point but throw exception // because it cannot be found, then go to RPP and throw exception because it cannot be transformed - controller->setPlan(path); + controller->setPlan(path, {}); EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); path.header.frame_id = "base_link"; @@ -286,7 +286,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // this should allow it to find the sampled point, then transform to base_link // validly because we setup the TF for it. The -1.0 should be selected since default min // is 0.5 and that should cause a rotation in place - controller->setPlan(path); + controller->setPlan(path, {}); tf_broadcaster->sendTransform(transform); auto effort = controller->computeVelocityCommands(pose, velocity, &checker); EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); @@ -304,7 +304,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // validly because we setup the TF for it. The 1.0 should be selected since default min // is 0.5 and that should cause a pass off to the RPP controller which will throw // and exception because it is off of the costmap - controller->setPlan(path); + controller->setPlan(path, {}); tf_broadcaster->sendTransform(transform); EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); } @@ -377,7 +377,7 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { path.poses[3].header.frame_id = "base_link"; // Calculate first velocity command - controller->setPlan(path); + controller->setPlan(path, {}); auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); @@ -448,14 +448,14 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[3].pose.orientation.w = 0.9238795; path.poses[3].header.frame_id = "base_link"; - controller->setPlan(path); + controller->setPlan(path, {}); auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); 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); + controller->setPlan(path, {}); cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } @@ -528,7 +528,7 @@ TEST(RotationShimControllerTest, accelerationTests) { path.poses[3].header.frame_id = "base_link"; // Test acceleration limits - controller->setPlan(path); + controller->setPlan(path, {}); auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); @@ -581,7 +581,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->setPlan(path, {}); EXPECT_EQ(controller->isGoalChangedWrapper(path), false); // Test: Last pose of the current path differs, should return true diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 32ce21abc0..fb5cfdd21b 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -513,7 +513,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 = '', + key_poses: list[PoseStamped] = []) -> Optional[RunningTask]: self.clearTaskError() """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") @@ -524,6 +525,7 @@ 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.key_poses = key_poses if key_poses is not None else [] self.info('Executing path...') send_goal_future = self.follow_path_client.send_goal_async( 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 cb5c626b2b..d457dd0967 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 @@ -17,6 +17,7 @@ #include #include +#include #include "nav2_core/controller.hpp" #include "nav2_core/controller_exceptions.hpp" @@ -41,7 +42,7 @@ class UnknownErrorController : public nav2_core::Controller void deactivate() {} - void setPlan(const nav_msgs::msg::Path &) {} + void setPlan(const nav_msgs::msg::Path &, const std::vector &) {} virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &,