File tree Expand file tree Collapse file tree
easynav_gridmap_rrtstar_planner/src/easynav_gridmap_rrtstar_planner Expand file tree Collapse file tree Original file line number Diff line number Diff line change @@ -198,7 +198,7 @@ std::vector<geometry_msgs::msg::Pose> GridMapRRTStarPlanner::rrt_star(
198198 for (int iter = 0 ; iter < max_iters_; ++iter) {
199199 // adaptive goal bias: increase bias if a candidate path exists, else grow modestly
200200 double adaptive_goal_bias = goal_bias_;
201-
201+
202202 if (best_goal_node) {
203203 adaptive_goal_bias = std::min (0.9 , goal_bias_ + 0.15 * (iter / 100.0 ));
204204 } else {
@@ -441,7 +441,7 @@ double GridMapRRTStarPlanner::distance(
441441 map.getPosition (a, pa);
442442 map.getPosition (b, pb);
443443
444- return std::hypot (pb.x () - pa.x (), pb.y () - pa.y ());
444+ return std::hypot (pb.x () - pa.x (), pb.y () - pa.y ());
445445}
446446
447447// Replace previous implementation: trim path by removing waypoints behind or already passed by the robot
@@ -1043,9 +1043,9 @@ std::vector<geometry_msgs::msg::Pose> GridMapRRTStarPlanner::smooth_path(
10431043 for (size_t i = start_preserve; i < smoothed.size (); ++i) {
10441044 smoothed[i].orientation = tf2::toMsg (goal_orientation);
10451045 }
1046- if (!smoothed.empty ()) {
1046+ if (!smoothed.empty ()) {
10471047 smoothed.back ().orientation = tf2::toMsg (goal_orientation);
1048- }
1048+ }
10491049 RCLCPP_DEBUG (get_node ()->get_logger (),
10501050 " Smoothed path: %zu -> %zu points (preserved %zu final orientations)" ,
10511051 input_path.size (), smoothed.size (), preserve_n);
You can’t perform that action at this time.
0 commit comments