feat: testing done and perception distance msg adapted

build/docker-compose.yml

@@ -57,7 +57,7 @@ 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 && 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"

code/planning/local_planner/CMakeLists.txt

@@ -11,7 +11,9 @@ find_package(catkin REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-
+find_package(catkin REQUIRED COMPONENTS
+  perception
+)
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -44,9 +46,7 @@ find_package(catkin REQUIRED)
 
 ## Generate messages in the 'msg' folder
 # add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
+#   MinDistance.msg
 # )
 
 ## Generate services in the 'srv' folder
@@ -65,8 +65,7 @@ find_package(catkin REQUIRED)
 
 ## Generate added messages and services with any dependencies listed here
 # generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
+#   perception
 # )
 
 ################################################
@@ -103,6 +102,7 @@ catkin_package(
 #  LIBRARIES planning
 #  CATKIN_DEPENDS other_catkin_pkg
 #  DEPENDS system_lib
+  CATKIN_DEPENDS perception
 )
 
 ###########

code/planning/local_planner/launch/local_planner.launch

@@ -9,6 +9,6 @@
     </node>
     <!-- <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" />
+        <param name="control_loop_rate" value="0.3" />
     </node> -->
 </launch>

code/planning/local_planner/package.xml

@@ -32,7 +32,6 @@
   <!-- Dependencies can be catkin packages or system dependencies -->
   <!-- Examples: -->
   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
   <!--   Note that this is equivalent to the following: -->
   <!--   <build_depend>roscpp</build_depend> -->
   <!--   <exec_depend>roscpp</exec_depend> -->
@@ -48,12 +47,14 @@
   <!--   <test_depend>gtest</test_depend> -->
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
+  <build_depend>perception</build_depend>
+  <build_export_depend>perception</build_export_depend>
+  <exec_depend>perception</exec_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

code/planning/local_planner/src/ACC.py

@@ -7,7 +7,7 @@
 from carla_msgs.msg import CarlaSpeedometer   # , CarlaWorldInfo
 from nav_msgs.msg import Path
 # from std_msgs.msg import String
-from std_msgs.msg import Float32MultiArray, Float32, Bool
+from std_msgs.msg import Float32MultiArray, Float32
 from collision_check import CollisionCheck
 import time
 from perception.msg import MinDistance
@@ -53,7 +53,7 @@ def __init__(self):
             qos_profile=1)
 
         self.emergency_sub: Subscriber = self.new_subscription(
-            msg_type=Bool,
+            msg_type=PoseStamped,
             topic="/paf/" + self.role_name + "/current_pos",
             callback=self.__current_position_callback,
             qos_profile=1)
@@ -140,7 +140,6 @@ def __current_position_callback(self, data: PoseStamped):
             pose.position
         next_wp = self.__trajectory.poses[self.__current_wp_index + 1].\
             pose.position
-
         # distances from agent to current and next waypoint
         d_old = abs(agent.x - current_wp.x) + abs(agent.y - current_wp.y)
         d_new = abs(agent.x - next_wp.x) + abs(agent.y - next_wp.y)
@@ -182,10 +181,12 @@ def loop(timer_event=None):
                     # 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

@@ -55,7 +55,7 @@ def __init__(self):
         # Approx speed publisher for ACC
         self.speed_publisher = self.new_publisher(
             Float32,
-            f"/paf/{self.rolename}/cc_speed",
+            f"/paf/{self.role_name}/cc_speed",
             qos_profile=1)
         # Variables to save vehicle data
         self.__current_velocity: float = None
@@ -99,7 +99,7 @@ def calculate_obstacle_speed(self, new_dist: MinDistance):
         relative_speed = distance/time_difference
         speed = self.__current_velocity + relative_speed
         # Publish speed to ACC for permanent distance check
-        self.speed_publisher(Float32(data=speed))
+        self.speed_publisher.publish(Float32(data=speed))
         # Check for crash
         self.check_crash((new_dist.distance, speed))
         self.__object_last_position = (current_time, new_dist.distance)

code/planning/local_planner/src/dev_collision_publisher.py

@@ -9,7 +9,9 @@
 # from nav_msgs.msg import Path
 # from std_msgs.msg import String
 from std_msgs.msg import Float32
+from carla_msgs.msg import CarlaSpeedometer
 import time
+from perception.msg import MinDistance
 # import numpy as np
 
 
@@ -25,13 +27,13 @@ def __init__(self):
         self.control_loop_rate = self.get_param("control_loop_rate", 1)
 
         self.pub_lidar = self.new_publisher(
-            msg_type=Float32,
-            topic='/carla/' + self.role_name + '/lidar_dist_dev',
+            msg_type=MinDistance,
+            topic=f'/paf/{self.role_name}/Center/min_distance',
             qos_profile=1)
 
         self.pub_test_speed = self.new_publisher(
-            msg_type=Float32,
-            topic='/paf/' + self.role_name + '/test_speed',
+            msg_type=CarlaSpeedometer,
+            topic='/carla/' + self.role_name + '/test_speed',
             qos_profile=1)
         self.sub_ACC = self.new_subscription(
             msg_type=Float32,
@@ -55,11 +57,11 @@ def __init__(self):
     def callback_manual(self, msg: Float32):
         if self.manual_start:
             self.manual_start = False
-            self.pub_lidar.publish(Float32(data=25))
+            self.pub_lidar.publish(MinDistance(distance=25))
             time.sleep(0.2)
-            self.pub_lidar.publish(Float32(data=25))
+            self.pub_lidar.publish(MinDistance(distance=25))
             time.sleep(0.2)
-            self.pub_lidar.publish(Float32(data=24))
+            self.pub_lidar.publish(MinDistance(distance=24))
             # time.sleep(0.2)
             # self.pub_lidar.publish(Float32(data=20))
             # time.sleep(0.2)
@@ -69,8 +71,8 @@ def callback_manual(self, msg: Float32):
 
     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,7 +81,7 @@ def run(self):
         :return:
         """
         def loop(timer_event=None):
-            self.pub_test_speed.publish(Float32(data=13.8889))
+            self.pub_test_speed.publish(CarlaSpeedometer(speed=13.8889))
 
         self.new_timer(self.control_loop_rate, loop)
         self.spin()