Skip to content

Commit edfe5bf

Browse files
meyerjmeyerj
authored
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 #3894, at least partially. * Address review comments (#3917 (review)) * Address more review comments (#3917 (review)) * Make uncrustify happy using cuddle braces for single line if statements... * Fix removed global_frame input for the GoalReached node in the unit test
1 parent c5eddb3 commit edfe5bf

File tree

9 files changed

+89
-41
lines changed

9 files changed

+89
-41
lines changed

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ class RemovePassedGoals : public BT::ActionNodeBase
5555
double viapoint_achieved_radius_;
5656
double transform_tolerance_;
5757
std::shared_ptr<tf2_ros::Buffer> tf_;
58-
std::string global_frame_, robot_base_frame_;
58+
std::string robot_base_frame_;
5959
};
6060

6161
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,7 @@ class GoalReachedCondition : public BT::ConditionNode
9292
bool initialized_;
9393
double goal_reached_tol_;
9494
double transform_tolerance_;
95-
std::string global_frame_, robot_base_frame_;
95+
std::string robot_base_frame_;
9696
};
9797

9898
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,6 @@ RemovePassedGoals::RemovePassedGoals(
3737
auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
3838
node->get_parameter("transform_tolerance", transform_tolerance_);
3939

40-
global_frame_ = BT::deconflictPortAndParamFrame<std::string, RemovePassedGoals>(
41-
node, "global_frame", this);
4240
robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, RemovePassedGoals>(
4341
node, "robot_base_frame", this);
4442
}
@@ -59,7 +57,7 @@ inline BT::NodeStatus RemovePassedGoals::tick()
5957

6058
geometry_msgs::msg::PoseStamped current_pose;
6159
if (!nav2_util::getCurrentPose(
62-
current_pose, *tf_, global_frame_, robot_base_frame_,
60+
current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_,
6361
transform_tolerance_))
6462
{
6563
return BT::NodeStatus::FAILURE;

‎nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -33,8 +33,6 @@ GoalReachedCondition::GoalReachedCondition(
3333
{
3434
auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
3535

36-
global_frame_ = BT::deconflictPortAndParamFrame<std::string, GoalReachedCondition>(
37-
node, "global_frame", this);
3836
robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, GoalReachedCondition>(
3937
node, "robot_base_frame", this);
4038
}
@@ -73,17 +71,17 @@ void GoalReachedCondition::initialize()
7371

7472
bool GoalReachedCondition::isGoalReached()
7573
{
76-
geometry_msgs::msg::PoseStamped current_pose;
74+
geometry_msgs::msg::PoseStamped goal;
75+
getInput("goal", goal);
7776

77+
geometry_msgs::msg::PoseStamped current_pose;
7878
if (!nav2_util::getCurrentPose(
79-
current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_))
79+
current_pose, *tf_, goal.header.frame_id, robot_base_frame_, transform_tolerance_))
8080
{
8181
RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
8282
return false;
8383
}
8484

85-
geometry_msgs::msg::PoseStamped goal;
86-
getInput("goal", goal);
8785
double dx = goal.pose.position.x - current_pose.pose.position.x;
8886
double dy = goal.pose.position.y - current_pose.pose.position.y;
8987

‎nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT
4545
R"(
4646
<root main_tree_to_execute = "MainTree" >
4747
<BehaviorTree ID="MainTree">
48-
<GoalReached goal="{goal}" global_frame="map" robot_base_frame="base_link"/>
48+
<GoalReached goal="{goal}" robot_base_frame="base_link"/>
4949
</BehaviorTree>
5050
</root>)";
5151

‎nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -104,8 +104,9 @@ class NavigateThroughPosesNavigator
104104

105105
/**
106106
* @brief Goal pose initialization on the blackboard
107+
* @return bool if goal was initialized successfully to be processed
107108
*/
108-
void initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal);
109+
bool initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal);
109110

110111
rclcpp::Time start_time_;
111112
std::string goals_blackboard_id_;

‎nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -116,8 +116,9 @@ class NavigateToPoseNavigator
116116
/**
117117
* @brief Goal pose initialization on the blackboard
118118
* @param goal Action template's goal message to process
119+
* @return bool if goal was initialized successfully to be processed
119120
*/
120-
void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal);
121+
bool initializeGoalPose(ActionT::Goal::ConstSharedPtr goal);
121122

122123
rclcpp::Time start_time_;
123124

‎nav2_bt_navigator/src/navigators/navigate_through_poses.cpp‎

Lines changed: 36 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -81,9 +81,7 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
8181
return false;
8282
}
8383

84-
initializeGoalPoses(goal);
85-
86-
return true;
84+
return initializeGoalPoses(goal);
8785
}
8886

8987
void
@@ -113,10 +111,14 @@ NavigateThroughPosesNavigator::onLoop()
113111
}
114112

115113
geometry_msgs::msg::PoseStamped current_pose;
116-
nav2_util::getCurrentPose(
117-
current_pose, *feedback_utils_.tf,
118-
feedback_utils_.global_frame, feedback_utils_.robot_frame,
119-
feedback_utils_.transform_tolerance);
114+
if (!nav2_util::getCurrentPose(
115+
current_pose, *feedback_utils_.tf,
116+
feedback_utils_.global_frame, feedback_utils_.robot_frame,
117+
feedback_utils_.transform_tolerance))
118+
{
119+
RCLCPP_ERROR(logger_, "Robot pose is not available.");
120+
return;
121+
}
120122

121123
try {
122124
// Get current path points
@@ -185,7 +187,13 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
185187
// if pending goal requests the same BT as the current goal, accept the pending goal
186188
// if pending goal has an empty behavior_tree field, it requests the default BT file
187189
// accept the pending goal if the current goal is running the default BT file
188-
initializeGoalPoses(bt_action_server_->acceptPendingGoal());
190+
if (!initializeGoalPoses(bt_action_server_->acceptPendingGoal())) {
191+
RCLCPP_WARN(
192+
logger_,
193+
"Preemption request was rejected since the goal poses could not be "
194+
"transformed. For now, continuing to track the last goal until completion.");
195+
bt_action_server_->terminatePendingGoal();
196+
}
189197
} else {
190198
RCLCPP_WARN(
191199
logger_,
@@ -198,13 +206,27 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
198206
}
199207
}
200208

201-
void
209+
bool
202210
NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal)
203211
{
204-
if (goal->poses.size() > 0) {
212+
Goals goal_poses = goal->poses;
213+
for (auto & goal_pose : goal_poses) {
214+
if (!nav2_util::transformPoseInTargetFrame(
215+
goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
216+
feedback_utils_.transform_tolerance))
217+
{
218+
RCLCPP_ERROR(
219+
logger_,
220+
"Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.",
221+
goal_pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str());
222+
return false;
223+
}
224+
}
225+
226+
if (goal_poses.size() > 0) {
205227
RCLCPP_INFO(
206228
logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)",
207-
goal->poses.size(), goal->poses.back().pose.position.x, goal->poses.back().pose.position.y);
229+
goal_poses.size(), goal_poses.back().pose.position.x, goal_poses.back().pose.position.y);
208230
}
209231

210232
// Reset state for new action feedback
@@ -213,7 +235,9 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
213235
blackboard->set<int>("number_recoveries", 0); // NOLINT
214236

215237
// Update the goal pose on the blackboard
216-
blackboard->set<Goals>(goals_blackboard_id_, goal->poses);
238+
blackboard->set<Goals>(goals_blackboard_id_, std::move(goal_poses));
239+
240+
return true;
217241
}
218242

219243
} // namespace nav2_bt_navigator

‎nav2_bt_navigator/src/navigators/navigate_to_pose.cpp‎

Lines changed: 41 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -94,9 +94,7 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
9494
return false;
9595
}
9696

97-
initializeGoalPose(goal);
98-
99-
return true;
97+
return initializeGoalPose(goal);
10098
}
10199

102100
void
@@ -114,10 +112,14 @@ NavigateToPoseNavigator::onLoop()
114112
auto feedback_msg = std::make_shared<ActionT::Feedback>();
115113

116114
geometry_msgs::msg::PoseStamped current_pose;
117-
nav2_util::getCurrentPose(
118-
current_pose, *feedback_utils_.tf,
119-
feedback_utils_.global_frame, feedback_utils_.robot_frame,
120-
feedback_utils_.transform_tolerance);
115+
if (!nav2_util::getCurrentPose(
116+
current_pose, *feedback_utils_.tf,
117+
feedback_utils_.global_frame, feedback_utils_.robot_frame,
118+
feedback_utils_.transform_tolerance))
119+
{
120+
RCLCPP_ERROR(logger_, "Robot pose is not available.");
121+
return;
122+
}
121123

122124
auto blackboard = bt_action_server_->getBlackboard();
123125

@@ -187,7 +189,13 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
187189
// if pending goal requests the same BT as the current goal, accept the pending goal
188190
// if pending goal has an empty behavior_tree field, it requests the default BT file
189191
// accept the pending goal if the current goal is running the default BT file
190-
initializeGoalPose(bt_action_server_->acceptPendingGoal());
192+
if (!initializeGoalPose(bt_action_server_->acceptPendingGoal())) {
193+
RCLCPP_WARN(
194+
logger_,
195+
"Preemption request was rejected since the goal pose could not be "
196+
"transformed. For now, continuing to track the last goal until completion.");
197+
bt_action_server_->terminatePendingGoal();
198+
}
191199
} else {
192200
RCLCPP_WARN(
193201
logger_,
@@ -200,27 +208,45 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
200208
}
201209
}
202210

203-
void
211+
bool
204212
NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
205213
{
206214
geometry_msgs::msg::PoseStamped current_pose;
207-
nav2_util::getCurrentPose(
208-
current_pose, *feedback_utils_.tf,
209-
feedback_utils_.global_frame, feedback_utils_.robot_frame,
210-
feedback_utils_.transform_tolerance);
215+
if (!nav2_util::getCurrentPose(
216+
current_pose, *feedback_utils_.tf,
217+
feedback_utils_.global_frame, feedback_utils_.robot_frame,
218+
feedback_utils_.transform_tolerance))
219+
{
220+
RCLCPP_ERROR(logger_, "Initial robot pose is not available.");
221+
return false;
222+
}
223+
224+
geometry_msgs::msg::PoseStamped goal_pose;
225+
if (!nav2_util::transformPoseInTargetFrame(
226+
goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
227+
feedback_utils_.transform_tolerance))
228+
{
229+
RCLCPP_ERROR(
230+
logger_,
231+
"Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.",
232+
goal->pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str());
233+
return false;
234+
}
211235

212236
RCLCPP_INFO(
213237
logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)",
214238
current_pose.pose.position.x, current_pose.pose.position.y,
215-
goal->pose.pose.position.x, goal->pose.pose.position.y);
239+
goal_pose.pose.position.x, goal_pose.pose.position.y);
216240

217241
// Reset state for new action feedback
218242
start_time_ = clock_->now();
219243
auto blackboard = bt_action_server_->getBlackboard();
220244
blackboard->set<int>("number_recoveries", 0); // NOLINT
221245

222246
// Update the goal pose on the blackboard
223-
blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose);
247+
blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal_pose);
248+
249+
return true;
224250
}
225251

226252
void

0 commit comments

Comments
 (0)