Skip to content

Commit

Permalink
[MPPI] Fix transformed path oscillations (ros-navigation#3443)
Browse files Browse the repository at this point in the history
* use path distance instead of euclidean for upper bound search

* rename to pruned_plan_end

* rename to pruned_plan_end

* fix tests

* do not use prune_distance to limit the search for the closest pose

---------

Co-authored-by: Guillaume Doisy <[email protected]>
  • Loading branch information
doisyg and Guillaume Doisy authored Mar 7, 2023
1 parent 395f0e1 commit 34f18d9
Showing 1 changed file with 8 additions and 8 deletions.
16 changes: 8 additions & 8 deletions nav2_mppi_controller/src/path_handler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,8 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
using nav2_util::geometry_utils::euclidean_distance;

auto begin = global_plan_.poses.begin();
auto end = global_plan_.poses.end();

// Limit the search for the closest pose up to max_robot_pose_search_dist on the path
auto closest_pose_upper_bound =
nav2_util::geometry_utils::first_after_integrated_distance(
global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_);
Expand All @@ -61,17 +61,17 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
transformed_plan.header.stamp = global_pose.header.stamp;

auto pruned_plan_end =
nav2_util::geometry_utils::first_after_integrated_distance(
closest_point, global_plan_.poses.end(), prune_distance_);

unsigned int mx, my;
// Find the furthest relevent pose on the path to consider within costmap
// bounds
// Transforming it to the costmap frame in the same loop
for (auto global_plan_pose = closest_point; global_plan_pose != end; ++global_plan_pose) {
// Distance relative to robot pose check
auto distance = euclidean_distance(global_pose, *global_plan_pose);
if (distance >= prune_distance_) {
return {transformed_plan, closest_point};
}

for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end;
++global_plan_pose)
{
// Transform from global plan frame to costmap frame
geometry_msgs::msg::PoseStamped costmap_plan_pose;
global_plan_pose->header.stamp = global_pose.header.stamp;
Expand Down

0 comments on commit 34f18d9

Please sign in to comment.