From 19a8165a75c9479e9d1bc017eab87255dc09c579 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Wed, 25 Oct 2023 13:46:04 +0200 Subject: [PATCH 1/5] nav2_behavior_tree/nav2_bt_navigator: enable goals to be sent in other frames than the pre-configured global_frame (map) Addresses https://github.com/ros-planning/navigation2/issues/3894, at least partially. --- .../plugins/condition/goal_reached_condition.cpp | 9 +++++---- .../src/navigators/navigate_to_pose.cpp | 15 +++++++++++++-- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e3c52928864..df605948fbe 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -73,17 +73,18 @@ void GoalReachedCondition::initialize() bool GoalReachedCondition::isGoalReached() { - geometry_msgs::msg::PoseStamped current_pose; + geometry_msgs::msg::PoseStamped goal; + getInput("goal", goal); + if (goal.header.frame_id.empty()) goal.header.frame_id = global_frame_; + geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( - current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) + current_pose, *tf_, goal.header.frame_id, robot_base_frame_, transform_tolerance_)) { RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); return false; } - geometry_msgs::msg::PoseStamped goal; - getInput("goal", goal); double dx = goal.pose.position.x - current_pose.pose.position.x; double dy = goal.pose.position.y - current_pose.pose.position.y; diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index cbd686cc040..1ee9e82c8a3 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -209,10 +209,21 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) feedback_utils_.global_frame, feedback_utils_.robot_frame, feedback_utils_.transform_tolerance); + geometry_msgs::msg::PoseStamped goal_pose; + if (!nav2_util::transformPoseInTargetFrame( + goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, + feedback_utils_.transform_tolerance)) { + RCLCPP_ERROR( + logger_, + "Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.", + goal->pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str()); + throw std::runtime_error("transformation failed"); + } + RCLCPP_INFO( logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)", current_pose.pose.position.x, current_pose.pose.position.y, - goal->pose.pose.position.x, goal->pose.pose.position.y); + goal_pose.pose.position.x, goal_pose.pose.position.y); // Reset state for new action feedback start_time_ = clock_->now(); @@ -220,7 +231,7 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goal_blackboard_id_, goal->pose); + blackboard->set(goal_blackboard_id_, goal_pose); } void From a3264681b34f9e4034411e794c4b225ce9bc73b9 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Tue, 14 Nov 2023 12:41:08 +0100 Subject: [PATCH 2/5] Address review comments (https://github.com/ros-planning/navigation2/pull/3917#pullrequestreview-1708583128) --- .../action/remove_passed_goals_action.hpp | 2 +- .../condition/goal_reached_condition.hpp | 2 +- .../action/remove_passed_goals_action.cpp | 17 +++++------ .../condition/goal_reached_condition.cpp | 3 -- .../navigators/navigate_through_poses.hpp | 3 +- .../navigators/navigate_to_pose.hpp | 3 +- .../src/navigators/navigate_through_poses.cpp | 28 ++++++++++++++----- .../src/navigators/navigate_to_pose.cpp | 27 ++++++++++-------- 8 files changed, 50 insertions(+), 35 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index 184bf1836c0..fcc02258a78 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -55,7 +55,7 @@ class RemovePassedGoals : public BT::ActionNodeBase double viapoint_achieved_radius_; double transform_tolerance_; std::shared_ptr tf_; - std::string global_frame_, robot_base_frame_; + std::string robot_base_frame_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index 674a8887b40..acbca8c960f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -92,7 +92,7 @@ class GoalReachedCondition : public BT::ConditionNode bool initialized_; double goal_reached_tol_; double transform_tolerance_; - std::string global_frame_, robot_base_frame_; + std::string robot_base_frame_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index fc768a3ab9d..3a4aee7cbf9 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -37,8 +37,6 @@ RemovePassedGoals::RemovePassedGoals( auto node = config().blackboard->get("node"); node->get_parameter("transform_tolerance", transform_tolerance_); - global_frame_ = BT::deconflictPortAndParamFrame( - node, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } @@ -57,16 +55,15 @@ inline BT::NodeStatus RemovePassedGoals::tick() using namespace nav2_util::geometry_utils; // NOLINT - geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose( - current_pose, *tf_, global_frame_, robot_base_frame_, - transform_tolerance_)) - { - return BT::NodeStatus::FAILURE; - } - double dist_to_goal; while (goal_poses.size() > 1) { + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_, + transform_tolerance_)) + { + return BT::NodeStatus::FAILURE; + } dist_to_goal = euclidean_distance(goal_poses[0].pose, current_pose.pose); if (dist_to_goal > viapoint_achieved_radius_) { diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index df605948fbe..d81df60a9ec 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -33,8 +33,6 @@ GoalReachedCondition::GoalReachedCondition( { auto node = config().blackboard->get("node"); - global_frame_ = BT::deconflictPortAndParamFrame( - node, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } @@ -75,7 +73,6 @@ bool GoalReachedCondition::isGoalReached() { geometry_msgs::msg::PoseStamped goal; getInput("goal", goal); - if (goal.header.frame_id.empty()) goal.header.frame_id = global_frame_; geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 615ce5ef64b..aee07de14c9 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -104,8 +104,9 @@ class NavigateThroughPosesNavigator /** * @brief Goal pose initialization on the blackboard + * @return bool if goal was initialized successfully to be processed */ - void initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal); + bool initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal); rclcpp::Time start_time_; std::string goals_blackboard_id_; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 4b690bb296a..447d02cc449 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -116,8 +116,9 @@ class NavigateToPoseNavigator /** * @brief Goal pose initialization on the blackboard * @param goal Action template's goal message to process + * @return bool if goal was initialized successfully to be processed */ - void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); + bool initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); rclcpp::Time start_time_; diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 92f53f9919a..389cbdd18aa 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -81,9 +81,7 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) return false; } - initializeGoalPoses(goal); - - return true; + return initializeGoalPoses(goal); } void @@ -198,13 +196,27 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) } } -void +bool NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal) { - if (goal->poses.size() > 0) { + Goals goal_poses = goal->poses; + for (auto & goal_pose : goal_poses) { + if (!nav2_util::transformPoseInTargetFrame( + goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, + feedback_utils_.transform_tolerance)) + { + RCLCPP_ERROR( + logger_, + "Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.", + goal_pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str()); + return false; + } + } + + if (goal_poses.size() > 0) { RCLCPP_INFO( logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)", - goal->poses.size(), goal->poses.back().pose.position.x, goal->poses.back().pose.position.y); + goal_poses.size(), goal_poses.back().pose.position.x, goal_poses.back().pose.position.y); } // Reset state for new action feedback @@ -213,7 +225,9 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goals_blackboard_id_, goal->poses); + blackboard->set(goals_blackboard_id_, std::move(goal_poses)); + + return true; } } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 1ee9e82c8a3..9f603b47b82 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -94,9 +94,7 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) return false; } - initializeGoalPose(goal); - - return true; + return initializeGoalPose(goal); } void @@ -200,24 +198,29 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) } } -void +bool NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) { geometry_msgs::msg::PoseStamped current_pose; - nav2_util::getCurrentPose( - current_pose, *feedback_utils_.tf, - feedback_utils_.global_frame, feedback_utils_.robot_frame, - feedback_utils_.transform_tolerance); + if (!nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance)) + { + RCLCPP_ERROR(logger_, "Initial robot pose is not available."); + return false; + } geometry_msgs::msg::PoseStamped goal_pose; if (!nav2_util::transformPoseInTargetFrame( - goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, - feedback_utils_.transform_tolerance)) { + goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, + feedback_utils_.transform_tolerance)) + { RCLCPP_ERROR( logger_, "Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.", goal->pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str()); - throw std::runtime_error("transformation failed"); + return false; } RCLCPP_INFO( @@ -232,6 +235,8 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) // Update the goal pose on the blackboard blackboard->set(goal_blackboard_id_, goal_pose); + + return true; } void From 3e64c33fb464a190870fb683339ebf99b31e36dd Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Wed, 15 Nov 2023 11:32:17 +0100 Subject: [PATCH 3/5] Address more review comments (https://github.com/ros-planning/navigation2/pull/3917#pullrequestreview-1730891741) --- .../action/remove_passed_goals_action.cpp | 15 ++++++------- .../src/navigators/navigate_through_poses.cpp | 21 ++++++++++++++----- .../src/navigators/navigate_to_pose.cpp | 21 ++++++++++++++----- 3 files changed, 40 insertions(+), 17 deletions(-) diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 3a4aee7cbf9..7a5816973dc 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -55,15 +55,16 @@ inline BT::NodeStatus RemovePassedGoals::tick() using namespace nav2_util::geometry_utils; // NOLINT + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_, + transform_tolerance_)) + { + return BT::NodeStatus::FAILURE; + } + double dist_to_goal; while (goal_poses.size() > 1) { - geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose( - current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_, - transform_tolerance_)) - { - return BT::NodeStatus::FAILURE; - } dist_to_goal = euclidean_distance(goal_poses[0].pose, current_pose.pose); if (dist_to_goal > viapoint_achieved_radius_) { diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 389cbdd18aa..22f3e49f213 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -111,10 +111,14 @@ NavigateThroughPosesNavigator::onLoop() } geometry_msgs::msg::PoseStamped current_pose; - nav2_util::getCurrentPose( - current_pose, *feedback_utils_.tf, - feedback_utils_.global_frame, feedback_utils_.robot_frame, - feedback_utils_.transform_tolerance); + if (!nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance)) + { + RCLCPP_ERROR(logger_, "Robot pose is not available."); + return; + } try { // Get current path points @@ -183,7 +187,14 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file // accept the pending goal if the current goal is running the default BT file - initializeGoalPoses(bt_action_server_->acceptPendingGoal()); + if (!initializeGoalPoses(bt_action_server_->acceptPendingGoal())) + { + RCLCPP_WARN( + logger_, + "Preemption request was rejected since the goal poses could not be " + "transformed. For now, continuing to track the last goal until completion."); + bt_action_server_->terminatePendingGoal(); + } } else { RCLCPP_WARN( logger_, diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 9f603b47b82..1dd18cd0a9f 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -112,10 +112,14 @@ NavigateToPoseNavigator::onLoop() auto feedback_msg = std::make_shared(); geometry_msgs::msg::PoseStamped current_pose; - nav2_util::getCurrentPose( - current_pose, *feedback_utils_.tf, - feedback_utils_.global_frame, feedback_utils_.robot_frame, - feedback_utils_.transform_tolerance); + if (!nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance)) + { + RCLCPP_ERROR(logger_, "Robot pose is not available."); + return; + } auto blackboard = bt_action_server_->getBlackboard(); @@ -185,7 +189,14 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file // accept the pending goal if the current goal is running the default BT file - initializeGoalPose(bt_action_server_->acceptPendingGoal()); + if (!initializeGoalPose(bt_action_server_->acceptPendingGoal())) + { + RCLCPP_WARN( + logger_, + "Preemption request was rejected since the goal pose could not be " + "transformed. For now, continuing to track the last goal until completion."); + bt_action_server_->terminatePendingGoal(); + } } else { RCLCPP_WARN( logger_, From 6814353b2f48dc67df5ac85c3ea12140290aa991 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Wed, 15 Nov 2023 11:38:44 +0100 Subject: [PATCH 4/5] Make uncrustify happy using cuddle braces for single line if statements... --- nav2_bt_navigator/src/navigators/navigate_through_poses.cpp | 3 +-- nav2_bt_navigator/src/navigators/navigate_to_pose.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 22f3e49f213..348ca0860fe 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -187,8 +187,7 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file // accept the pending goal if the current goal is running the default BT file - if (!initializeGoalPoses(bt_action_server_->acceptPendingGoal())) - { + if (!initializeGoalPoses(bt_action_server_->acceptPendingGoal())) { RCLCPP_WARN( logger_, "Preemption request was rejected since the goal poses could not be " diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 1dd18cd0a9f..a8298fc36f7 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -189,8 +189,7 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file // accept the pending goal if the current goal is running the default BT file - if (!initializeGoalPose(bt_action_server_->acceptPendingGoal())) - { + if (!initializeGoalPose(bt_action_server_->acceptPendingGoal())) { RCLCPP_WARN( logger_, "Preemption request was rejected since the goal pose could not be " From 93ca51bea640c6cad37022a6ef0257db6706914f Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Wed, 15 Nov 2023 11:50:11 +0100 Subject: [PATCH 5/5] Fix removed global_frame input for the GoalReached node in the unit test --- nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp index 44986f8fdb9..1580a8d1bc0 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp @@ -45,7 +45,7 @@ class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT R"( - + )";