diff --git a/build/docker-compose.yml b/build/docker-compose.yml
index 1545f945..a44d725e 100644
--- a/build/docker-compose.yml
+++ b/build/docker-compose.yml
@@ -57,8 +57,9 @@ 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:
diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch
index 5a90b88a..586101e7 100644
--- a/code/acting/launch/acting.launch
+++ b/code/acting/launch/acting.launch
@@ -29,10 +29,10 @@
-
+
diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch
index 6a13825f..cdb6dec9 100644
--- a/code/planning/global_planner/launch/global_planner.launch
+++ b/code/planning/global_planner/launch/global_planner.launch
@@ -1,7 +1,7 @@
diff --git a/code/planning/local_planner/package.xml b/code/planning/local_planner/package.xml
index 6dc4269a..a1e4978c 100644
--- a/code/planning/local_planner/package.xml
+++ b/code/planning/local_planner/package.xml
@@ -32,7 +32,6 @@
-
@@ -48,12 +47,14 @@
+ perception
+ perception
+ perception
catkin
-
diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py
new file mode 100755
index 00000000..d13357fa
--- /dev/null
+++ b/code/planning/local_planner/src/ACC.py
@@ -0,0 +1,207 @@
+#!/usr/bin/env python
+import ros_compatibility as roscomp
+# import tf.transformations
+from ros_compatibility.node import CompatibleNode
+from rospy import Subscriber, Publisher
+from geometry_msgs.msg import PoseStamped
+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
+from collision_check import CollisionCheck
+import time
+from perception.msg import MinDistance
+
+
+class ACC(CompatibleNode):
+ """
+ This node recieves a possible collision and
+ """
+
+ def __init__(self):
+ super(ACC, self).__init__('ACC')
+ 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
+
+ # Get current speed
+ self.velocity_sub: Subscriber = self.new_subscription(
+ CarlaSpeedometer,
+ f"/carla/{self.role_name}/Speed",
+ self.__get_current_velocity,
+ qos_profile=1)
+
+ # Subscriber for lidar distance
+ # TODO: Change to real lidar distance
+ self.lidar_dist = self.new_subscription(
+ MinDistance,
+ f"/paf/{self.role_name}/Center/min_distance",
+ self._set_distance,
+ qos_profile=1)
+ # Get initial set of speed limits
+ self.speed_limit_OD_sub: Subscriber = self.new_subscription(
+ Float32MultiArray,
+ f"/paf/{self.role_name}/speed_limits_OpenDrive",
+ self.__set_speed_limits_opendrive,
+ qos_profile=1)
+
+ # Get trajectory to determine current speed limit
+ self.trajectory_sub: Subscriber = self.new_subscription(
+ Path,
+ f"/paf/{self.role_name}/trajectory",
+ self.__set_trajectory,
+ qos_profile=1)
+
+ self.emergency_sub: Subscriber = self.new_subscription(
+ msg_type=PoseStamped,
+ topic="/paf/" + self.role_name + "/current_pos",
+ callback=self.__current_position_callback,
+ qos_profile=1)
+ self.approx_speed_sub = self.new_subscription(
+ Float32,
+ f"/paf/{self.role_name}/cc_speed",
+ self.__approx_speed_callback,
+ qos_profile=1)
+ # Publish desired speed to acting
+ self.velocity_pub: Publisher = self.new_publisher(
+ Float32,
+ f"/paf/{self.role_name}/acc_velocity",
+ qos_profile=1)
+
+ # List of all speed limits, sorted by waypoint index
+ self.__speed_limits_OD: [float] = []
+ # Current Trajectory
+ self.__trajectory: Path = None
+ # Current index from waypoint
+ self.__current_wp_index: int = 0
+ # Current speed
+ self.__current_velocity: float = None
+ # Distance and speed from possible collsion object
+ self.obstacle_speed: tuple = None
+ # Obstalce distance
+ self.obstacle_distance = None
+ # Current speed limit
+ self.speed_limit: float = None # m/s
+
+ def _set_distance(self, data: MinDistance):
+ """Get min distance to object in front from perception
+
+ Args:
+ data (MinDistance): Minimum Distance from LIDAR
+ """
+ self.obstacle_distance = data.distance
+
+ def __approx_speed_callback(self, data: Float32):
+ """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 __get_current_velocity(self, data: CarlaSpeedometer):
+ """_summary_
+
+ Args:
+ data (CarlaSpeedometer): _description_
+ """
+ self.__current_velocity = float(data.speed)
+
+ def __set_trajectory(self, data: Path):
+ """Recieve trajectory from global planner
+
+ Args:
+ data (Path): Trajectory path
+ """
+ self.__trajectory = data
+
+ def __set_speed_limits_opendrive(self, data: Float32MultiArray):
+ """Recieve speed limits from OpenDrive via global planner
+
+ Args:
+ data (Float32MultiArray): speed limits per waypoint
+ """
+ self.__speed_limits_OD = data.data
+
+ def __current_position_callback(self, data: PoseStamped):
+ """Get current position and check if next waypoint is reached
+ If yes -> update current waypoint and speed limit
+
+ Args:
+ data (PoseStamped): Current position from perception
+ """
+ if len(self.__speed_limits_OD) < 1 or self.__trajectory is None:
+ return
+
+ agent = data.pose.position
+ current_wp = self.__trajectory.poses[self.__current_wp_index].\
+ 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)
+ if d_new < d_old:
+ # update current waypoint and corresponding speed limit
+ self.__current_wp_index += 1
+ self.speed_limit = \
+ self.__speed_limits_OD[self.__current_wp_index]
+
+ def run(self):
+ """
+ Control loop
+ :return:
+ """
+ def loop(timer_event=None):
+ """
+ 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()
+
+
+if __name__ == "__main__":
+ """
+ main function starts the ACC node
+ :param args:
+ """
+ roscomp.init('ACC')
+
+ try:
+ node = ACC()
+ node.run()
+ except KeyboardInterrupt:
+ pass
+ finally:
+ roscomp.shutdown()
diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py
index 34bcdbb4..5fe0d95a 100755
--- a/code/planning/local_planner/src/collision_check.py
+++ b/code/planning/local_planner/src/collision_check.py
@@ -1,14 +1,16 @@
#!/usr/bin/env python
# import rospy
+import numpy as np
# import tf.transformations
import ros_compatibility as roscomp
from ros_compatibility.node import CompatibleNode
-
-# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
-# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo
-# from nav_msgs.msg import Path
+from rospy import Subscriber
+from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo
# from std_msgs.msg import String
-# from std_msgs.msg import Float32MultiArray
+from std_msgs.msg import Float32, Float32MultiArray
+from std_msgs.msg import Bool
+from perception.msg import MinDistance
+import time
class CollisionCheck(CompatibleNode):
@@ -21,73 +23,173 @@ 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.logdebug("CollisionCheck started")
-
- def update(self, speed):
- self.current_speed = speed
+ self.loginfo("CollisionCheck started")
+
+ # self.obstacle_sub: Subscriber = self.new_subscription(
+ # )
+ # Subscriber for current speed
+ self.velocity_sub: Subscriber = self.new_subscription(
+ CarlaSpeedometer,
+ f"/carla/{self.role_name}/Speed",
+ self.__get_current_velocity,
+ qos_profile=1)
+ # Subscriber for lidar distance
+ # TODO: Change to real lidar distance
+ self.lidar_dist = self.new_subscription(
+ MinDistance,
+ f"/paf/{self.role_name}/Center/min_distance",
+ self.calculate_obstacle_speed,
+ qos_profile=1)
+ # Publisher for emergency stop
+ self.emergency_pub = self.new_publisher(
+ Bool,
+ f"/paf/{self.role_name}/emergency",
+ qos_profile=1)
+ # Publisher for distance to collision
+ self.collision_pub = self.new_publisher(
+ Float32MultiArray,
+ f"/paf/{self.role_name}/collision",
+ qos_profile=1)
+ # Approx speed publisher for ACC
+ self.speed_publisher = self.new_publisher(
+ Float32,
+ f"/paf/{self.role_name}/cc_speed",
+ qos_profile=1)
+ # Variables to save vehicle data
+ self.__current_velocity: float = None
+ self.__object_last_position: tuple = None
+
+ 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 (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.distance) is not True:
+ self.__object_last_position = (time.time(),
+ new_dist.distance)
+ return
+
+ # If distance is np.inf no car is in front
+ if np.isinf(new_dist.distance):
+ self.__object_last_position = None
+ 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.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.distance - self.__object_last_position[1]
+
+ # Speed is distance/time (m/s)
+ relative_speed = distance/time_difference
+ speed = self.__current_velocity + relative_speed
+ # Publish speed to ACC for permanent distance check
+ 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)
+
+ 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.speed)
def time_to_collision(self, obstacle_speed, distance):
- return distance / (self.current_speed - obstacle_speed)
+ """calculates the time to collision with the obstacle in front
+
+ Args:
+ obstacle_speed (float): Speed from obstacle in front
+ distance (float): Distance to obstacle in front
+
+ Returns:
+ float: Time until collision with obstacle in front
+ """
+ if (self.__current_velocity - obstacle_speed) == 0:
+ return -1
+ return distance / (self.__current_velocity - obstacle_speed)
def meters_to_collision(self, obstacle_speed, distance):
- return self.time_to_collision(obstacle_speed, distance) * \
- self.current_speed
+ """Calculates the meters until collision with the obstacle in front
- # PAF 22
- def calculate_safe_dist(self) -> float:
+ Args:
+ obstacle_speed (float): speed from obstacle in front
+ distance (float): distance from obstacle in front
+
+ Returns:
+ float: distance (in meters) until collision with obstacle in front
"""
- Calculates the distance you have to keep to the vehicle in front to
- have t_reaction to react to the vehicle suddenly stopping
- The formula replicates official recommendations for safe distances
+ return self.time_to_collision(obstacle_speed, distance) * \
+ self.__current_velocity
+
+ @staticmethod
+ def calculate_rule_of_thumb(emergency, speed):
+ """Calculates the rule of thumb as approximation
+ for the braking distance
+
+ Args:
+ emergency (bool): if emergency brake is initiated
+
+ Returns:
+ float: distance calculated with rule of thumb
"""
- t_reaction = 1 # s
- t_breaking = 1 # s
- a = 8 # m/s^2
- v = self.current_speed
- s = - 0.5 * a * t_breaking ** 2 + v * t_breaking + v * t_reaction
- return s + 5
-
- def calculate_rule_of_thumb(self, emergency):
- 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:
return reaction_distance + braking_distance
def check_crash(self, obstacle):
+ """ Checks if and when the ego vehicle will crash
+ with the obstacle in front
+
+ Args:
+ obstacle (tuple): tuple with distance and
+ speed from obstacle in front
+ """
distance, obstacle_speed = obstacle
collision_time = self.time_to_collision(obstacle_speed, distance)
- collision_meter = self.meters_to_collision(obstacle_speed, distance)
-
- safe_distance = self.calculate_safe_dist()
- safe_distance2 = self.calculate_rule_of_thumb(False)
- emergency_distance2 = self.calculate_rule_of_thumb(True)
-
- # TODO: Convert to Publishers
+ # collision_meter = self.meters_to_collision(obstacle_speed, distance)
+ # safe_distance2 = self.calculate_rule_of_thumb(False)
+ emergency_distance2 = self.calculate_rule_of_thumb(
+ True, self.__current_velocity)
if collision_time > 0:
if distance < emergency_distance2:
- print(f"Emergency Brake needed, {emergency_distance2:.2f}")
- print(f"Ego reaches obstacle after {collision_time:.2f} seconds.")
- print(f"Ego reaches obstacle after {collision_meter:.2f} meters.")
- print(f"Safe Distance PAF 22: {safe_distance:.2f}")
- print(f"Safe Distance Thumb: {safe_distance2:.2f}")
+ # Initiate emergency brake
+ self.emergency_pub.publish(True)
+ return
+ # When no emergency brake is needed publish collision object
+ data = Float32MultiArray(data=[distance, obstacle_speed])
+ self.collision_pub.publish(data)
else:
- print("Ego slower then car in front")
+ # If no collision is ahead publish np.Inf
+ data = Float32MultiArray(data=[np.Inf, obstacle_speed])
+ self.collision_pub.publish(data)
def run(self):
"""
Control loop
:return:
"""
-
- def loop(timer_event=None):
- pass
-
- self.new_timer(self.control_loop_rate, loop)
self.spin()
diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/local_planner/src/dev_collision_publisher.py
new file mode 100755
index 00000000..9bbd267a
--- /dev/null
+++ b/code/planning/local_planner/src/dev_collision_publisher.py
@@ -0,0 +1,103 @@
+#!/usr/bin/env python
+# import rospy
+# import tf.transformations
+import ros_compatibility as roscomp
+from ros_compatibility.node import CompatibleNode
+
+# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
+# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo
+# 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
+
+
+class DevCollisionCheck(CompatibleNode):
+ """
+ This is currently a test node. In the future this node will be
+ responsible for detecting collisions and reporting them.
+ """
+
+ def __init__(self):
+ super(DevCollisionCheck, self).__init__('DevCollisionCheck')
+ self.role_name = self.get_param("role_name", "hero")
+ self.control_loop_rate = self.get_param("control_loop_rate", 1)
+
+ self.pub_lidar = self.new_publisher(
+ msg_type=MinDistance,
+ topic=f'/paf/{self.role_name}/Center/min_distance',
+ qos_profile=1)
+
+ self.pub_test_speed = self.new_publisher(
+ msg_type=CarlaSpeedometer,
+ topic='/carla/' + self.role_name + '/test_speed',
+ qos_profile=1)
+ self.sub_ACC = self.new_subscription(
+ msg_type=Float32,
+ topic='/paf/' + self.role_name + '/acc_velocity',
+ callback=self.callback_ACC,
+ qos_profile=1)
+
+ self.sub_manual = self.new_subscription(
+ msg_type=Float32,
+ topic='/paf/' + self.role_name + '/manual',
+ callback=self.callback_manual,
+ qos_profile=1)
+ self.loginfo("DevCollisionCheck started")
+ self.last_position_update = None
+ self.simulated_speed = 12 # m/s
+ self.distance_to_collision = 0
+ self.current_speed = 0
+ self.manual_start = True
+ self.acc_activated = False
+
+ def callback_manual(self, msg: Float32):
+ if self.manual_start:
+ self.manual_start = False
+ self.pub_lidar.publish(MinDistance(distance=25))
+ time.sleep(0.2)
+ self.pub_lidar.publish(MinDistance(distance=25))
+ time.sleep(0.2)
+ self.pub_lidar.publish(MinDistance(distance=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.current_speed = msg.data
+
+ def run(self):
+ """
+ Control loop
+ :return:
+ """
+ def loop(timer_event=None):
+ self.pub_test_speed.publish(CarlaSpeedometer(speed=13.8889))
+
+ self.new_timer(self.control_loop_rate, loop)
+ self.spin()
+
+
+if __name__ == "__main__":
+ """
+ main function starts the CollisionCheck node
+ :param args:
+ """
+ roscomp.init('DevCollisionCheck')
+
+ try:
+ node = DevCollisionCheck()
+ node.run()
+ except KeyboardInterrupt:
+ pass
+ finally:
+ roscomp.shutdown()
diff --git a/doc/03_research/Leaderboard-2/capture_sensor_data.py b/doc/03_research/Leaderboard-2/capture_sensor_data.py
deleted file mode 100644
index ee2b26df..00000000
--- a/doc/03_research/Leaderboard-2/capture_sensor_data.py
+++ /dev/null
@@ -1,431 +0,0 @@
-#!/usr/bin/env python
-
-# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de
-# Barcelona (UAB).
-#
-# This work is licensed under the terms of the MIT license.
-# For a copy, see .
-
-"""
-Welcome to the capture sensor data script, a script that provides users with a baseline for data collection,
-which they can later modify to their specific needs, easying the process of creating a database.
-
-This script will start with a CARLA recorder log, spawning the desired sensor configuration at the ego vehicle,
-and saving their data into a folder. The exact parameters can be found at the very top of the script and include:
-
-> SENSORS: List of all the sensors tha will be spawned in the simulation
-> WEATHER: Weather of the simulation
-> RECORDER_INFO: List of all the CARLA recorder logs that will be run. Each recorder has three elements:
- - folder: path to the folder with the `*.log` recorder file, and a `log.json` file, which has all the ego vehicle information
- - start_time: start time of the recorder
- - duration: duration of the recorder
-"""
-
-import time
-import os
-import carla
-import argparse
-import random
-import json
-import threading
-import glob
-
-from queue import Queue, Empty
-
-################### User simulation configuration ####################
-# 1) Choose the sensors
-SENSORS = [
- [
- 'CameraTest',
- {
- 'bp': 'sensor.camera.rgb',
- 'image_size_x': 720, 'image_size_y': 1080, 'fov': 100,
- 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0
- },
- ],
- [
- 'LidarTest',
- {
- 'bp': 'sensor.lidar.ray_cast',
- 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0
- }
- ],
- [
- 'SemanticLidarTest',
- {
- 'bp': 'sensor.lidar.ray_cast_semantic',
- 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0
- }
- ],
- [
- 'RADARTest',
- {
- 'bp': 'sensor.other.radar',
- 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0
- }
- ],
- [
- 'GnssTest',
- {
- 'bp': 'sensor.other.gnss',
- 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0
- }
- ],
- [
- 'IMUTest',
- {
- 'bp': 'sensor.other.imu',
- 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0
- }
- ]
-]
-
-# 2) Choose a weather
-WEATHER = carla.WeatherParameters(
- sun_azimuth_angle=-1.0, sun_altitude_angle=70.0,
- cloudiness=30.0, precipitation=0.0, precipitation_deposits=80.0, wetness=15.0,
- wind_intensity=10.0,
- fog_density=2.0, fog_distance=0.0, fog_falloff=0.0)
-
-# 3) Choose a recorder file (folder path, start time, duration)
-RECORDER_INFO = [
- {
- 'folder': "records/Town10_1",
- 'start_time': 20,
- 'duration': 10
- },
- {
- 'folder': "records/Town10_1",
- 'start_time': 80,
- 'duration': 5
- }
-]
-
-# 4) Choose the destination folder where the sensor data will be saved
-DESTINATION_FOLDER = "database"
-################# End user simulation configuration ##################
-
-FPS = 20
-THREADS = 5
-CURRENT_THREADS = 0
-
-def create_folders(endpoint, sensors):
- for sensor_id, sensor_bp in sensors:
- sensor_endpoint = f"{endpoint}/{sensor_id}"
- if not os.path.exists(sensor_endpoint):
- os.makedirs(sensor_endpoint)
-
- if 'gnss' in sensor_bp:
- sensor_endpoint = f"{endpoint}/{sensor_id}/gnss_data.csv"
- with open(sensor_endpoint, 'w') as data_file:
- data_txt = f"Frame,Altitude,Latitude,Longitude\n"
- data_file.write(data_txt)
-
- if 'imu' in sensor_bp:
- sensor_endpoint = f"{endpoint}/{sensor_id}/imu_data.csv"
- with open(sensor_endpoint, 'w') as data_file:
- data_txt = f"Frame,Accelerometer X,Accelerometer y,Accelerometer Z,Compass,Gyroscope X,Gyroscope Y,Gyroscope Z\n"
- data_file.write(data_txt)
-
-def add_listener(sensor, sensor_queue, sensor_id):
- sensor.listen(lambda data: sensor_listen(data, sensor_queue, sensor_id))
-
-def sensor_listen(data, sensor_queue, sensor_id):
- sensor_queue.put((sensor_id, data.frame, data))
- return
-
-def get_ego_id(recorder_file):
- found_lincoln = False
- found_id = None
-
- for line in recorder_file.split("\n"):
-
- # Check the role_name for hero
- if found_lincoln:
- if not line.startswith(" "):
- found_lincoln = False
- found_id = None
- else:
- data = line.split(" = ")
- if 'role_name' in data[0] and 'hero' in data[1]:
- return found_id
-
- # Search for all lincoln vehicles
- if not found_lincoln and line.startswith(" Create ") and 'vehicle.lincoln' in line:
- found_lincoln = True
- found_id = int(line.split(" ")[2][:-1])
-
- return found_id
-
-def save_data_to_disk(sensor_id, frame, data, imu_data, endpoint):
- """
- Saves the sensor data into file:
- - Images -> '.png', one per frame, named as the frame id
- - Lidar: -> '.ply', one per frame, named as the frame id
- - SemanticLidar: -> '.ply', one per frame, named as the frame id
- - RADAR: -> '.csv', one per frame, named as the frame id
- - GNSS: -> '.csv', one line per frame, named 'gnss_data.csv'
- - IMU: -> '.csv', one line per frame, named 'imu_data.csv'
- """
- global CURRENT_THREADS
- CURRENT_THREADS += 1
- if isinstance(data, carla.Image):
- sensor_endpoint = f"{endpoint}/{sensor_id}/{frame}.png"
- data.save_to_disk(sensor_endpoint)
-
- elif isinstance(data, carla.LidarMeasurement):
- sensor_endpoint = f"{endpoint}/{sensor_id}/{frame}.ply"
- data.save_to_disk(sensor_endpoint)
-
- elif isinstance(data, carla.SemanticLidarMeasurement):
- sensor_endpoint = f"{endpoint}/{sensor_id}/{frame}.ply"
- data.save_to_disk(sensor_endpoint)
-
- elif isinstance(data, carla.RadarMeasurement):
- sensor_endpoint = f"{endpoint}/{sensor_id}/{frame}.csv"
- data_txt = f"Altitude,Azimuth,Depth,Velocity\n"
- for point_data in data:
- data_txt += f"{point_data.altitude},{point_data.azimuth},{point_data.depth},{point_data.velocity}\n"
- with open(sensor_endpoint, 'w') as data_file:
- data_file.write(data_txt)
-
- elif isinstance(data, carla.GnssMeasurement):
- sensor_endpoint = f"{endpoint}/{sensor_id}/gnss_data.csv"
- with open(sensor_endpoint, 'a') as data_file:
- data_txt = f"{frame},{data.altitude},{data.latitude},{data.longitude}\n"
- data_file.write(data_txt)
-
- elif isinstance(data, carla.IMUMeasurement):
- sensor_endpoint = f"{endpoint}/{sensor_id}/imu_data.csv"
- with open(sensor_endpoint, 'a') as data_file:
- data_txt = f"{frame},{imu_data[0][0]},{imu_data[0][1]},{imu_data[0][2]},{data.compass},{imu_data[1][0]},{imu_data[1][1]},{imu_data[1][2]}\n"
- data_file.write(data_txt)
-
- else:
- print(f"WARNING: Ignoring sensor '{sensor_id}', as no callback method is known for data of type '{type(data)}'.")
-
- CURRENT_THREADS -= 1
-
-def extract_imu_data(log):
- records = log["records"]
- log_data = []
- for record in records:
- acceleration_data = record["state"]["acceleration"]
- acceleration_vector = [acceleration_data["x"], acceleration_data["y"], acceleration_data["z"]]
-
- # TODO: Remove this (don't use logs without angular velocity)
- if "angular_velcoity" in record["state"]:
- angular_data = record["state"]["angular_velocity"]
- angular_vector = [angular_data["x"], angular_data["y"], angular_data["z"]]
- else:
- angular_vector = [random.random(), random.random(), random.random()]
-
- log_data.append([acceleration_vector, angular_vector])
-
- return log_data
-
-def main():
-
- argparser = argparse.ArgumentParser(description=__doc__)
- argparser.add_argument('--host', default='127.0.0.1', help='IP of the host server (default: 127.0.0.1)')
- argparser.add_argument('--port', default=2000, type=int, help='TCP port to listen to (default: 2000)')
- args = argparser.parse_args()
- print(__doc__)
-
- active_sensors = []
-
- try:
-
- # Initialize the simulation
- client = carla.Client(args.host, args.port)
- client.set_timeout(120.0)
- world = client.get_world()
-
- for recorder_info in RECORDER_INFO:
- recorder_folder = recorder_info['folder']
- recorder_start = recorder_info['start_time']
- recorder_duration = recorder_info['duration']
-
- # 0) Get the recorder files
- recorder_path_list = glob.glob(f"{os.getcwd()}/{recorder_folder}/*.log")
- if recorder_path_list:
- recorder_path = recorder_path_list[0]
- else:
- print(f"Couldn't find the recorder file for the folder '{recorder_folder}'. Stopping...")
- continue
- recorder_log_list = glob.glob(f"{os.getcwd()}/{recorder_folder}/log.json")
- if recorder_log_list:
- recorder_log = recorder_log_list[0]
- else:
- recorder_log = None
-
- print(f"Running: {recorder_path}")
- endpoint = f"{DESTINATION_FOLDER}/{recorder_path.split('/')[-1][:-4]}"
-
- # 1) Get the recorder map and load the world
- recorder_str = client.show_recorder_file_info(recorder_path, True)
- recorder_map = recorder_str.split("\n")[1][5:]
- world = client.load_world(recorder_map)
- world.tick()
-
- # 2) Change the weather and synchronous mode
- world.set_weather(WEATHER)
- settings = world.get_settings()
- settings.fixed_delta_seconds = 1 / FPS
- settings.synchronous_mode = True
- world.apply_settings(settings)
-
- for _ in range(100):
- world.tick()
-
- # 3) Replay the recorder
- max_duration = float(recorder_str.split("\n")[-2].split(" ")[1])
- if recorder_duration == 0:
- recorder_duration = max_duration
- elif recorder_start + recorder_duration > max_duration:
- print("WARNING: Found a duration that exceeds the recorder length. Reducing it...")
- recorder_duration = max_duration - recorder_start
- if recorder_start >= max_duration:
- print("WARNING: Found a start point that exceeds the recoder duration. Ignoring it...")
- continue
- print(f"Duration: {round(recorder_duration, 2)} - Frames: {round(20*recorder_duration, 0)}")
-
- if recorder_log:
- with open(recorder_log) as fd:
- log_json = json.load(fd)
- imu_logs = extract_imu_data(log_json)
- else:
- imu_logs = None
-
- client.replay_file(recorder_path, recorder_start, recorder_duration, get_ego_id(recorder_str), False)
- with open(f"{recorder_path[:-4]}.txt", 'w') as fd:
- fd.write(recorder_str)
- world.tick()
-
- # 4) Link onto the ego vehicle
- hero = None
- while hero is None:
- print("Waiting for the ego vehicle...")
- possible_vehicles = world.get_actors().filter('vehicle.*')
- for vehicle in possible_vehicles:
- if vehicle.attributes['role_name'] == 'hero':
- print("Ego vehicle found")
- hero = vehicle
- break
- time.sleep(5)
-
- # 5) Create the sensors, and save their data into a queue
- create_folders(endpoint, [[s[0], s[1].get('bp')] for s in SENSORS])
- blueprint_library = world.get_blueprint_library()
- sensor_queue = Queue()
- for sensor in SENSORS:
-
- # Extract the data from the sesor configuration
- sensor_id = sensor[0]
- attributes = sensor[1]
- blueprint_name = attributes.get('bp')
- sensor_transform = carla.Transform(
- carla.Location(x=attributes.get('x'), y=attributes.get('y'), z=attributes.get('z')),
- carla.Rotation(pitch=attributes.get('pitch'), roll=attributes.get('roll'), yaw=attributes.get('yaw'))
- )
-
- # Get the blueprint and add the attributes
- blueprint = blueprint_library.find(blueprint_name)
- for key, value in attributes.items():
- if key in ['bp', 'x', 'y', 'z', 'roll', 'pitch', 'yaw']:
- continue
- blueprint.set_attribute(str(key), str(value))
-
- # Create the sensors and its callback
- sensor = world.spawn_actor(blueprint, sensor_transform, hero)
- add_listener(sensor, sensor_queue, sensor_id)
- active_sensors.append(sensor)
-
- world.tick()
-
- # 6) Run the simulation
- start_time = world.get_snapshot().timestamp.elapsed_seconds
- start_frame = world.get_snapshot().frame
- sensor_amount = len(SENSORS)
-
- max_threads = THREADS
- results = []
-
- while True:
- current_time = world.get_snapshot().timestamp.elapsed_seconds
- current_duration = current_time - start_time
- if current_duration >= recorder_duration:
- print(f">>>>> Running recorded simulation: 100.00% completed <<<<<")
- break
-
- completion = format(round(current_duration / recorder_duration * 100, 2), '3.2f')
- print(f">>>>> Running recorded simulation: {completion}% completed <<<<<", end="\r")
-
- # Get and save the sensor data from the queue.
- missing_sensors = sensor_amount
- while True:
-
- frame = world.get_snapshot().frame
- try:
- sensor_data = sensor_queue.get(True, 2.0)
- if sensor_data[1] != frame:
- continue # Ignore previous frame data
- missing_sensors -= 1
- except Empty:
- raise ValueError("A sensor took too long to send their data")
-
- # Get the data
- sensor_id = sensor_data[0]
- frame_diff = sensor_data[1] - start_frame
- data = sensor_data[2]
- if imu_logs:
- imu_data = imu_logs[int(FPS*recorder_start + frame_diff)]
- else:
- imu_data = [[0,0,0], [0,0,0]]
-
- res = threading.Thread(target=save_data_to_disk, args=(sensor_id, frame_diff, data, imu_data, endpoint))
- results.append(res)
- res.start()
-
- if CURRENT_THREADS > max_threads:
- for res in results:
- res.join()
- results = []
-
- if missing_sensors <= 0:
- break
-
- world.tick()
-
- for res in results:
- res.join()
-
- for sensor in active_sensors:
- sensor.stop()
- sensor.destroy()
- active_sensors = []
-
- for _ in range(50):
- world.tick()
-
- # End the simulation
- finally:
- # stop and remove cameras
- for sensor in active_sensors:
- sensor.stop()
- sensor.destroy()
-
- # set fixed time step length
- settings = world.get_settings()
- settings.fixed_delta_seconds = None
- settings.synchronous_mode = False
- world.apply_settings(settings)
-
-if __name__ == '__main__':
-
- try:
- main()
- except KeyboardInterrupt:
- pass
- finally:
- print('\ndone.')
diff --git a/doc/07_planning/local_planning.md b/doc/07_planning/local_planning.md
new file mode 100644
index 00000000..cefa0a5e
--- /dev/null
+++ b/doc/07_planning/local_planning.md
@@ -0,0 +1,104 @@
+# Local Planning
+
+**Summary:** Documentation of Collision Check node and ACC in package local planner
+
+---
+
+## Author
+
+Samuel Kühnel
+
+## Date
+
+17.12.2023
+
+## Collsision Check
+
+### Subscibed Topics
+
+* `/carla/hero/Speed`: Get current vehicle speed
+* `/paf/hero/Center/min_distance`: Get min distance from LIDAR
+
+### Published Topics+
+
+* `/paf/hero/emergency`: Boolean that indicates if emergency brake is needed
+* `/paf/hero/collision`: Collision object (Float32 Array) with distance and speed of obstacle in front
+
+### Tasks
+
+#### Calculate speed of obstacle in front
+
+When the node recieves a distance from the LIDAR it is saved together with a timestamp so when the next distance message arrives the speed can be approximated.
+
+This could be removed in the future, as when the radar gets involved the speed no longer needs to be approximated.
+
+#### Check if crash is ahead
+
+The Collision Check checks based on the current speed and last distance if a collsision with the obstacle in front is ahead.
+
+The code looks like this:
+
+```python
+ obstacle_speed, distance = obstacle
+ collision_time = time_to_collision(obstacle_speed, distance)
+ # Calculation of distance for emergency stop
+ emergency_distance = calculate_rule_of_thumb(True, __current_velocity)
+ if collision_time > 0:
+ # Check if emergency brake is needed to stop
+ if distance < emergency_distance:
+ # Initiate emergency brake
+ self.emergency_pub.publish(True)
+ 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)
+ else:
+ # If no collision is ahead publish np.Inf
+ data = Float32MultiArray(data=[np.Inf, obstacle_speed])
+```
+
+For calculating the distance the "rule of thumb" is used.
+
+$$
+ distance_{safety} = speed + (speed \cdot 0.36)^2
+$$
+
+$$
+ distance_{emergency} = speed + \frac{(speed \cdot 0.36)^2}{2}
+$$
+
+## ACC
+
+### Subscibed Topics
+
+* `/carla/hero/Speed`: Get current vehicle speed
+* `/paf/hero/collision`: Get the collision object
+* `/paf/hero/speed_limits_OpenDrive`: Get speedlimits from waypoints
+* `/paf/hero/trajectory`: Get current trajectory
+* `/paf/hero/emergency`: Deactivate ACC if emergency brake is initiated
+* `/paf/hero/current_pos`: Get current position
+
+### Published Topics+
+
+* `/paf/hero/acc_velocity`: Velocity to keep distance to object in front
+
+### Tasks
+
+#### Get current speed limit
+
+The ACC subscribes to the trajectory, speed limit and current position.
+
+Every time the current position is updated the node calculates the current speed limit based on the trajectory and the speedlimits ordered by the waypoints.
+
+#### Calculate speed for motion planning
+
+By default the node publishes the current speed limit.
+
+If a collision is recieved by the Collision Check the loop gets deactivated and a appropriate speed is calculated by this formula:
+
+$$
+speed_{acc} = speed_{obstacle} \cdot \frac{distance_{obstacle}}{distance_{safety}}
+$$
+
+The ACC speed depends on the obstacles speed and distance.