From edfe5bf6bd785f92061f880535512cddc67da5ae Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Wed, 15 Nov 2023 19:14:54 +0100 Subject: [PATCH] Enable goals to be sent in other frames than the pre-configured global_frame (#3917) * 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. * Address review comments (https://github.com/ros-planning/navigation2/pull/3917#pullrequestreview-1708583128) * Address more review comments (https://github.com/ros-planning/navigation2/pull/3917#pullrequestreview-1730891741) * Make uncrustify happy using cuddle braces for single line if statements... * Fix removed global_frame input for the GoalReached node in the unit test --- .../action/remove_passed_goals_action.hpp | 2 +- .../condition/goal_reached_condition.hpp | 2 +- .../action/remove_passed_goals_action.cpp | 4 +- .../condition/goal_reached_condition.cpp | 10 ++-- .../plugins/condition/test_goal_reached.cpp | 2 +- .../navigators/navigate_through_poses.hpp | 3 +- .../navigators/navigate_to_pose.hpp | 3 +- .../src/navigators/navigate_through_poses.cpp | 48 ++++++++++++---- .../src/navigators/navigate_to_pose.cpp | 56 ++++++++++++++----- 9 files changed, 89 insertions(+), 41 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 184bf1836c..fcc02258a7 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 674a8887b4..acbca8c960 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 fc768a3ab9..7a5816973d 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); } @@ -59,7 +57,7 @@ inline BT::NodeStatus RemovePassedGoals::tick() geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( - current_pose, *tf_, global_frame_, robot_base_frame_, + current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_, transform_tolerance_)) { return BT::NodeStatus::FAILURE; diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e3c5292886..d81df60a9e 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); } @@ -73,17 +71,17 @@ void GoalReachedCondition::initialize() bool GoalReachedCondition::isGoalReached() { - geometry_msgs::msg::PoseStamped current_pose; + geometry_msgs::msg::PoseStamped goal; + getInput("goal", goal); + 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_behavior_tree/test/plugins/condition/test_goal_reached.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp index 44986f8fdb..1580a8d1bc 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"( - + )"; 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 615ce5ef64..aee07de14c 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 4b690bb296..447d02cc44 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 92f53f9919..348ca0860f 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 @@ -113,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 @@ -185,7 +187,13 @@ 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_, @@ -198,13 +206,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 +235,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 cbd686cc04..a8298fc36f 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 @@ -114,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(); @@ -187,7 +189,13 @@ 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_, @@ -200,19 +208,35 @@ 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)) + { + 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; + } 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 +244,9 @@ 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); + + return true; } void