diff --git a/code/planning/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py index c19d3386..6c4d4470 100755 --- a/code/planning/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/src/behavior_agent/behaviours/intersection.py @@ -97,7 +97,9 @@ def update(self): - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] Gets the current traffic light status, stop sign status - and the stop line distance + and the stop line distance. Calcualtes a virtual stop line and + publishes a distance to it. Slows down car until virtual stop line + is reached when there is a red traffic light or a stop sign. :return: py_trees.common.Status.RUNNING, if too far from intersection py_trees.common.Status.SUCCESS, if stopped in front of inter- section or entered the intersection @@ -145,7 +147,10 @@ def update(self): or (self.stop_sign_detected and not self.traffic_light_detected) ): - rospy.loginfo("slowing down!") + rospy.loginfo( + f"Intersection Approach: slowing down! Stop sign: " + f"{self.stop_sign_detected}, Light: {self.traffic_light_status}" + ) self.curr_behavior_pub.publish(bs.int_app_to_stop.name) # approach slowly when traffic light is green as traffic lights are @@ -164,14 +169,14 @@ def update(self): self.traffic_light_distance > 150 ): # too far - print("still approaching") + rospy.loginfo("Intersection still approaching") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and ( (self.virtual_stopline_distance < target_distance) or (self.traffic_light_distance < 150) ): # stopped - print("stopped") + rospy.loginfo("Intersection Approach: stopped") return py_trees.common.Status.SUCCESS elif ( speed > convert_to_ms(5.0) @@ -180,6 +185,10 @@ def update(self): ): # drive through intersection even if traffic light turns yellow + rospy.loginfo( + f"Intersection Approach Light is green, light:" + f"{self.traffic_light_status}" + ) return py_trees.common.Status.SUCCESS elif speed > convert_to_ms(5.0) and self.virtual_stopline_distance < 3.5: # running over line @@ -189,7 +198,7 @@ def update(self): self.virtual_stopline_distance < target_distance and not self.stopline_detected ): - rospy.loginfo("Leave intersection!") + rospy.loginfo("Intersection Approach: Leave intersection!") return py_trees.common.Status.SUCCESS else: return py_trees.common.Status.RUNNING @@ -276,7 +285,7 @@ def update(self): """ light_status_msg = self.blackboard.get("/paf/hero/Center/traffic_light_state") - # ADD FEATURE: Check if intersection is clear + # TODO: ADD FEATURE Check if intersection is clear lidar_data = None intersection_clear = True if lidar_data is not None: @@ -292,7 +301,7 @@ def update(self): # Wait at traffic light self.red_light_flag = True self.green_light_time = rospy.get_rostime() - rospy.loginfo(f"Light Status: {traffic_light_status}") + rospy.loginfo(f"Intersection Wait Light Status: {traffic_light_status}") self.curr_behavior_pub.publish(bs.int_wait.name) return py_trees.common.Status.RUNNING elif ( @@ -300,7 +309,7 @@ def update(self): and traffic_light_status == "green" ): # Wait approx 1s for confirmation - rospy.loginfo("Confirm green light!") + rospy.loginfo("Intersection Wait Confirm green light!") return py_trees.common.Status.RUNNING elif self.red_light_flag and traffic_light_status != "green": rospy.loginfo(f"Light Status: {traffic_light_status}" "-> prev was red") @@ -310,7 +319,9 @@ def update(self): rospy.get_rostime() - self.green_light_time > rospy.Duration(1) and traffic_light_status == "green" ): - rospy.loginfo(f"Light Status: {traffic_light_status}") + rospy.loginfo( + f"Driving through Intersection Light Status: {traffic_light_status}" + ) # Drive through intersection return py_trees.common.Status.SUCCESS else: diff --git a/code/planning/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py index 49ce0d6f..b6c67eae 100755 --- a/code/planning/src/behavior_agent/behaviours/lane_change.py +++ b/code/planning/src/behavior_agent/behaviours/lane_change.py @@ -27,7 +27,7 @@ def __init__(self, name): :param name: name of the behaviour """ super(Approach, self).__init__(name) - rospy.loginfo("Approach started") + rospy.loginfo("Lane Change Approach started") def setup(self, timeout): """ @@ -72,7 +72,8 @@ def update(self): - Triggering, checking, monitoring. Anything...but do not block! - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - Gets the current lane change distance. + Calculates a virtual stop line and slows down while approaching unless + lane change is not blocked. :return: py_trees.common.Status.RUNNING, if too far from lane change py_trees.common.Status.SUCCESS, if stopped in front of lane change or entered the lane change @@ -91,15 +92,15 @@ def update(self): if self.change_distance != np.inf and self.change_detected: self.virtual_change_distance = self.change_distance - # ADD FEATURE: Check for Traffic + # TODO: ADD FEATURE Check for Traffic distance_lidar = 20 if distance_lidar is not None and distance_lidar > 15.0: - rospy.loginfo("Change is free not slowing down!") + rospy.loginfo("Lane Change is free not slowing down!") self.curr_behavior_pub.publish(bs.lc_app_free.name) self.blocked = False else: - rospy.loginfo("Change blocked slowing down") + rospy.loginfo("Lane Change blocked slowing down") self.blocked = True # get speed @@ -110,11 +111,14 @@ def update(self): if speedometer is not None: speed = speedometer.speed else: - rospy.logwarn("no speedometer connected") + rospy.logwarn("Lane Change: no speedometer connected") return py_trees.common.Status.RUNNING if self.virtual_change_distance > target_dis and self.blocked: # too far - rospy.loginfo("still approaching") + rospy.loginfo( + f"Lane Change: still approaching, " + f"distance:{self.virtual_change_distance}" + ) self.curr_behavior_pub.publish(bs.lc_app_blocked.name) return py_trees.common.Status.RUNNING elif ( @@ -123,7 +127,7 @@ def update(self): and self.blocked ): # stopped - rospy.loginfo("stopped") + rospy.loginfo("Lane Change: stopped at virtual stop line") return py_trees.common.Status.SUCCESS elif ( speed > convert_to_ms(5.0) @@ -191,7 +195,7 @@ def initialise(self): Any initialisation you need before putting your behaviour to work. This just prints a state status message. """ - rospy.loginfo("Wait Change") + rospy.loginfo("Lane Change Wait") return True def update(self): @@ -213,14 +217,14 @@ def update(self): if speedometer is not None: speed = speedometer.speed else: - rospy.logwarn("no speedometer connected") + rospy.logwarn("Lane change wait: no speedometer connected") return py_trees.common.Status.RUNNING if speed > convert_to_ms(10): - rospy.loginfo("Forward to enter") + rospy.loginfo("Lane change wait: Was not blocked, proceed to drive forward") return py_trees.common.Status.SUCCESS - # ADD FEATURE: Check for Traffic + # TODO: ADD FEATURE Check for Traffic distance_lidar = 20 change_clear = False @@ -231,11 +235,11 @@ def update(self): else: change_clear = True if not change_clear: - rospy.loginfo("Change blocked") + rospy.loginfo("Lane Change Wait: blocked") self.curr_behavior_pub.publish(bs.lc_wait.name) return py_trees.common.Status.RUNNING else: - rospy.loginfo("Change clear") + rospy.loginfo("Lane Change Wait: Change clear") return py_trees.common.Status.SUCCESS def terminate(self, new_status): @@ -256,9 +260,8 @@ def terminate(self, new_status): class Enter(py_trees.behaviour.Behaviour): """ - This behavior handles the driving through an intersection, it initially - sets a speed and finishes if the ego vehicle is close to the end of the - intersection. + This behavior inititates the lane change and waits until the + lane change is finished. """ def __init__(self, name): @@ -297,7 +300,7 @@ def initialise(self): This prints a state status message and changes the driving speed for the lane change. """ - rospy.loginfo("Enter next Lane") + rospy.loginfo("Lane Change: Enter next Lane") self.curr_behavior_pub.publish(bs.lc_enter_init.name) def update(self): @@ -321,7 +324,7 @@ def update(self): if next_waypoint_msg is None: return py_trees.common.Status.FAILURE if next_waypoint_msg.distance < 5: - rospy.loginfo("Drive on the next lane!") + rospy.loginfo("Lane Change Enter: Drive on the next lane!") return py_trees.common.Status.RUNNING else: return py_trees.common.Status.SUCCESS @@ -345,7 +348,7 @@ def terminate(self, new_status): class Leave(py_trees.behaviour.Behaviour): """ This behaviour defines the leaf of this subtree, if this behavior is - reached, the vehicle left the intersection. + reached, the vehicle has finished the lane change. """ def __init__(self, name): @@ -384,7 +387,7 @@ def initialise(self): This prints a state status message and changes the driving speed to the street speed limit. """ - rospy.loginfo("Leave Change") + rospy.loginfo("Lane Change Finished") self.curr_behavior_pub.publish(bs.lc_exit.name) return True diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index f90ec1ae..41ad24f4 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -75,8 +75,9 @@ def update(self): - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - Gets the current distance to overtake, the current lane status and the - distance to collsion object. + Gets the current distance to overtake, the current oncoming lane status and the + distance to collsion object. Slows down while oncoming blocked until stopped + or oncoming clear. :return: py_trees.common.Status.RUNNING, if too far from overtaking py_trees.common.Status.SUCCESS, if stopped behind the blocking object or entered the process. @@ -110,7 +111,7 @@ def update(self): self.curr_behavior_pub.publish(bs.ot_app_free.name) return py_trees.common.Status.SUCCESS else: - rospy.loginfo("Overtake blocked slowing down") + rospy.loginfo("Overtake Approach: oncoming blocked slowing down") self.curr_behavior_pub.publish(bs.ot_app_blocked.name) # get speed @@ -123,11 +124,14 @@ def update(self): if self.ot_distance > 20.0: # too far - rospy.loginfo("still approaching") + rospy.loginfo( + f"Overtake Approach: still approaching obstacle, " + f"distance: {self.ot_distance}" + ) return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and self.ot_distance < TARGET_DISTANCE_TO_STOP: # stopped - rospy.loginfo("stopped") + rospy.loginfo("Overtake Approach: stopped behind obstacle") return py_trees.common.Status.SUCCESS else: # still approaching @@ -216,7 +220,7 @@ def update(self): clear_distance = 30 obstacle_msg = self.blackboard.get("/paf/hero/collision") if obstacle_msg is None: - rospy.logerr("No OBSTACLE") + rospy.logerr("No OBSTACLE in overtake wait") return py_trees.common.Status.FAILURE data = self.blackboard.get("/paf/hero/oncoming") @@ -231,14 +235,16 @@ def update(self): self.curr_behavior_pub.publish(bs.ot_wait_free.name) return py_trees.common.Status.SUCCESS else: - rospy.loginfo(f"Overtake still blocked: {distance_oncoming}") + rospy.loginfo( + f"Overtake still blocked, distance to oncoming: {distance_oncoming}" + ) self.curr_behavior_pub.publish(bs.ot_wait_stopped.name) return py_trees.common.Status.RUNNING elif obstacle_msg.data[0] == np.inf: - rospy.loginf("No OBSTACLE") + rospy.loginf("No OBSTACLE in overtake wait") return py_trees.common.Status.FAILURE else: - rospy.loginfo("No Lidar Distance") + rospy.loginfo("No Lidar Distance in overtake wait") return py_trees.common.Status.SUCCESS def terminate(self, new_status): @@ -312,7 +318,7 @@ def update(self): - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - + Waits for motion_planner to finish the new trajectory. :return: py_trees.common.Status.RUNNING, py_trees.common.Status.SUCCESS, py_trees.common.Status.FAILURE, @@ -320,17 +326,17 @@ def update(self): status = self.blackboard.get("/paf/hero/overtake_success") if status is not None: if status.data == 1: - rospy.loginfo("Overtake: Trajectory planned") + rospy.loginfo("Overtake Enter: Trajectory planned") return py_trees.common.Status.SUCCESS elif status.data == 0: self.curr_behavior_pub.publish(bs.ot_enter_slow.name) - rospy.loginfo("Overtake: Slowing down") + rospy.loginfo("Overtake Enter: Slowing down") return py_trees.common.Status.RUNNING else: - rospy.loginfo("OvertakeEnter: Abort") + rospy.loginfo("Overtake Enter: Abort") return py_trees.common.Status.FAILURE else: - rospy.loginfo("Overtake: Waiting for status update") + rospy.loginfo("Overtake Enter: Waiting for status update") return py_trees.common.Status.RUNNING def terminate(self, new_status): @@ -393,7 +399,7 @@ def initialise(self): self.curr_behavior_pub.publish(bs.ot_leave.name) data = self.blackboard.get("/paf/hero/current_pos") self.first_pos = np.array([data.pose.position.x, data.pose.position.y]) - rospy.loginfo(f"Leave Overtake: {self.first_pos}") + rospy.loginfo(f"Init Leave Overtake: {self.first_pos}") return True def update(self): @@ -412,7 +418,7 @@ def update(self): self.current_pos = np.array([data.pose.position.x, data.pose.position.y]) distance = np.linalg.norm(self.first_pos - self.current_pos) if distance > OVERTAKE_EXECUTING + NUM_WAYPOINTS: - rospy.loginfo(f"Left Overtake: {self.current_pos}") + rospy.loginfo(f"Overtake executed: {self.current_pos}") return py_trees.common.Status.FAILURE else: return py_trees.common.Status.RUNNING diff --git a/doc/assets/planning/behaviour_tree.PNG b/doc/assets/planning/behaviour_tree.PNG new file mode 100644 index 00000000..87ff5eb4 Binary files /dev/null and b/doc/assets/planning/behaviour_tree.PNG differ diff --git a/doc/assets/planning/behaviour_tree.drawio.xml b/doc/assets/planning/behaviour_tree.drawio.xml new file mode 100644 index 00000000..973300fc --- /dev/null +++ b/doc/assets/planning/behaviour_tree.drawio.xml @@ -0,0 +1,230 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/planning/Behavior_tree.md b/doc/planning/Behavior_tree.md index aae559df..bf78edb9 100644 --- a/doc/planning/Behavior_tree.md +++ b/doc/planning/Behavior_tree.md @@ -36,9 +36,9 @@ For visualization at runtime you might want to also install this [rqt-Plugin](ht ## Our behaviour tree The following section describes the behaviour tree we use for normal driving using all functionality provided by the agent. In the actual implementation this is part of a bigger tree, that handles things like writing topics to the blackboard, starting and finishing the decision tree. -The following tree is a simplification. +The following tree is a simplification. The draw.io xml file to update this diagram can be found inside /assets/planning/. -![Simple Tree](../assets/planning/simple_final_tree.png) +![Simple Tree](../assets/planning/behaviour_tree.PNG) ### Behavior diff --git a/doc/planning/README.md b/doc/planning/README.md index aff116ab..2fc268a0 100644 --- a/doc/planning/README.md +++ b/doc/planning/README.md @@ -25,7 +25,7 @@ The decision making collects most of the available information of the other comp the information. All possible traffic scenarios are covered in this component. The decision making uses a so called decision tree, which is easy to adapt and to expand. -![Simple Tree](../assets/planning/simple_final_tree.png) +![Simple Tree](../assets/planning/behaviour_tree.PNG) ### [Local Planning](./Local_Planning.md) diff --git a/doc/planning/behaviours/Intersection.md b/doc/planning/behaviours/Intersection.md new file mode 100644 index 00000000..7d17dd90 --- /dev/null +++ b/doc/planning/behaviours/Intersection.md @@ -0,0 +1,47 @@ +# Intersection Behavior + +**Summary:** This file explains the Intersection behavior. + +- [Intersection Behavior](#intersection-behavior) + - [General](#general) + - [Intersection Ahead](#intersection-ahead) + - [Approach](#approach) + - [Wait](#wait) + - [Enter](#enter) + - [Leave](#leave) + +## General + +The Intersection behaviour is used to control the vehicle when it encounters a intersection. It handles stop signs as well as traffic lights. +The Intersection sequence consists of the sub-behaviours "Approach", "Wait", "Enter" and "Leave". + +To enter the Intersection sequence "Intersection ahead" must firstly be successful. + +## Intersection ahead + +Successful when there is a stop line within a set distance. + +## Approach + +Handles approaching the intersection by slowing down the vehicle. Returns RUNNING while still far from the intersection, SUCCESS when the vehicle has stopped or has already entered the intersection and FAILURE when the path is faulty. + +Calculates a virtual stopline based on whether a stopline or only a stop sign has been detected and publishes a distance to it. While the vehicle is not stopped at the virtual stopline nor has entered the intersection, int_app_to_stop is published as the current behaviour. +This is used inside motion_planning to calculate a stopping velocity. + +A green light is approached with 30 km/h. + +## Wait + +Handles wating at the stop line until the vehicle is allowed to drive. + +If the light is green or when there isn't a traffic light returns SUCCESS otherwise RUNNING. + +## Enter + +Handles driving through the intersection. Uses 50 km/h as target speed. + +Returns SUCCESS once the next waypoint is not this intersection anymore. + +## Leave + +Signifies that the vehicle has left the intersection and simply returns FAILURE to leave the Intersection sequence. diff --git a/doc/planning/behaviours/LaneChange.md b/doc/planning/behaviours/LaneChange.md new file mode 100644 index 00000000..341b40a4 --- /dev/null +++ b/doc/planning/behaviours/LaneChange.md @@ -0,0 +1,39 @@ +# Lane Change Behavior + +**Summary:** This file explains the Lane Change behavior. + +- [Lane Change Behavior](#lanechange-behavior) + - [General](#general) + - [Lane Change Ahead](#lanechange-ahead) + - [Approach](#approach) + - [Wait](#wait) + - [Enter](#enter) + - [Leave](#leave) + +## General + +This behaviour executes a lane change. It slows the vehicle down until the lane change point is reached and then proceeds to switch lanes. + +## Lane Change ahead + +Checks whether the next waypoint is a lane change and inititates the lane change sequence accordingly. + +## Approach + +Calculates a virtual stop line at the lane change point and publishes lc_app_blocked for motion planner to slow down while too far away. + +If the lane change is not blocked (currently not implemented) the car does not slow down (30 km/h). + +Once the car is within a set distance of the virtual stop line and not blocked it returns SUCCESS. SUCCESS is also returned when the car stops at the stop line. + +## Wait + +Waits at the lane change point until the lane change is not blocked (not implemented). + +## Enter + +Inititates the lane change with 20 km/h and continues driving on the next lane until the lane change waypoint is far enough away. + +## Leave + +Simply exits the behaviour. diff --git a/doc/planning/behaviours/LeaveParkingSpace.md b/doc/planning/behaviours/LeaveParkingSpace.md new file mode 100644 index 00000000..51192638 --- /dev/null +++ b/doc/planning/behaviours/LeaveParkingSpace.md @@ -0,0 +1,13 @@ +# Leave Parking Space Behavior + +**Summary:** This file explains the Leave Parking Space behavior. + +- [Leave Parking Space Behavior](#leaveparkingspace-behavior) + - [General](#general) + +## General + +The leave parking space behaviour is only executed at the beginning of the simulation to leave the parking space. + +The behaviour calculates the euclidian distance between the starting position and the current position to determine whether the car has fully left the parking space. If that is the case a "called" +flag is set to true so that this behaviour is never executed again and FAILURE is returned to end the behaviour. Otherwise it stays in RUNNING. diff --git a/doc/planning/behaviours/Overtake.md b/doc/planning/behaviours/Overtake.md new file mode 100644 index 00000000..3058995a --- /dev/null +++ b/doc/planning/behaviours/Overtake.md @@ -0,0 +1,44 @@ +# Overtake Behavior + +**Summary:** This file explains the Overtake behavior. + +- [General](#general) +- [Overtake ahead](#overtake-ahead) +- [Approach](#approach) +- [Wait](#wait) +- [Enter](#enter) +- [Leave](#leave) + +## General + +This behaviour is used to overtake an object in close proximity. This behaviour is currently not working and more like a initial prototype. + +## Overtake ahead + +Checks whether there is a object in front of the car that needs overtaking. + +Estimates whether the car would collide with the object soon. If that is the case a counter gets incremented. When that counter reaches 4 SUCCESS is returned. If the object is not blocking the trajectory, FAILURE is returned. + +## Approach + +This is running while the obstacle is still in front of the car. + +Checks whether the oncoming traffic is far away or clear, if that is the case then ot_app_free is published as the current behaviour for the motion_planner and returns SUCCESS. Otherwise ot_app_blocked is published for the car to slow down. + +If the car stops behind the obstacle SUCCESS is also returned. + +## Wait + +This handles wating for clear oncoming traffic if the car has stopped behind the obstacle. If the overtake is clear ot_wait_free gets published and SUCCESS is returned. Otherwise ot_wait_stopped gets published and the behaviour stays in RUNNING. + +If the obstacle in front is gone the behaviour is aborted with FAILURE. + +## Enter + +Handles switching the lane for overtaking. + +Waits for motion planner to finish the trajectory changes and for it to set the overtake_success flag. + +## Leave + +Runs until the overtake is fully finished and then leaves the behaviour. diff --git a/doc/planning/Unstuck_Behavior.md b/doc/planning/behaviours/Unstuck.md similarity index 100% rename from doc/planning/Unstuck_Behavior.md rename to doc/planning/behaviours/Unstuck.md