diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml index 62ab233..d905983 100644 --- a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml @@ -102,7 +102,7 @@ controller_server: GoalCritic: enabled: true cost_power: 1 - cost_weight: 6.0 + cost_weight: 5.0 threshold_to_consider: 1.4 GoalAngleCritic: enabled: true @@ -126,7 +126,7 @@ controller_server: PathAlignCritic: enabled: true cost_power: 1 - cost_weight: 8.0 + cost_weight: 10.0 max_path_occupancy_ratio: 0.05 trajectory_point_step: 4 threshold_to_consider: 0.5 @@ -224,8 +224,8 @@ global_costmap: extra_inflation: 0.02 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 2.0 - inflation_radius: 0.35 + cost_scaling_factor: 3.0 + inflation_radius: 0.30 always_send_full_costmap: True global_costmap_client: ros__parameters: @@ -252,10 +252,10 @@ planner_server: analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 minimum_turning_radius: 0.40 - reverse_penalty: 1.6 - change_penalty: 2.0 + reverse_penalty: 2.0 + change_penalty: 1.0 non_straight_penalty: 1.2 - cost_penalty: 4.0 + cost_penalty: 3.0 retrospective_penalty: 0.015 # 5 m covers the rolling planning horizon without the startup and memory # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. diff --git a/src/racing_control/config/racing_control.yaml b/src/racing_control/config/racing_control.yaml index 2cb63e5..cc2648b 100644 --- a/src/racing_control/config/racing_control.yaml +++ b/src/racing_control/config/racing_control.yaml @@ -5,7 +5,7 @@ racing_control: frame_id: odom use_post_qr_pose: false enable_vlm_image_relay: false - enable_dynamic_replanning: true + enable_dynamic_replanning: false enable_recovery: true vlm_image_input_topic: /image vlm_image_output_topic: /vlm_image diff --git a/src/racing_control/include/racing_control/racing_control.hpp b/src/racing_control/include/racing_control/racing_control.hpp index 95d7f8a..2a5c4ff 100644 --- a/src/racing_control/include/racing_control/racing_control.hpp +++ b/src/racing_control/include/racing_control/racing_control.hpp @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -153,6 +154,38 @@ inline geometry_msgs::msg::PoseStamped poseFromXYYaw( return pose; } +inline double poseDistance2d( + const geometry_msgs::msg::PoseStamped & a, + const geometry_msgs::msg::PoseStamped & b) +{ + const auto dx = a.pose.position.x - b.pose.position.x; + const auto dy = a.pose.position.y - b.pose.position.y; + return std::hypot(dx, dy); +} + +inline std::size_t advanceReachedWaypointIndex( + const std::vector & waypoints, + const geometry_msgs::msg::PoseStamped & current, + const std::size_t next_waypoint_index, + const double tolerance) +{ + auto next = next_waypoint_index; + while (next < waypoints.size() && poseDistance2d(current, waypoints[next]) <= tolerance) { + ++next; + } + return next; +} + +inline std::vector remainingWaypoints( + const std::vector & waypoints, + const std::size_t next_waypoint_index) +{ + if (next_waypoint_index >= waypoints.size()) { + return {}; + } + return {waypoints.begin() + static_cast(next_waypoint_index), waypoints.end()}; +} + inline std::vector posesFromFlatDoubles( const std::vector & values, const std::string & frame_id) { diff --git a/src/racing_control/src/racing_control.cpp b/src/racing_control/src/racing_control.cpp index 257b264..7838047 100644 --- a/src/racing_control/src/racing_control.cpp +++ b/src/racing_control/src/racing_control.cpp @@ -616,6 +616,7 @@ private: if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) { active_segment_ = RouteSegment::FullRoute; active_segment_waypoints_ = route.waypoints; + active_segment_next_waypoint_index_ = 0; runRouteSegmentPlanning("full route with pass-through VLM capture"); return; } @@ -624,6 +625,7 @@ private: active_segment_waypoints_.assign( route.waypoints.begin(), route.waypoints.begin() + vlm_index + 1); + active_segment_next_waypoint_index_ = 0; runRouteSegmentPlanning("to VLM waypoint"); } @@ -640,6 +642,7 @@ private: active_segment_waypoints_.assign( route.waypoints.begin() + vlm_index + 1, route.waypoints.end()); + active_segment_next_waypoint_index_ = 0; runRouteSegmentPlanning("after VLM waypoint"); } @@ -809,6 +812,10 @@ private: if (!current) { return; } + updateActiveSegmentWaypointProgress(*current); + if (active_segment_next_waypoint_index_ >= active_segment_waypoints_.size()) { + return; + } const auto distance_to_goal = distance2d(*current, active_segment_waypoints_.back()); const auto elapsed = (now() - last_dynamic_replan_time_).seconds(); if (!shouldDynamicReplan( @@ -830,8 +837,15 @@ private: return; } + const auto replan_goals = remainingWaypoints( + active_segment_waypoints_, active_segment_next_waypoint_index_); + if (replan_goals.empty()) { + dynamic_replan_in_flight_ = false; + return; + } + ComputePathThroughPoses::Goal goal; - goal.goals = stampPoses({active_segment_waypoints_.back()}); + goal.goals = stampPoses(replan_goals); goal.planner_id = planner_id_; goal.use_start = false; @@ -864,6 +878,22 @@ private: }; compute_path_client_->async_send_goal(goal, options); + RCLCPP_INFO( + get_logger(), "dynamic route replanning through %zu remaining waypoint(s), next index=%zu", + replan_goals.size(), active_segment_next_waypoint_index_); + } + + void updateActiveSegmentWaypointProgress(const geometry_msgs::msg::PoseStamped & current) + { + const auto previous_index = active_segment_next_waypoint_index_; + active_segment_next_waypoint_index_ = advanceReachedWaypointIndex( + active_segment_waypoints_, current, active_segment_next_waypoint_index_, + circle_goal_tolerance_); + if (active_segment_next_waypoint_index_ != previous_index) { + RCLCPP_INFO( + get_logger(), "route waypoint progress advanced: next index %zu -> %zu", + previous_index, active_segment_next_waypoint_index_); + } } void onDynamicReplanFailed(const std::string & reason) @@ -1293,6 +1323,7 @@ private: VlmCaptureMode vlm_capture_mode_{VlmCaptureMode::Stop}; RouteSegment active_segment_{RouteSegment::None}; std::vector active_segment_waypoints_; + std::size_t active_segment_next_waypoint_index_{0}; nav_msgs::msg::Path active_path_; bool vlm_capture_triggered_{false}; std::optional recovery_start_pose_; diff --git a/src/racing_control/test/test_racing_control_helpers.cpp b/src/racing_control/test/test_racing_control_helpers.cpp index 543aa2d..fca25c2 100644 --- a/src/racing_control/test/test_racing_control_helpers.cpp +++ b/src/racing_control/test/test_racing_control_helpers.cpp @@ -132,6 +132,37 @@ TEST(RacingControlHelpers, DynamicReplanningStopsNearGoal) EXPECT_FALSE(racing_control::shouldDynamicReplan(false, false, 0.6, 0.5, 1.0, 1.0)); } +TEST(RacingControlHelpers, AdvancesNextWaypointOnlyAfterItIsReached) +{ + const auto waypoints = racing_control::posesFromFlatDoubles( + {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0}, + "map"); + + EXPECT_EQ( + racing_control::advanceReachedWaypointIndex( + waypoints, racing_control::poseFromXYYaw(0.65, 0.0, 0.0, "map"), 1, 0.30), + 1U); + EXPECT_EQ( + racing_control::advanceReachedWaypointIndex( + waypoints, racing_control::poseFromXYYaw(1.05, 0.0, 0.0, "map"), 1, 0.30), + 2U); +} + +TEST(RacingControlHelpers, DynamicReplanningKeepsRemainingUnreachedWaypoints) +{ + const auto waypoints = racing_control::posesFromFlatDoubles( + {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0}, + "map"); + + const auto remaining = racing_control::remainingWaypoints(waypoints, 1); + + ASSERT_EQ(remaining.size(), 2U); + EXPECT_DOUBLE_EQ(remaining[0].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(remaining[1].pose.position.x, 2.0); + EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size()).empty()); + EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size() + 1).empty()); +} + TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout) { EXPECT_TRUE(racing_control::recoveryBackupComplete(0.04, 0.04, 0.1, 0.2));