553 feature document behaviour tree and behaviours

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,15 +301,15 @@ 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 (
                 rospy.get_rostime() - self.green_light_time < rospy.Duration(1)
                 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:

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

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,25 +318,25 @@ 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,
         """
         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