feat: Testing done

build/docker-compose.yml

@@ -57,8 +57,8 @@ services:
     tty: true
     shm_size: 2gb
     #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS"
-    command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
-    # command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP"
+    #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
+    command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP"
     logging:
       driver: "local"
     environment:

code/planning/local_planner/src/ACC.py

@@ -9,6 +9,7 @@
 from nav_msgs.msg import Path
 # from std_msgs.msg import String
 from std_msgs.msg import Float32MultiArray, Float32
+from collision_check import CollisionCheck
 
 
 class ACC(CompatibleNode):
@@ -32,8 +33,8 @@ def __init__(self):
 
         # Get current speed
         self.velocity_sub: Subscriber = self.new_subscription(
-            CarlaSpeedometer,
-            f"/carla/{self.role_name}/Speed",
+            Float32,
+            f"/paf/{self.role_name}/test_speed",
             self.__get_current_velocity,
             qos_profile=1)
 
@@ -77,7 +78,7 @@ def __init__(self):
         # Distance and speed from possible collsion object
         self.obstacle: tuple = None
         # Current speed limit
-        self.speed_limit: float = np.Inf
+        self.speed_limit: float = None  # m/s
 
     def __get_collision(self, data: Float32MultiArray):
         """Check if collision is ahead
@@ -86,9 +87,10 @@ def __get_collision(self, data: Float32MultiArray):
             data (Float32MultiArray): Distance and speed from possible
                                         collsion object
         """
-        if data.data[1] == np.Inf:
+        if np.isinf(data.data[0]):
             # No collision ahead
             self.collision_ahead = False
+            self.logerr("No Collision ahead -> ACC")
         else:
             # Collision ahead
             self.collision_ahead = True
@@ -108,10 +110,10 @@ def calculate_safe_speed(self):
             return None
         if self.obstacle is None:
             return None
-        # 1s * m/s = reaction distance
-        reaction_distance = self.__current_velocity
-        safety_distance = reaction_distance + \
-            (self.__current_velocity * 0.36)**2
+        # Calculate safety distance
+        safety_distance = CollisionCheck.calculate_rule_of_thumb(
+            False, self.__current_velocity)
+        self.logerr("Safety Distance: " + str(safety_distance))
         if self.obstacle[0] < safety_distance:
             # If safety distance is reached, we want to reduce the speed to
             # meet the desired distance
@@ -130,7 +132,7 @@ def calculate_safe_speed(self):
             # TODO:
             # Incooperate overtaking ->
             # Communicate with decision tree about overtaking
-            self.logerr("saftey distance gooood; Speed from obstacle: " +
+            self.logerr("saftey distance good; Speed from obstacle: " +
                         str(self.obstacle[1]))
             return self.obstacle[1]
 
@@ -140,7 +142,7 @@ def __get_current_velocity(self, data: CarlaSpeedometer):
         Args:
             data (CarlaSpeedometer): _description_
         """
-        self.__current_velocity = float(data.speed)
+        self.__current_velocity = float(data.data)
 
     def __set_trajectory(self, data: Path):
         """Recieve trajectory from global planner

code/planning/local_planner/src/collision_check.py

@@ -23,7 +23,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.current_speed = 50 / 3.6  # m/ss
+        # self.current_speed = 50 / 3.6  # m/ss
         # TODO: Add Subscriber for Speed and Obstacles
         self.logerr("CollisionCheck started")
 
@@ -76,15 +76,18 @@ def calculate_obstacle_speed(self, new_dist: Float32):
         # Check if this is the first time the callback is called
         self.logerr("distance recieved: " + str(new_dist.data))
         if self.__object_last_position is None and \
-                new_dist.data is not np.inf:
+                np.isinf(new_dist.data) is not True:
             self.__object_last_position = (time.time(),
                                            new_dist.data)
             self.logerr("First Position")
             return
 
         # If distance is np.inf no car is in front
-        if new_dist.data is np.inf:
+        if np.isinf(new_dist.data):
             self.__object_last_position = None
+            data = Float32MultiArray(data=[np.Inf, -1])
+            self.collision_pub.publish(data)
+            self.logerr("No car in front: dist recieved is inf")
             return
         # Check if too much time has passed since last position update
         if self.__object_last_position[0] + \
@@ -102,15 +105,15 @@ def calculate_obstacle_speed(self, new_dist: Float32):
 
         # Speed is distance/time (m/s)
         self.logerr("Time Difference: " + str(time_difference))
-        self.logerr("Distance: " + str(distance))
+        self.logerr("Distance difference: " + str(distance))
         relative_speed = distance/time_difference
         self.logerr("Relative Speed: " + str(relative_speed))
         self.logerr("Current Speed: " + str(self.__current_velocity))
         speed = self.__current_velocity + relative_speed
         self.logerr("Speed: " + str(speed))
 
         # Check for crash
-        self.check_crash((distance, speed))
+        self.check_crash((new_dist.data, speed))
         self.__object_last_position = (current_time, new_dist.data)
 
     def __get_current_velocity(self, data: Float32):
@@ -139,7 +142,9 @@ def time_to_collision(self, obstacle_speed, distance):
         Returns:
             float: Time until collision with obstacle in front
         """
-        return distance / (self.current_speed - obstacle_speed)
+        if (self.__current_velocity - obstacle_speed) == 0:
+            return -1
+        return distance / (self.__current_velocity - obstacle_speed)
 
     def meters_to_collision(self, obstacle_speed, distance):
         """Calculates the meters until collision with the obstacle in front
@@ -154,7 +159,8 @@ def meters_to_collision(self, obstacle_speed, distance):
         return self.time_to_collision(obstacle_speed, distance) * \
             self.__current_velocity
 
-    def calculate_rule_of_thumb(self, emergency):
+    @staticmethod
+    def calculate_rule_of_thumb(emergency, speed):
         """Calculates the rule of thumb as approximation
         for the braking distance
 
@@ -164,8 +170,8 @@ def calculate_rule_of_thumb(self, emergency):
         Returns:
             float: distance calculated with rule of thumb
         """
-        reaction_distance = self.current_speed
-        braking_distance = (self.current_speed * 0.36)**2
+        reaction_distance = speed
+        braking_distance = (speed * 0.36)**2
         if emergency:
             return reaction_distance + braking_distance / 2
         else:
@@ -182,11 +188,12 @@ def check_crash(self, obstacle):
         distance, obstacle_speed = obstacle
 
         collision_time = self.time_to_collision(obstacle_speed, distance)
-        collision_meter = self.meters_to_collision(obstacle_speed, distance)
-
+        # collision_meter = self.meters_to_collision(obstacle_speed, distance)
+        self.logerr("Collision Time: " + str(collision_time))
         # safe_distance2 = self.calculate_rule_of_thumb(False)
-        emergency_distance2 = self.calculate_rule_of_thumb(False)
-
+        emergency_distance2 = self.calculate_rule_of_thumb(
+            True, self.__current_velocity)
+        self.logerr("Emergency Distance: " + str(emergency_distance2))
         if collision_time > 0:
             if distance < emergency_distance2:
                 # Initiate emergency brake
@@ -195,13 +202,13 @@ def check_crash(self, obstacle):
                 return
             # When no emergency brake is needed publish collision distance for
             # ACC and Behaviour tree
-            data = Float32MultiArray(data=[collision_meter, obstacle_speed])
+            data = Float32MultiArray(data=[distance, obstacle_speed])
             self.collision_pub.publish(data)
-            self.logerr("Collision Distance: " + str(collision_meter))
+            self.logerr("Collision published")
             # print(f"Safe Distance Thumb: {safe_distance2:.2f}")
         else:
             # If no collision is ahead publish np.Inf
-            data = Float32MultiArray(data=[np.Inf, -1])
+            data = Float32MultiArray(data=[np.Inf, obstacle_speed])
             self.collision_pub.publish(data)
             self.logerr("No Collision ahead")
 

code/planning/local_planner/src/dev_collision_publisher.py

@@ -10,6 +10,7 @@
 # from std_msgs.msg import String
 from std_msgs.msg import Float32
 import time
+# import numpy as np
 
 
 class DevCollisionCheck(CompatibleNode):
@@ -34,7 +35,7 @@ def __init__(self):
             qos_profile=1)
         self.sub_ACC = self.new_subscription(
             msg_type=Float32,
-            topic='/paf/' + self.role_name + '/ACC',
+            topic='/paf/' + self.role_name + '/acc_velocity',
             callback=self.callback_ACC,
             qos_profile=1)
 
@@ -59,18 +60,18 @@ def callback_manual(self, msg: Float32):
             time.sleep(0.2)
             self.pub_lidar.publish(Float32(data=25))
             time.sleep(0.2)
-            self.pub_lidar.publish(Float32(data=22))
-            time.sleep(0.2)
-            self.pub_lidar.publish(Float32(data=20))
-            time.sleep(0.2)
-            self.pub_lidar.publish(Float32(data=20))
-            time.sleep(0.2)
-            self.pub_lidar.publish(Float32(data=20))
+            self.pub_lidar.publish(Float32(data=24))
+            # time.sleep(0.2)
+            # self.pub_lidar.publish(Float32(data=20))
+            # time.sleep(0.2)
+            # self.pub_lidar.publish(Float32(data=20))
+            # time.sleep(0.2)
+            # self.pub_lidar.publish(Float32(data=20))
 
     def callback_ACC(self, msg: Float32):
         self.acc_activated = True
-        self.logerr("Timestamp: " + time.time().__str__())
-        self.logerr("ACC: " + str(msg.data))
+        # self.logerr("Timestamp: " + time.time().__str__())
+        # self.logerr("ACC: " + str(msg.data))
         self.current_speed = msg.data
 
     def run(self):
@@ -79,10 +80,8 @@ def run(self):
         :return:
         """
         def loop(timer_event=None):
-            if self.acc_activated is False:
-                self.pub_test_speed.publish(Float32(data=13.8889))
-            else:
-                self.pub_test_speed.publish(Float32(data=self.current_speed))
+            self.pub_test_speed.publish(Float32(data=13.8889))
+
         self.new_timer(self.control_loop_rate, loop)
         self.spin()