From cff40c8b271568247818dd2d9d7242b6893ed387 Mon Sep 17 00:00:00 2001 From: Anastasiia Bilinska Date: Sat, 2 Nov 2024 22:10:10 +0100 Subject: [PATCH] .md length --- .../src/local_planner/motion_planning.py | 26 +++++++------------ .../planning/Unstuck_Overtake Behavior.md | 9 ++++--- 2 files changed, 14 insertions(+), 21 deletions(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 87ff244e..c57e8dd9 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/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() \ No newline at end of file diff --git a/doc/research/paf24/planning/Unstuck_Overtake Behavior.md b/doc/research/paf24/planning/Unstuck_Overtake Behavior.md index 513376cf..5e5e2c9e 100644 --- a/doc/research/paf24/planning/Unstuck_Overtake Behavior.md +++ b/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. \ No newline at end of file