.md length

code/planning/src/local_planner/motion_planning.py

@@ -233,13 +233,11 @@ def change_trajectory(self, distance_obj):
         Args:
             distance_obj (float): distance to overtake object
         """
-        pose_list = self.trajectory.poses #vehicle's planned path.
+        pose_list = self.trajectory.poses
 
         # Only use fallback
         self.generate_overtake_trajectory(distance_obj, pose_list)
         self.__overtake_status = 1
-        self.overtake_fallback(distance_obj, pose_list) #likely implements a simpler, predefined maneuver for overtaking, which adjusts the trajectory if more sophisticated planning is unavailable or as a safety fallback option.
-        self.__overtake_status = 1 #overtake successfully planed
         self.overtake_success_pub.publish(self.__overtake_status)
         return
 
@@ -263,8 +261,6 @@ def generate_overtake_trajectory(self, distance, pose_list, unstuck=False):
         Returns:
             None: The method updates the self.trajectory attribute with the new path.
         """
-    def overtake_fallback(self, distance, pose_list, unstuck=False): #his method constructs a temporary path around an obstacle based on the current location, intended distance to overtake, and a choice between two different lateral offsets depending on whether the vehicle is in an “unstuck” situation.
-        #codepart for overtake behaviour
         currentwp = self.current_wp
         normal_x_offset = 2
         unstuck_x_offset = 3  # could need adjustment with better steering
@@ -304,8 +300,6 @@ def overtake_fallback(self, distance, pose_list, unstuck=False): #his method con
             pos.header.frame_id = "global"
             pos.pose = pose
             result.append(pos)
-
-        #code part for returning back on the lane
         path = Path()
         path.header.stamp = rospy.Time.now()
         path.header.frame_id = "global"
@@ -504,10 +498,10 @@ def __set_curr_behavior(self, data: String):
         If the behavior is an overtake behavior, a trajectory change is triggered.
         """
         self.__curr_behavior = data.data
-        if data.data == bs.ot_enter_init.name: #is beggins overtake manuever
-            if np.isinf(self.__collision_point): # if no obsticle to overtake
-                self.__overtake_status = -1 #unsuccessfull or canceled overtake
-                self.overtake_success_pub.publish(self.__overtake_status) #abandonment of the overtake attempt.
+        if data.data == bs.ot_enter_init.name:
+            if np.isinf(self.__collision_point):
+                self.__overtake_status = -1
+                self.overtake_success_pub.publish(self.__overtake_status)
                 return
             self.change_trajectory(self.__collision_point)
 
@@ -543,7 +537,6 @@ def get_speed_by_behavior(self, behavior: str) -> float:
             speed = self.__get_speed_cruise()
         return speed
 
-    #manage the speed of the vehicle when it encounters situations where it becomes "stuck"
     def __get_speed_unstuck(self, behavior: str) -> float:
         # TODO check if this 'global' is necessary
         global UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE
@@ -554,7 +547,7 @@ def __get_speed_unstuck(self, behavior: str) -> float:
             speed = bs.us_stop.speed
         elif behavior == bs.us_overtake.name:
             pose_list = self.trajectory.poses
-            if self.unstuck_distance is None: # Without knowing the distance to the obstacle, the vehicle cannot safely plan an overtake trajectory.
+            if self.unstuck_distance is None:
                 self.logfatal("Unstuck distance not set")
                 return speed
 
@@ -564,12 +557,12 @@ def __get_speed_unstuck(self, behavior: str) -> float:
                 )
                 # self.logfatal(f"Unstuck Distance in mp: {distance}")
                 # clear distance to last unstuck -> avoid spamming overtake
-                if distance > UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE: #The method checks if the vehicle has moved a significant distance (UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE) since the last overtake. This prevents repeated overtake attempts when they are unnecessary or unsafe.
+                if distance > UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE:
                     self.unstuck_overtake_flag = False
                     self.logwarn("Unstuck Overtake Flag Cleared")
 
             # to avoid spamming the overtake_fallback
-            if self.unstuck_overtake_flag is False: 
+            if self.unstuck_overtake_flag is False:
                 # create overtake trajectory starting 6 meteres before
                 # the obstacle
                 # 6 worked well in tests, but can be adjusted
@@ -724,5 +717,4 @@ def loop(timer_event=None):
     except KeyboardInterrupt:
         pass
     finally:
-        roscomp.shutdown()
-
+        roscomp.shutdown()

doc/research/paf24/planning/Unstuck_Overtake Behavior.md

@@ -35,7 +35,8 @@ The "unstuck" behavior is designed to address situations where the vehicle becom
 
 ### How unstuck is triggered
 
-The method `__get_speed_unstuck` checks the current behavior (e.g., `us_unstuck`), adjusting speed accordingly and invoking `overtake_fallback` if an obstacle is detected and the vehicle is in an "unstuck" situation. `UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE` ensures that once an unstuck maneuver is initiated, it won't be repeated until a certain distance is cleared.
+The method `__get_speed_unstuck` checks the current behavior (e.g., `us_unstuck`), adjusting speed accordingly and invoking `overtake_fallback` if an obstacle is detected and the vehicle is in an "unstuck" situation. 
+`UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE` ensures that once an unstuck maneuver is initiated, it won't be repeated until a certain distance is cleared.
 
 ---
 
@@ -51,7 +52,8 @@ The overtake behavior allows the vehicle to safely overtake a slower-moving or s
 
 ### How overtake is triggered
 
-The method `__set_curr_behavior` monitors the vehicle’s behavior, triggering an overtake when `ot_enter_init` behavior is detected and if an obstacle or collision point is near. It then calls `change_trajectory` to modify the route. Speed adjustments specific to overtaking are handled in `__get_speed_overtake`, where the vehicle might slow down (e.g., `ot_enter_slow`) or proceed at normal speed after overtaking (e.g., `ot_leave`).
+The method `__set_curr_behavior` monitors the vehicle’s behavior, triggering an overtake when `ot_enter_init` behavior is detected and if an obstacle or collision point is near. It then calls `change_trajectory` to modify the route. 
+Speed adjustments specific to overtaking are handled in `__get_speed_overtake`, where the vehicle might slow down (e.g., `ot_enter_slow`) or proceed at normal speed after overtaking (e.g., `ot_leave`).
 
 ---
 
@@ -143,5 +145,4 @@ The trajectory generated for overtaking often leads to incorrect paths, such as
 - `__get_speed_unstuck`: Include checks for nearby vehicles and their speeds. Make `UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE` dynamic.
 - `__check_emergency`: Currently, it only considers whether the vehicle is in a parking behavior state. Expand this method to evaluate various emergency conditions (e.g., obstacles detected by sensors) and initiate appropriate responses (e.g., stopping or rerouting).
 - `get_speed_by_behavior`: Consider feedback from sensors regarding current traffic conditions.
-- `__calc_corner_points`: Consider a more intelligent approach rather than relying on simple angle thresholds.
-
+- `__calc_corner_points`: Consider a more intelligent approach rather than relying on simple angle thresholds.