222

src/racing_control/config/racing_control.yaml

@@ -3,8 +3,10 @@ racing_control:
     # Startup
     auto_start: false
     frame_id: odom
-    use_post_qr_pose: true
+    use_post_qr_pose: false
     enable_vlm_image_relay: false
+    enable_dynamic_replanning: true
+    enable_recovery: true
     vlm_image_input_topic: /image
     vlm_image_output_topic: /vlm_image
 
@@ -13,6 +15,7 @@ racing_control:
     qr_result_topic: /qr_results
     vlm_result_topic: /vlm_result
     odom_topic: /odom_combined
+    recovery_cmd_vel_topic: /cmd_vel
     trajectory_guard_input_topic: /trajectory_guard/input_path
 
     # Nav2 actions and plugin IDs
@@ -22,16 +25,23 @@ racing_control:
     planner_id: GridBased
     controller_id: FollowPath
     goal_checker_id: ""
-    use_trajectory_guard: true
+    use_trajectory_guard: false
 
     # Timeouts and settling waits, in seconds
     navigation_timeout_sec: 120.0
     path_planning_timeout_sec: 30.0
     circle_timeout_sec: 120.0
-    qr_result_timeout_sec: 8.0
+    qr_result_timeout_sec: 20.0
     profile_switch_wait_sec: 1.0
     post_qr_wait_sec: 1.0
     vlm_capture_wait_sec: 0.5
+    dynamic_replan_interval_sec: 1.0
+    dynamic_replan_stop_distance: 0.5
+    dynamic_replan_max_consecutive_failures: 3
+    recovery_backup_speed: -0.2
+    recovery_backup_distance: 0.04
+    recovery_backup_timeout_sec: 0.2
+    max_recovery_attempts: 2
     circle_goal_tolerance: 0.30
 
     # VLM capture:
@@ -48,61 +58,36 @@ racing_control:
     sign_profile_task2: 11
 
     # Pose parameters are flat x/y/yaw-radians triples in frame_id.
-    qr_pose: [4.3860322643582883, 1.2934897914487595, 0.94658891138326606]
-    post_qr_pose: [4.4004663023808863, 0.46113350031559491, 1.7514152174101529]
-    entry_pose: [2.499999919243812, 2.1932039710724882, 1.5174118916273021]
+    qr_pose: [4.333107888975101, 1.028867429995691, 1.1383894869544392]
+    post_qr_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
+    entry_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
 
     # The route's Nth waypoint is used as the VLM capture point.
-    # With the saved JSON files, the 4th point is goal_011 for main_1
-    # and goal_004 for main_2.
-    vlm_waypoint_number: 4
+    # For the current point set, the 2nd waypoint is the VLM point.
+    vlm_waypoint_number: 2
 
     # main_1.json: clockwise route, excluding home.
     clockwise_waypoints:
-      - 1.542550031688728
-      - 3.1698990676859604
-      - 3.1415926535897918
-      - 0.71981664792046363
-      - 3.7665012349236271
-      - 1.5495230091246759
-      - 1.3164186536457545
-      - 4.3294241954584018
-      - -0.016392199787162172
-      - 3.659525047016253
-      - 4.324612741775951
-      - 4.6883147711881255e-16
-      - 4.2320702688664218
-      - 3.7761238192637752
-      - -1.5784882007253702
-      - 3.510374505206836
-      - 3.1650879370282627
-      - -3.0916338672023889
-      - 2.5288679952890067
-      - 2.4578258883665218
-      - -1.6718998381125814
-    clockwise_home_pose: [0.51774173072785878, 0.17726629320698351, -2.2032146659725105]
+      - 0.7534956931109804
+      - 3.766501234923627
+      - 1.5592359900104606
+      - 3.8567885105264077
+      - 4.329424195458402
+      - 0.03997856199797794
+      - 2.5144339572664096
+      - 2.4337692660037766
+      - -1.6101462328054222
+    clockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]
 
     # main_2.json: counterclockwise route, excluding home.
     counterclockwise_waypoints:
-      - 3.3612239633974195
-      - 3.1747105213684104
-      - 0.010869470292991894
-      - 4.2272591382087246
-      - 3.7280108975630366
-      - 1.5707963267948966
-      - 3.4237709231207547
-      - 4.3486693641386971
-      - 3.123412893984987
-      - 1.2731168626027138
-      - 4.334235487628475
-      - -3.1165979438417697
-      - 0.74868440094090649
-      - 3.7376334819031838
-      - -1.5593026199905524
-      - 1.3933994898793114
-      - 3.1795219750508603
-      - -0.01136349401017399
-      - 2.4663210355656715
-      - 2.4145240973234814
-      - -1.5556465256578393
-    counterclockwise_home_pose: [0.51774173072785878, 0.15802112452668779, -2.1396781415005068]
+      - 4.2128251001861265
+      - 3.7376334819031842
+      - 1.5450283923222128
+      - 1.1720794040064113
+      - 4.319801449605877
+      - 3.1112984779040144
+      - 2.4951887885861144
+      - 2.318297930897253
+      - -1.539556591616333
+    counterclockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]