fix: implemented Feedback from review

ACC now permanently checking the distance

code/planning/local_planner/launch/local_planner.launch

@@ -5,7 +5,7 @@
     </node>
     <node pkg="local_planner" type="ACC.py" name="ACC" output="screen">
         <param name="role_name" value="hero" />
-        <param name="control_loop_rate" value="1" />
+        <param name="control_loop_rate" value="0.3" />
     </node>
     <!-- <node pkg="local_planner" type="dev_collision_publisher.py" name="DevCollisionCheck" output="screen">
         <param name="role_name" value="hero" />

code/planning/local_planner/src/ACC.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python
-import numpy as np
 import ros_compatibility as roscomp
 # import tf.transformations
 from ros_compatibility.node import CompatibleNode
@@ -36,7 +35,7 @@ def __init__(self):
         # TODO: Change to real lidar distance
         self.lidar_dist = self.new_subscription(
             MinDistance,
-            f"/carla/{self.role_name}/LIDAR_range",
+            f"/paf/{self.role_name}/Center/min_distance",
             self._set_distance,
             qos_profile=1)
         # Get initial set of speed limits
@@ -77,8 +76,6 @@ def __init__(self):
         self.__current_wp_index: int = 0
         # Current speed
         self.__current_velocity: float = None
-        # Is an obstacle ahead where we would collide with?
-        self.collision_ahead: bool = False
         # Distance and speed from possible collsion object
         self.obstacle_speed: tuple = None
         # Obstalce distance
@@ -95,48 +92,15 @@ def _set_distance(self, data: MinDistance):
         self.obstacle_distance = data.distance
 
     def __approx_speed_callback(self, data: Float32):
-        """Safe approximated speed form obstacle in front together with timestamp 
-        when recieved. 
+        """Safe approximated speed form obstacle in front together with
+        timestamp when recieved.
         Timestamp is needed to check wether we still have a vehicle in front
 
         Args:
             data (Float32): Speed from obstacle in front
         """
         self.obstacle_speed = (time.time(), data.data)
 
-    def calculate_safe_speed(self):
-        """calculates the speed to meet the desired distance to the object
-
-        Returns:
-            float: safe speed tp meet the desired distance
-        """
-        # No speed or obstacle recieved yet
-        if self.__current_velocity is None:
-            return None
-        if self.obstacle is None:
-            return None
-        # Calculate safety distance
-        safety_distance = CollisionCheck.calculate_rule_of_thumb(
-            False, self.__current_velocity)
-        if self.obstacle[0] < safety_distance:
-            # If safety distance is reached, we want to reduce the speed to
-            # meet the desired distance
-            # Speed is reduced by the factor of the distance to the safety
-            # distance
-            # Another solution could be
-            # object_speed - (safety_distance-distance)
-
-            safe_speed = self.obstacle[1] * (self.obstacle[0] /
-                                             safety_distance)
-            return safe_speed
-        else:
-            # If safety distance is reached, drive with same speed as
-            # Object in front
-            # TODO:
-            # Incooperate overtaking ->
-            # Communicate with decision tree about overtaking
-            return self.obstacle[1]
-
     def __get_current_velocity(self, data: CarlaSpeedometer):
         """_summary_
 
@@ -192,7 +156,35 @@ def run(self):
         :return:
         """
         def loop(timer_event=None):
-            if self.collision_ahead is False:
+            """
+            Checks if distance to a possible object is too small and
+            publishes the desired speed to motion planning
+            """
+            if self.obstacle_speed is not None:
+                # Check if too much time has passed since last speed update
+                if self.obstacle_speed[0] + 0.5 < time.time():
+                    self.obstacle_speed = None
+
+            if self.obstacle_distance is not None and \
+                    self.obstacle_speed is not None and \
+                    self.__current_velocity is not None:
+                # If we have obstalce speed and distance, we can
+                # calculate the safe speed
+                safety_distance = CollisionCheck.calculate_rule_of_thumb(
+                    False, self.__current_velocity)
+                if self.obstacle_distance < safety_distance:
+                    # If safety distance is reached, we want to reduce the
+                    # speed to meet the desired distance
+                    safe_speed = self.obstacle_speed[1] * \
+                        (self.obstacle_distance / safety_distance)
+                    self.velocity_pub.publish(safe_speed)
+                else:
+                    # If safety distance is reached, drive with same speed as
+                    # Object in front
+                    self.velocity_pub.publish(self.obstacle_speed[1])
+            elif self.speed_limit is not None:
+                # If we have no obstacle, we want to drive with the current
+                # speed limit
                 self.velocity_pub.publish(self.speed_limit)
         self.new_timer(self.control_loop_rate, loop)
         self.spin()

code/planning/local_planner/src/collision_check.py

@@ -9,6 +9,7 @@
 # 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
 
 
@@ -37,8 +38,8 @@ def __init__(self):
         # Subscriber for lidar distance
         # TODO: Change to real lidar distance
         self.lidar_dist = self.new_subscription(
-            Float32,
-            f"/carla/{self.role_name}/lidar_dist_dev",
+            MinDistance,
+            f"/paf/{self.role_name}/Center/min_distance",
             self.calculate_obstacle_speed,
             qos_profile=1)
         # Publisher for emergency stop
@@ -60,50 +61,48 @@ def __init__(self):
         self.__current_velocity: float = None
         self.__object_last_position: tuple = None
 
-    def calculate_obstacle_speed(self, new_dist: Float32):
+    def calculate_obstacle_speed(self, new_dist: MinDistance):
         """Caluclate the speed of the obstacle in front of the ego vehicle
             based on the distance between to timestamps
 
         Args:
-            new_position (Float32): new position received from the lidar
+            new_position (MinDistance): new position received from the lidar
         """
         # Check if current speed from vehicle is not None
         if self.__current_velocity is None:
             return
         # Check if this is the first time the callback is called
         if self.__object_last_position is None and \
-                np.isinf(new_dist.data) is not True:
+                np.isinf(new_dist.distance) is not True:
             self.__object_last_position = (time.time(),
-                                           new_dist.data)
+                                           new_dist.distance)
             return
 
         # If distance is np.inf no car is in front
-        if np.isinf(new_dist.data):
+        if np.isinf(new_dist.distance):
             self.__object_last_position = None
-            data = Float32MultiArray(data=[np.Inf, -1])
-            self.collision_pub.publish(data)
             return
         # Check if too much time has passed since last position update
         if self.__object_last_position[0] + \
                 0.5 < time.time():
             self.__object_last_position = (time.time(),
-                                           new_dist.data)
+                                           new_dist.distance)
             return
         # Calculate time since last position update
         current_time = time.time()
         time_difference = current_time-self.__object_last_position[0]
 
         # Calculate distance (in m)
-        distance = new_dist.data - self.__object_last_position[1]
+        distance = new_dist.distance - self.__object_last_position[1]
 
         # Speed is distance/time (m/s)
         relative_speed = distance/time_difference
         speed = self.__current_velocity + relative_speed
-        # Publish distance and speed to ACC for permanent distance check
+        # Publish speed to ACC for permanent distance check
         self.speed_publisher(Float32(data=speed))
         # Check for crash
-        self.check_crash((new_dist.data, speed))
-        self.__object_last_position = (current_time, new_dist.data)
+        self.check_crash((new_dist.distance, speed))
+        self.__object_last_position = (current_time, new_dist.distance)
 
     def __get_current_velocity(self, data: CarlaSpeedometer,):
         """Saves current velocity of the ego vehicle
@@ -178,11 +177,9 @@ def check_crash(self, obstacle):
                 # Initiate emergency brake
                 self.emergency_pub.publish(True)
                 return
-            # When no emergency brake is needed publish collision distance for
-            # ACC and Behaviour tree
+            # When no emergency brake is needed publish collision object
             data = Float32MultiArray(data=[distance, obstacle_speed])
             self.collision_pub.publish(data)
-            # print(f"Safe Distance Thumb: {safe_distance2:.2f}")
         else:
             # If no collision is ahead publish np.Inf
             data = Float32MultiArray(data=[np.Inf, obstacle_speed])