From b31bd4c255eb5529c0f4934192b0cac533cdb543 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 2 Mar 2023 18:28:49 +0000 Subject: [PATCH 1/5] use path distance instead of euclidean for upper bound search --- nav2_mppi_controller/src/path_handler.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 83ec415e17..b7e819d3e2 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -44,11 +44,12 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( using nav2_util::geometry_utils::euclidean_distance; auto begin = global_plan_.poses.begin(); - auto end = global_plan_.poses.end(); + // Don't search further away than the prune distance 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_); + global_plan_.poses.begin(), global_plan_.poses.end(), + std::min(max_robot_pose_search_dist_, prune_distance_)); // Find closest point to the robot auto closest_point = nav2_util::geometry_utils::min_by( @@ -61,17 +62,17 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); transformed_plan.header.stamp = global_pose.header.stamp; + auto pose_above_prune_distance = + 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 != pose_above_prune_distance; + ++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; From eddd8e5ca5fb34974c60ae3c2c242c8f48b22fab Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 6 Mar 2023 10:27:18 +0000 Subject: [PATCH 2/5] rename to pruned_plan_end --- nav2_mppi_controller/src/path_handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index b7e819d3e2..bd3a56407e 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -62,7 +62,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); transformed_plan.header.stamp = global_pose.header.stamp; - auto pose_above_prune_distance = + auto pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( closest_point, global_plan_.poses.end(), prune_distance_); @@ -70,7 +70,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( // 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 != pose_above_prune_distance; + for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; ++global_plan_pose) { // Transform from global plan frame to costmap frame From 8359f66d7e6a09fcf2125ed643c349edfbfc526f Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 6 Mar 2023 10:27:18 +0000 Subject: [PATCH 3/5] rename to pruned_plan_end From 63f31d16f63c08790a05f60fa23fad70762ce768 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 6 Mar 2023 13:28:31 +0000 Subject: [PATCH 4/5] fix tests --- nav2_mppi_controller/src/path_handler.cpp | 2 +- nav2_mppi_controller/test/path_handler_test.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index bd3a56407e..783063a2e6 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -49,7 +49,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( global_plan_.poses.begin(), global_plan_.poses.end(), - std::min(max_robot_pose_search_dist_, prune_distance_)); + std::min(max_robot_pose_search_dist_, prune_distance_)); // Find closest point to the robot auto closest_point = nav2_util::geometry_utils::min_by( diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 3eb737ed36..c9bbc05913 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -92,6 +92,7 @@ TEST(PathHandlerTests, TestBounds) PathHandlerWrapper handler; auto node = std::make_shared("my_node"); node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); + node->declare_parameter("dummy.prune_distance", rclcpp::ParameterValue(99999.9)); auto costmap_ros = std::make_shared( "dummy_costmap", "", "dummy_costmap", true); auto results = costmap_ros->set_parameters_atomically( From 92c2087f25c02e939697e5f90e89b9112e5bd473 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 7 Mar 2023 11:00:46 +0000 Subject: [PATCH 5/5] do not use prune_distance to limit the search for the closest pose --- nav2_mppi_controller/src/path_handler.cpp | 5 ++--- nav2_mppi_controller/test/path_handler_test.cpp | 1 - 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 783063a2e6..aa872321d6 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -45,11 +45,10 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( auto begin = global_plan_.poses.begin(); - // Don't search further away than the prune distance + // 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(), - std::min(max_robot_pose_search_dist_, prune_distance_)); + global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); // Find closest point to the robot auto closest_point = nav2_util::geometry_utils::min_by( diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index c9bbc05913..3eb737ed36 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -92,7 +92,6 @@ TEST(PathHandlerTests, TestBounds) PathHandlerWrapper handler; auto node = std::make_shared("my_node"); node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); - node->declare_parameter("dummy.prune_distance", rclcpp::ParameterValue(99999.9)); auto costmap_ros = std::make_shared( "dummy_costmap", "", "dummy_costmap", true); auto results = costmap_ros->set_parameters_atomically(