fix: additional tests

code/planning/src/behavior_agent/behavior_tree.py

@@ -96,7 +96,7 @@ def main():
         rospy.logerr("Tree Setup failed")
         sys.exit(1)
     rospy.loginfo("tree setup worked")
-    r = rospy.Rate(5)
+    r = rospy.Rate(5.3)
     while not rospy.is_shutdown():
         behaviour_tree.tick()
         try:

code/planning/src/behavior_agent/behaviours/maneuvers.py

@@ -59,7 +59,6 @@ def initialise(self):
         execution
         """
         self.initPosition = self.blackboard.get("/paf/hero/current_pos")
-        rospy.loginfo("LEAVEPARKINGSPACE init")
 
     def update(self):
         """
@@ -99,7 +98,7 @@ def update(self):
                 endPos = np.array([self.initPosition.pose.position.x,
                                    self.initPosition.pose.position.y])
                 distance = np.linalg.norm(startPos - endPos)
-                if distance < 2 or speed.speed < 2:
+                if distance < 1 or speed.speed < 2:
                     self.curr_behavior_pub.publish(bs.parking.name)
                     self.initPosition = position
                     return py_trees.common.Status.RUNNING

code/planning/src/local_planner/ACC.py

@@ -10,7 +10,6 @@
 from std_msgs.msg import Float32MultiArray, Float32
 from collision_check import CollisionCheck
 import time
-from perception.msg import MinDistance
 
 
 class ACC(CompatibleNode):
@@ -34,8 +33,8 @@ def __init__(self):
         # Subscriber for lidar distance
         # TODO: Change to real lidar distance
         self.lidar_dist = self.new_subscription(
-            MinDistance,
-            f"/paf/{self.role_name}/LIDAR_range",
+            Float32,
+            f"/carla/{self.role_name}/LIDAR_range",
             self._set_distance,
             qos_profile=1)
         # Get initial set of speed limits
@@ -85,13 +84,13 @@ def __init__(self):
 
         self.logdebug("ACC initialized")
 
-    def _set_distance(self, data: MinDistance):
+    def _set_distance(self, data: Float32):
         """Get min distance to object in front from perception
 
         Args:
             data (MinDistance): Minimum Distance from LIDAR
         """
-        self.obstacle_distance = data.distance
+        self.obstacle_distance = data.data
 
     def __approx_speed_callback(self, data: Float32):
         """Safe approximated speed form obstacle in front together with
@@ -101,6 +100,7 @@ def __approx_speed_callback(self, data: Float32):
         Args:
             data (Float32): Speed from obstacle in front
         """
+        self.logerr("ACC: Approx speed recieved: " + str(data.data))
         self.obstacle_speed = (time.time(), data.data)
 
     def __get_current_velocity(self, data: CarlaSpeedometer):

code/planning/src/local_planner/collision_check.py

@@ -9,7 +9,6 @@
 # from std_msgs.msg import String
 from std_msgs.msg import Float32, Float32MultiArray
 from std_msgs.msg import Bool
-from perception.msg import MinDistance
 import time
 
 
@@ -23,7 +22,7 @@ def __init__(self):
         super(CollisionCheck, self).__init__('CollisionCheck')
         self.role_name = self.get_param("role_name", "hero")
         self.control_loop_rate = self.get_param("control_loop_rate", 1)
-
+        self.lidar_position_offset = 2
         # self.obstacle_sub: Subscriber = self.new_subscription(
         # )
         # Subscriber for current speed
@@ -35,14 +34,14 @@ def __init__(self):
         # Subscriber for lidar distance
         # TODO: Change to real lidar distance
         self.lidar_dist = self.new_subscription(
-            MinDistance,
-            f"/paf/{self.role_name}/LIDAR_range",
+            Float32,
+            f"/carla/{self.role_name}/LIDAR_range",
             self.__set_distance,
             qos_profile=1)
         # Publisher for emergency stop
         self.emergency_pub = self.new_publisher(
             Bool,
-            f"/paf/{self.role_name}/unchecked_emergency",
+            f"/paf/{self.role_name}/emergency",
             qos_profile=1)
         # Publisher for distance to collision
         self.collision_pub = self.new_publisher(
@@ -60,24 +59,26 @@ def __init__(self):
         self.__object_last_position: tuple = None
         self.logdebug("CollisionCheck started")
 
-    def __set_distance(self, data: MinDistance):
-        """Saves the distance from the lidar
-
-        Args:
-            data (MinDistance): Message from lidar with distance
+    def update_distance(self):
+        """Updates the distance to the obstacle in front
         """
-        if np.isinf(data.distance):
+        if np.isinf(self.__object_last_position[1]):
             self.__object_last_position = None
             self.__object_first_position = None
             return
         if self.__object_first_position is None:
-            self.__object_first_position = (time.time(), data.distance)
-            return
-        if self.__object_last_position is None:
-            self.__object_last_position = (time.time(), data.distance)
+            self.__object_first_position = self.__object_last_position
+            self.__object_last_position = None
             return
         self.__object_first_position = self.__object_last_position
-        self.__object_last_position = (time.time(), data.distance)
+
+    def __set_distance(self, data: Float32):
+        """Saves last distance from  LIDAR
+
+        Args:
+            data (Float32): Message from lidar with distance
+        """
+        self.__object_last_position = (time.time(), data.data)
 
     def calculate_obstacle_speed(self):
         """Caluclate the speed of the obstacle in front of the ego vehicle
@@ -91,8 +92,8 @@ def calculate_obstacle_speed(self):
         # If distance is np.inf no car is in front
 
         # Calculate time since last position update
-        current_time = time.time()
-        time_difference = current_time-self.__object_last_position[0]
+        time_difference = self.__object_last_position[0] - \
+            self.__object_first_position[0]
 
         # Calculate distance (in m)
         distance = self.__object_last_position[1] -\
@@ -177,8 +178,8 @@ def check_crash(self, obstacle):
         if collision_time > 0:
             if distance < emergency_distance2:
                 # Initiate emergency brake
-                self.emergency_pub.publish(True)
                 self.logerr("Emergency Brake")
+                self.emergency_pub.publish(True)
                 return
             # When no emergency brake is needed publish collision object
             data = Float32MultiArray(data=[distance, obstacle_speed])
@@ -198,6 +199,7 @@ def loop(timer_event=None):
             Checks if distance to a possible object is too small and
             publishes the desired speed to motion planning
             """
+            self.update_distance()
             self.calculate_obstacle_speed()
         self.new_timer(self.control_loop_rate, loop)
         self.spin()

code/planning/src/local_planner/motion_planning.py

@@ -91,7 +91,9 @@ def __check_emergency(self, data: Bool):
         Args:
             data (Bool): True if emergency stop detected by collision check
         """
-        if self.__curr_behavior is not bs.parking.name:
+        self.logerr("Emergency stop detected")
+        if not self.__curr_behavior == bs.parking.name:
+            self.logerr("Emergency stop detected and executed")
             self.emergency_pub.publish(data)
 
     def update_target_speed(self, acc_speed, behavior):
@@ -100,6 +102,7 @@ def update_target_speed(self, acc_speed, behavior):
             self.target_speed = min(be_speed, acc_speed)
         else:
             self.target_speed = be_speed
+        # self.logerr("target speed: " + str(self.target_speed))
         self.velocity_pub.publish(self.target_speed)
 
     def __set_acc_speed(self, data: Float32):
@@ -131,7 +134,6 @@ def get_speed_by_behavior(self, behavior: str) -> float:
             speed = bs.parking.speed
         else:
             speed = self.__get_speed_cruise()
-
         return speed
 
     def __get_speed_intersection(self, behavior: str) -> float:
@@ -212,7 +214,6 @@ def run(self):
         """
 
         def loop(timer_event=None):
-            self.loginfo("MotionPlanning loop")
             self.update_target_speed(self.__acc_speed, self.__curr_behavior)
 
         self.new_timer(self.control_loop_rate, loop)