fix: removed test code

code/planning/local_planner/launch/local_planner.launch

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

code/planning/local_planner/src/ACC.py

@@ -23,7 +23,7 @@ def __init__(self):
         self.control_loop_rate = self.get_param("control_loop_rate", 1)
         self.current_speed = 50 / 3.6  # m/ss
 
-        self.logerr("ACC started")
+        self.loginfo("ACC started")
         # TODO: Add Subscriber for Obsdacle from Collision Check
         self.collision_sub = self.new_subscription(
             Float32MultiArray,
@@ -33,8 +33,8 @@ def __init__(self):
 
         # Get current speed
         self.velocity_sub: Subscriber = self.new_subscription(
-            Float32,
-            f"/paf/{self.role_name}/test_speed",
+            CarlaSpeedometer,
+            f"/carla/{self.role_name}/Speed",
             self.__get_current_velocity,
             qos_profile=1)
 
@@ -90,7 +90,6 @@ def __get_collision(self, data: Float32MultiArray):
         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
@@ -113,7 +112,6 @@ def calculate_safe_speed(self):
         # 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
@@ -124,16 +122,13 @@ def calculate_safe_speed(self):
 
             safe_speed = self.obstacle[1] * (self.obstacle[0] /
                                              safety_distance)
-            self.logerr("Safe Speed: " + str(safe_speed))
             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
-            self.logerr("saftey distance good; Speed from obstacle: " +
-                        str(self.obstacle[1]))
             return self.obstacle[1]
 
     def __get_current_velocity(self, data: CarlaSpeedometer):
@@ -142,7 +137,7 @@ def __get_current_velocity(self, data: CarlaSpeedometer):
         Args:
             data (CarlaSpeedometer): _description_
         """
-        self.__current_velocity = float(data.data)
+        self.__current_velocity = float(data.speed)
 
     def __set_trajectory(self, data: Path):
         """Recieve trajectory from global planner

code/planning/local_planner/src/collision_check.py

@@ -6,7 +6,7 @@
 from ros_compatibility.node import CompatibleNode
 from rospy import Subscriber
 from geometry_msgs.msg import PoseStamped
-# from carla_msgs.msg import CarlaSpeedometer   # , CarlaWorldInfo
+from carla_msgs.msg import CarlaSpeedometer   # , CarlaWorldInfo
 # from std_msgs.msg import String
 from std_msgs.msg import Float32, Float32MultiArray
 from std_msgs.msg import Bool
@@ -25,14 +25,14 @@ def __init__(self):
         self.control_loop_rate = self.get_param("control_loop_rate", 1)
         # self.current_speed = 50 / 3.6  # m/ss
         # TODO: Add Subscriber for Speed and Obstacles
-        self.logerr("CollisionCheck started")
+        self.loginfo("CollisionCheck started")
 
         # self.obstacle_sub: Subscriber = self.new_subscription(
         # )
         # Subscriber for current speed
         self.velocity_sub: Subscriber = self.new_subscription(
-            Float32,  # CarlaSpeedometer # f"/carla/{self.role_name}/Speed"
-            f"/paf/{self.role_name}/test_speed",
+            CarlaSpeedometer,
+            f"/carla/{self.role_name}/Speed",
             self.__get_current_velocity,
             qos_profile=1)
         # Subscriber for current position
@@ -44,7 +44,7 @@ def __init__(self):
         # Subscriber for lidar distance
         self.lidar_dist = self.new_subscription(
             Float32,
-            f"/carla/{self.role_name}/lidar_dist_dev",
+            f"/carla/{self.role_name}/lidar_dist_dev", # TODO: Change to lidar topic
             self.calculate_obstacle_speed,
             qos_profile=1)
         # Publisher for emergency stop
@@ -71,30 +71,25 @@ def calculate_obstacle_speed(self, new_dist: Float32):
         """
         # Check if current speed from vehicle is not None
         if self.__current_velocity is None:
-            self.logerr("Current Speed is None")
             return
         # 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 \
                 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 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] + \
                 0.5 < time.time():
             self.__object_last_position = (time.time(),
                                            new_dist.data)
-            self.logerr("Time difference too big")
             return
         # Calculate time since last position update
         current_time = time.time()
@@ -104,25 +99,20 @@ def calculate_obstacle_speed(self, new_dist: Float32):
         distance = new_dist.data - self.__object_last_position[1]
 
         # Speed is distance/time (m/s)
-        self.logerr("Time Difference: " + str(time_difference))
-        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((new_dist.data, speed))
         self.__object_last_position = (current_time, new_dist.data)
 
-    def __get_current_velocity(self, data: Float32):
+    def __get_current_velocity(self, data: CarlaSpeedometer,):
         """Saves current velocity of the ego vehicle
 
         Args:
             data (CarlaSpeedometer): Message from carla with current speed
         """
-        self.__current_velocity = float(data.data)
+        self.__current_velocity = float(data.speed)
 
     def __current_position_callback(self, data: PoseStamped):
         """Saves current position of the ego vehicle
@@ -189,28 +179,23 @@ def check_crash(self, obstacle):
 
         collision_time = self.time_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(
             True, self.__current_velocity)
-        self.logerr("Emergency Distance: " + str(emergency_distance2))
         if collision_time > 0:
             if distance < emergency_distance2:
                 # Initiate emergency brake
                 self.emergency_pub.publish(True)
-                self.logerr("Emergency Brake")
                 return
             # When no emergency brake is needed publish collision distance for
             # ACC and Behaviour tree
             data = Float32MultiArray(data=[distance, obstacle_speed])
             self.collision_pub.publish(data)
-            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, obstacle_speed])
             self.collision_pub.publish(data)
-            self.logerr("No Collision ahead")
 
     def run(self):
         """

code/planning/local_planner/src/dev_collision_publisher.py

@@ -44,7 +44,7 @@ def __init__(self):
             topic='/paf/' + self.role_name + '/manual',
             callback=self.callback_manual,
             qos_profile=1)
-        self.logerr("DevCollisionCheck started")
+        self.loginfo("DevCollisionCheck started")
         self.last_position_update = None
         self.simulated_speed = 12  # m/s
         self.distance_to_collision = 0
@@ -54,7 +54,6 @@ def __init__(self):
 
     def callback_manual(self, msg: Float32):
         if self.manual_start:
-            self.logerr("Manual start")
             self.manual_start = False
             self.pub_lidar.publish(Float32(data=25))
             time.sleep(0.2)