Skip to content

Fix bt navigator feedback in looping path #5864

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

Merged
SteveMacenski merged 12 commits into from
Jan 15, 2026
Merged

Fix bt navigator feedback in looping path #5864

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
ed2c2b3
Fix bt navigator feedback in looping path
mini-1235 Jan 14, 2026
69a9013
Revert
mini-1235 Jan 14, 2026
ae75f66
Revert
mini-1235 Jan 14, 2026
f1797fc
Lint
mini-1235 Jan 14, 2026
934d99e
Update nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
mini-1235 Jan 15, 2026
81884b2
Update nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
mini-1235 Jan 15, 2026
d613c4c
Add tracking error to Navigators feedback
mini-1235 Jan 15, 2026
95b1239
Lint
mini-1235 Jan 15, 2026
1c4cb61
Lint
mini-1235 Jan 15, 2026
72a65b3
Add search window to bringup
mini-1235 Jan 15, 2026
28bab96
Add test
mini-1235 Jan 15, 2026
28607de
Lint
mini-1235 Jan 15, 2026
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
3 changes: 3 additions & 0 deletions nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,9 @@ class NavigateThroughPosesNavigator

// Odometry smoother object
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
size_t start_index_ = 0;
nav_msgs::msg::Path previous_path_;
double search_window_;
};

} // namespace nav2_bt_navigator
Expand Down
3 changes: 3 additions & 0 deletions nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,9 @@ class NavigateToPoseNavigator

// Odometry smoother object
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
size_t start_index_ = 0;
nav_msgs::msg::Path previous_path_;
double search_window_;
};

} // namespace nav2_bt_navigator
Expand Down
30 changes: 15 additions & 15 deletions nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <limits>
#include <stdexcept>
#include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
#include "nav2_util/path_utils.hpp"

namespace nav2_bt_navigator
{
Expand All @@ -39,6 +40,7 @@ NavigateThroughPosesNavigator::configure(
node->declare_or_get_parameter(
getName() + ".waypoint_statuses_blackboard_id",
std::string("waypoint_statuses"));
search_window_ = node->declare_or_get_parameter(getName() + "search_window", 2.0);

// Odometry smoother object for getting current speed
odom_smoother_ = odom_smoother;
Expand Down Expand Up @@ -157,25 +159,21 @@ NavigateThroughPosesNavigator::onLoop()
nav_msgs::msg::Path current_path;
res = blackboard->get(path_blackboard_id_, current_path);
if (res && current_path.poses.size() > 0u) {
// Reset start index if path is updated
if (nav2_util::isPathUpdated(current_path,
previous_path_) || previous_path_.poses.size() == 0u)
{
start_index_ = 0;
previous_path_ = current_path;
}
// Find the closest pose to current pose on global path
auto find_closest_pose_idx =
[&current_pose, &current_path]() {
size_t closest_pose_idx = 0;
double curr_min_dist = std::numeric_limits<double>::max();
for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
double curr_dist = nav2_util::geometry_utils::euclidean_distance(
current_pose, current_path.poses[curr_idx]);
if (curr_dist < curr_min_dist) {
curr_min_dist = curr_dist;
closest_pose_idx = curr_idx;
}
}
return closest_pose_idx;
};
const auto path_search_result = nav2_util::distance_from_path(
current_path, current_pose.pose, start_index_, search_window_);

// Calculate distance on the path
start_index_ = path_search_result.closest_segment_index;
double distance_remaining =
nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx());
nav2_util::geometry_utils::calculate_path_length(current_path, start_index_);

// Default value for time remaining
rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0);
Expand Down Expand Up @@ -279,6 +277,8 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
start_time_ = clock_->now();
auto blackboard = bt_action_server_->getBlackboard();
blackboard->set("number_recoveries", 0); // NOLINT
nav_msgs::msg::Path empty_path;
previous_path_ = empty_path;
Comment thread
mini-1235 marked this conversation as resolved.
Outdated

// Update the goal pose on the blackboard
blackboard->set<nav_msgs::msg::Goals>(
Expand Down
30 changes: 15 additions & 15 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 @@ -17,6 +17,7 @@
#include <memory>
#include <limits>
#include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
#include "nav2_util/path_utils.hpp"

namespace nav2_bt_navigator
{
Expand All @@ -35,6 +36,7 @@ NavigateToPoseNavigator::configure(
path_blackboard_id_ = node->declare_or_get_parameter(
getName() + ".path_blackboard_id",
std::string("path"));
search_window_ = node->declare_or_get_parameter(getName() + "search_window", 2.0);

// Odometry smoother object for getting current speed
odom_smoother_ = odom_smoother;
Expand Down Expand Up @@ -138,25 +140,21 @@ NavigateToPoseNavigator::onLoop()
nav_msgs::msg::Path current_path;
auto res = blackboard->get(path_blackboard_id_, current_path);
if (res && current_path.poses.size() > 0u) {
// Reset start index if path is updated
if (nav2_util::isPathUpdated(current_path,
previous_path_) || previous_path_.poses.size() == 0u)
{
start_index_ = 0;
previous_path_ = current_path;
}
// Find the closest pose to current pose on global path
auto find_closest_pose_idx =
[&current_pose, &current_path]() {
size_t closest_pose_idx = 0;
double curr_min_dist = std::numeric_limits<double>::max();
for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
double curr_dist = nav2_util::geometry_utils::euclidean_distance(
current_pose, current_path.poses[curr_idx]);
if (curr_dist < curr_min_dist) {
curr_min_dist = curr_dist;
closest_pose_idx = curr_idx;
}
}
return closest_pose_idx;
};
const auto path_search_result = nav2_util::distance_from_path(
current_path, current_pose.pose, start_index_, search_window_);

// Calculate distance on the path
start_index_ = path_search_result.closest_segment_index;
double distance_remaining =
nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx());
nav2_util::geometry_utils::calculate_path_length(current_path, start_index_);

// Default value for time remaining
rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0);
Expand Down Expand Up @@ -255,6 +253,8 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
start_time_ = clock_->now();
auto blackboard = bt_action_server_->getBlackboard();
blackboard->set("number_recoveries", 0); // NOLINT
nav_msgs::msg::Path empty_path;
previous_path_ = empty_path;
Comment thread
mini-1235 marked this conversation as resolved.
Outdated

// Update the goal pose on the blackboard
blackboard->set(goal_blackboard_id_, goal_pose);
Expand Down
2 changes: 1 addition & 1 deletion nav2_controller/src/controller_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -744,9 +744,9 @@ void ControllerServer::computeAndPublishVelocity()
tracking_feedback_msg->robot_pose = pose;
tracking_feedback_msg->distance_to_goal = current_distance_to_goal;
tracking_feedback_msg->speed = std::hypot(twist.linear.x, twist.linear.y);
start_index_ = path_search_result.closest_segment_index;
Comment thread
SteveMacenski marked this conversation as resolved.
tracking_feedback_msg->remaining_path_length =
nav2_util::geometry_utils::calculate_path_length(current_path_, start_index_);
start_index_ = path_search_result.closest_segment_index;

// Update current tracking error and publish
current_tracking_feedback = *tracking_feedback_msg;
Expand Down
3 changes: 3 additions & 0 deletions nav2_rviz_plugins/src/nav2_panel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1084,6 +1084,9 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame)
syncTabsWithAccumulatedPoses(); // Sync tabs with new pose
} else {
acummulated_poses_ = nav_msgs::msg::Goals();
// Reset goal index for single goal navigation

@mini-1235 mini-1235 Jan 14, 2026

Copy link
Copy Markdown
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There are some issues with the rviz panel when doing single goal navigation:

  • The goal index is not reset after using waypoint follower
  • The goal is not added to store poses

However, I am not sure if this is the best place to add these

@SteveMacenski SteveMacenski Jan 15, 2026

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Got it - I default to your judgement on this

@mini-1235 mini-1235 Jan 25, 2026

Copy link
Copy Markdown
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry, it seems that this introduced a minor bug, will open a PR tomorrow

goal_index_ = 0;
store_poses_.goals.push_back(pose);
updateWpNavigationMarkers();
std::cout << "Start navigation" << std::endl;
startNavigation(pose);
Expand Down
10 changes: 10 additions & 0 deletions nav2_util/include/nav2_util/path_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,16 @@ unsigned int removePosesAfterFirstConstraint(
bool enforce_path_inversion,
float rotation_threshold);

/**
* @brief Checks if the global path is updated
* @param new_path new path to the goal
* @param old_path current path to the goal
* @return whether the path is updated for the current goal
*/
bool isPathUpdated(
nav_msgs::msg::Path & new_path,
nav_msgs::msg::Path & old_path);

} // namespace nav2_util

#endif // NAV2_UTIL__PATH_UTILS_HPP_
10 changes: 10 additions & 0 deletions nav2_util/src/path_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -219,4 +219,14 @@ unsigned int removePosesAfterFirstConstraint(
return first_after_constraint;
}

bool isPathUpdated(
Comment thread
SteveMacenski marked this conversation as resolved.

@SteveMacenski SteveMacenski Jan 15, 2026 •
edited
Loading

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add a test for this plz

Otherwise this PR looks good to me. I merged the docs as well

@mini-1235 mini-1235 Jan 15, 2026

Copy link
Copy Markdown
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Added, waiting for the ci to run again

nav_msgs::msg::Path & new_path,
nav_msgs::msg::Path & old_path)
{
return old_path.poses.size() != 0 &&
new_path.poses.size() != 0 &&
new_path.poses.size() != old_path.poses.size() &&
old_path.poses.back().pose.position == new_path.poses.back().pose.position;
}

} // namespace nav2_util
Loading