Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Enable goals to be sent in other frames than the pre-configured global_frame #3917

Merged
merged 6 commits into from
Nov 15, 2023
Merged

Enable goals to be sent in other frames than the pre-configured global_frame #3917

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
19a8165
nav2_behavior_tree/nav2_bt_navigator: enable goals to be sent in othe…
meyerj Oct 25, 2023
a326468
Address review comments (https://github.com/ros-planning/navigation2/…
meyerj Nov 14, 2023
3e64c33
Address more review comments (https://github.com/ros-planning/navigat…
meyerj Nov 15, 2023
2573131
Merge branch 'main' of https://github.com/ros-planning/navigation2 in…
meyerj Nov 15, 2023
6814353
Make uncrustify happy using cuddle braces for single line if statemen…
meyerj Nov 15, 2023
93ca51b
Fix removed global_frame input for the GoalReached node in the unit test
meyerj Nov 15, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 5 additions & 4 deletions nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved

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;

Expand Down
15 changes: 13 additions & 2 deletions nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -209,18 +209,29 @@ 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(
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
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");
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
}

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();
auto blackboard = bt_action_server_->getBlackboard();
blackboard->set<int>("number_recoveries", 0); // NOLINT

// Update the goal pose on the blackboard
blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose);
blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal_pose);
}

void
Expand Down