diff --git a/.gitignore b/.gitignore index bf3c7859..b667dead 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,5 @@ code/output # Byte-compiled / optimized / DLL files __pycache__/ + +*.tsv diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1545f945..70123763 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -26,6 +26,10 @@ services: # image: carlasim/carla:0.9.14 image: ghcr.io/una-auxme/paf23:leaderboard-2.0 init: true + deploy: + resources: + limits: + memory: 16G expose: - 2000 - 2001 @@ -57,8 +61,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..975d0b0e 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -1,4 +1,4 @@ - + @@ -28,33 +28,31 @@ - - - + + - - - + + - - + + + + + + diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py new file mode 100755 index 00000000..d2556604 --- /dev/null +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -0,0 +1,422 @@ +#!/usr/bin/env python + +""" +This node provides full testability for all Acting +components by offering different testcases +to hopefully fully implement and tune Acting without +the need of working Perception and Planning components. +This also generates Lists of all the important values +to be saved in a file and plotted again. +TODO: emergency brake behavior +""" + +import math +import ros_compatibility as roscomp +import numpy as np +from nav_msgs.msg import Path +from std_msgs.msg import Float32, Bool +from geometry_msgs.msg import PoseStamped +from ros_compatibility.node import CompatibleNode +import rospy +from rospy import Publisher, Subscriber +from carla_msgs.msg import CarlaSpeedometer, CarlaEgoVehicleControl + +from trajectory_interpolation import interpolate_route + +# since this dummy is supposed to test everything ACTING, +# Testers can choose which test to run from changing this Constant + +# TEST_TYPE to choose which kind of Test to run: +# 0: Test Velocity Controller with constant one velocity +# const. velocity = TARGET_VELOCITY_1 +# const. steering = 0 +# no trajectory +# TURN OFF stanley and PP Controllers in acting.launch! + +# 1: Test Velocity Controller with changing velocity +# velocity = alternate all 20 secs: TARGET_VELOCITY_1/_HIGH +# const. steering = 0 +# no trajectory +# TURN OFF stanley and PP Controllers in acting.launch! + +# 2: Test Steering Controller on chooseable trajectory +# velocity = TARGET_VELOCITY_1 TODO: maybe use velocity publisher? +# steering = STEERING_CONTROLLER_USED (see below) +# trajectory = TRAJECTORY_TYPE (see below) + +# 3: Test Emergency Breaks on TestType 1 +# const velocity = TARGET_VELOCITY_1 +# const steering = 0 +# no trajectory +# Triggers emergency break after 15 Seconds +# TODO implement evaluation etc. + +# 4: Test Steering-PID in vehicleController +# TODO TODO +TEST_TYPE = 2 # aka. TT + +STEERING: float = 0.0 # for TT0: steering -> always straight +TARGET_VELOCITY_1: float = 7 # for TT0/TT1: low velocity +TARGET_VELOCITY_2: float = 0 # for TT1: high velocity + +STEERING_CONTROLLER_USED = 1 # for TT2: 0 = both ; 1 = PP ; 2 = Stanley +TRAJECTORY_TYPE = 0 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve + + +class Acting_Debugger(CompatibleNode): + """ + Creates a node with testability for all acting components + without the need of working/running perception or planning. + """ + + def __init__(self): + """ + Constructor of the class + :return: + """ + super(Acting_Debugger, self).__init__('dummy_trajectory_pub') + self.loginfo('Acting_Debugger node started') + self.role_name = self.get_param('role_name', 'ego_vehicle') + self.control_loop_rate = self.get_param('control_loop_rate', 0.05) + + # Publisher for Dummy Trajectory + self.trajectory_pub: Publisher = self.new_publisher( + Path, + "/paf/" + self.role_name + "/trajectory", + qos_profile=1) + + # Publisher for Dummy Velocity + self.velocity_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/target_velocity", + qos_profile=1) + + # Stanley: Publisher for Dummy Stanley-Steer + self.stanley_steer_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/stanley_steer", + qos_profile=1) + + # PurePursuit: Publisher for Dummy PP-Steer + self.pure_pursuit_steer_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/pure_pursuit_steer", + qos_profile=1) + + # Publisher for Steeringcontrollers selector to test separately + # Subscribed to in vehicle controller + self.controller_selector_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/controller_selector_debug", + qos_profile=1) + + # Subscriber of current_pos, used for TODO nothing yet + self.current_pos_sub: Subscriber = self.new_subscription( + msg_type=PoseStamped, + topic="/paf/" + self.role_name + "/current_pos", + callback=self.__current_position_callback, + qos_profile=1) + + # ---> EVALUATION/TUNING: Subscribers for plotting + # Subscriber for target_velocity for plotting + self.target_velocity_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/target_velocity", + self.__get_target_velocity, + qos_profile=1) + + # Subscriber for current_velocity for plotting + self.current_velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + + # Subscriber for current_throttle for plotting + self.current_throttle_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/throttle", + self.__get_throttle, + qos_profile=1) + + # Subscriber for PurePursuit_steer + self.pure_pursuit_steer_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/pure_pursuit_steer", + self.__get_purepursuit_steer, + qos_profile=1) + + # Subscriber for Stanley_Steer + self.stanley_steer_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/stanley_steer", + self.__get_stanley_steer, + qos_profile=1) + + # Subscriber for Stanley_Steer + self.vehicle_steer_sub: Subscriber = self.new_subscription( + CarlaEgoVehicleControl, + f'/carla/{self.role_name}/vehicle_control_cmd', + self.__get_vehicle_steer, + qos_profile=10) + + # Publisher for emergency message TODO: should VC really trigger this? + self.emergency_pub: Publisher = self.new_publisher( + Bool, + f"/paf/{self.role_name}/emergency", + qos_profile=1) + + # Initialize all needed "global" variables here + self.current_trajectory = [] + self.switchVelocity = False + self.driveVel = TARGET_VELOCITY_1 + + self.switch_checkpoint_time = rospy.get_time() + self.switch_time_set = False + + self.checkpoint_time = rospy.get_time() + self.time_set = False + + self.__current_velocities = [] + self.__max_velocities = [] + self.__throttles = [] + + self.__purepursuit_steers = [] + self.__stanley_steers = [] + self.__vehicle_steers = [] + + self.path_msg = Path() + self.path_msg.header.stamp = rospy.Time.now() + self.path_msg.header.frame_id = "global" + + # Generate Trajectory as selected in TRAJECTORY_TYPE + # Spawncoords at the simulationstart TODO: get from position + startx = 984.5 + starty = -5442.0 + + if (TRAJECTORY_TYPE == 0): # Straight trajectory + self.current_trajectory = [ + (startx, starty), + (startx, starty-200) + ] + self.updated_trajectory(self.current_trajectory) + + elif (TRAJECTORY_TYPE == 1): # Sinewave Serpentines trajectory + # Generate a sine-wave with the global Constants to + # automatically generate a trajectory with serpentine waves + cycles = 4 # how many sine cycles + resolution = 50 # how many datapoints to generate + + length = np.pi * 2 * cycles + step = length / resolution # spacing between values + my_wave = np.sin(np.arange(0, length, step)) + x_wave = 2 * my_wave # to have a serpentine line with +/- 2 meters + # to have the serpentine line drive around the middle + # of the road/start point of the car + x_wave += startx + traj_y = starty + # starting point in the middle of the road, + # sine wave swings around this later + trajectory_wave = [(startx, traj_y)] + for traj_x in x_wave: + traj_y -= 2 + trajectory_wave.append((traj_x, traj_y)) + # back to the middle of the road + trajectory_wave.append((startx, traj_y-2)) + # add a long straight path after the serpentines + trajectory_wave.append((startx, starty-200)) + self.current_trajectory = trajectory_wave + self.updated_trajectory(self.current_trajectory) + + elif (TRAJECTORY_TYPE == 2): # straight into 90° Curve + self.current_trajectory = [ + (986.0, -5442.0), + (986.0, -5463.2), + (984.5, -5493.2), + + (984.5, -5563.5), + (985.0, -5573.2), + (986.3, -5576.5), + (987.3, -5578.5), + (988.7, -5579.0), + (990.5, -5579.8), + (1000.0, -5580.2), + + (1040.0, -5580.0), + (1070.0, -5580.0), + (1080.0, -5582.0), + (1090.0, -5582.0), + (1100.0, -5580.0), + (1110.0, -5578.0), + (1120.0, -5578.0), + (1130.0, -5580.0), + (1464.6, -5580.0), + (1664.6, -5580.0) + ] + self.updated_trajectory(self.current_trajectory) + + def updated_trajectory(self, target_trajectory): + """ + Updates the published Path message with the new target trajectory. + :param: target_trajectory: the new target trajectory to be published + :return: + """ + self.current_trajectory = interpolate_route(target_trajectory, 0.25) + self.path_msg.header.stamp = rospy.Time.now() + self.path_msg.header.frame_id = "global" + # clear old waypoints + self.path_msg.poses.clear() + for wp in self.current_trajectory: + pos = PoseStamped() + pos.header.stamp = rospy.Time.now() + pos.header.frame_id = "global" + pos.pose.position.x = wp[0] + pos.pose.position.y = wp[1] + pos.pose.position.z = 37.6 # why?? + # currently not used therefore zeros + pos.pose.orientation.x = 0 + pos.pose.orientation.y = 0 + pos.pose.orientation.z = 0 + pos.pose.orientation.w = 0 + self.path_msg.poses.append(pos) + + def __current_position_callback(self, data: PoseStamped): + agent = data.pose.position + self.x = agent.x + self.y = agent.y + self.z = agent.z + # TODO use this to get spawnpoint? necessary? + + def __get_target_velocity(self, data: Float32): + self.__max_velocities.append(float(data.data)) + + def __get_current_velocity(self, data: CarlaSpeedometer): + self.__current_velocities.append(float(data.speed)) + + def __get_throttle(self, data: Float32): + self.__throttles.append(float(data.data)) + + def __get_stanley_steer(self, data: Float32): + self.__stanley_steers.append(float(data.data)) + + def __get_purepursuit_steer(self, data: Float32): + r = 1 / (math.pi / 2) + steering_float = float(data.data) * r + self.__purepursuit_steers.append(steering_float) + + def __get_vehicle_steer(self, data: CarlaEgoVehicleControl): + self.__vehicle_steers.append(float(data.steer)) + + def run(self): + """ + Control loop + :return: + """ + self.checkpoint_time = rospy.get_time() + + def loop(timer_event=None): + """ + Publishes different speeds, trajectories ... + depending on the selected TEST_TYPE + """ + # Drive const. velocity on fixed straight steering + if (TEST_TYPE == 0): + self.drive_Vel = TARGET_VELOCITY_1 + self.stanley_steer_pub.publish(STEERING) + self.pure_pursuit_steer_pub.publish(STEERING) + self.velocity_pub.publish(self.driveVel) + + # Drive alternating velocities on fixed straight steering + elif (TEST_TYPE == 1): + if not self.time_set: + self.drive_Vel = TARGET_VELOCITY_1 + self.switch_checkpoint_time = rospy.get_time() + self.switch_time_set = True + if (self.switch_checkpoint_time < rospy.get_time() - 10): + self.switch_checkpoint_time = rospy.get_time() + self.switchVelocity = not self.switchVelocity + if (self.switchVelocity): + self.driveVel = TARGET_VELOCITY_2 + else: + self.driveVel = TARGET_VELOCITY_1 + self.stanley_steer_pub.publish(STEERING) + self.pure_pursuit_steer_pub.publish(STEERING) + self.velocity_pub.publish(self.driveVel) + + # drive const. velocity on trajectoy with steering controller + elif (TEST_TYPE == 2): + # Continuously update path and publish it + self.drive_Vel = TARGET_VELOCITY_1 + self.updated_trajectory(self.current_trajectory) + self.trajectory_pub.publish(self.path_msg) + self.velocity_pub.publish(self.driveVel) + + # drive const. velocity on fixed straight steering and + # trigger an emergency brake after 15 secs + elif (TEST_TYPE == 3): + # Continuously update path and publish it + self.drive_Vel = TARGET_VELOCITY_1 + if not self.time_set: + self.checkpoint_time = rospy.get_time() + self.time_set = True + if (self.checkpoint_time < rospy.get_time() - 15.0): + self.checkpoint_time = rospy.get_time() + self.emergency_pub.publish(True) + self.stanley_steer_pub.publish(STEERING) + self.pure_pursuit_steer_pub.publish(STEERING) + self.velocity_pub.publish(self.driveVel) + + # drive const. velocity and follow trajectory by + # publishing self-calculated steering + elif (TEST_TYPE == 4): + self.drive_Vel = TARGET_VELOCITY_1 + steer = self.calculate_steer() + self.stanley_steer_pub.publish(steer) + self.pure_pursuit_steer_pub.publish(steer) + + if (STEERING_CONTROLLER_USED == 1): + self.controller_selector_pub.publish(1) + elif (STEERING_CONTROLLER_USED == 2): + self.controller_selector_pub.publish(2) + + # set starttime to when simulation is actually starting to run + # to really get 10 secs plots every time + if not self.time_set: + self.checkpoint_time = rospy.get_time() + self.time_set = True + + # Uncomment the prints of the data you want to plot + if (self.checkpoint_time < rospy.get_time() - 10.0): + self.checkpoint_time = rospy.get_time() + print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") + # print(self.__max_velocities) + # print(self.__current_velocities) + # print(self.__throttles) + print(self.__purepursuit_steers) + print(self.__stanley_steers) + print(self.__vehicle_steers) + print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") + + self.new_timer(self.control_loop_rate, loop) + self.spin() + + +def main(args=None): + """ + main function + :param args: + :return: + """ + + roscomp.init("Acting_Debugger", args=args) + try: + node = Acting_Debugger() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() + + +if __name__ == "__main__": + main() diff --git a/code/acting/src/acting/DummyTrajectoryPublisher.py b/code/acting/src/acting/DummyTrajectoryPublisher.py deleted file mode 100755 index 61a8e8ae..00000000 --- a/code/acting/src/acting/DummyTrajectoryPublisher.py +++ /dev/null @@ -1,134 +0,0 @@ -#!/usr/bin/env python - -""" -This node publishes a dummy trajectory between predefined points. -""" - -import ros_compatibility as roscomp -from nav_msgs.msg import Path -from geometry_msgs.msg import PoseStamped -from ros_compatibility.node import CompatibleNode -from trajectory_interpolation import interpolate_route -import rospy - - -class DummyTrajectoryPub(CompatibleNode): - """ - Creates a node that publishes an interpolated trajectory between - predefined points as a nav_msgs/Path message. - """ - - def __init__(self): - """ - Constructor - :return: - """ - super(DummyTrajectoryPub, self).__init__('dummy_trajectory_pub') - self.loginfo('DummyTrajectoryPub node started') - # basic info - self.role_name = self.get_param('role_name', 'ego_vehicle') - self.control_loop_rate = self.get_param('control_loop_rate', 0.05) - - self.current_trajectory = [] - self.path_msg = Path() - self.path_msg.header.stamp = rospy.Time.now() - self.path_msg.header.frame_id = "global" - - # Static trajectory for testing purposes - self.initial_trajectory = [ - (986.0, -5442.0), - (986.0, -5463.2), - (984.5, -5493.2), - - (984.5, -5563.5), - (985.0, -5573.2), - (986.3, -5576.5), - (987.3, -5578.5), - (988.7, -5579.0), - (990.5, -5579.8), - (1000.0, -5580.2), - - (1040.0, -5580.0), - (1070.0, -5580.0), - (1080.0, -5582.0), - (1090.0, -5582.0), - (1100.0, -5580.0), - (1110.0, -5578.0), - (1120.0, -5578.0), - (1130.0, -5580.0), - (1464.6, -5580.0), - (1664.6, -5580.0) - ] - - self.updated_trajectory(self.initial_trajectory) - - # publisher for the current trajectory - self.trajectory_publisher = self.new_publisher( - Path, - "/paf/" + self.role_name + "/trajectory", - qos_profile=1) - - def updated_trajectory(self, target_trajectory): - """ - Updates the published Path message with the new target trajectory. - :param: target_trajectory: the new target trajectory to be published - :return: - """ - self.current_trajectory = interpolate_route(target_trajectory, 0.25) - self.path_msg.header.stamp = rospy.Time.now() - self.path_msg.header.frame_id = "global" - - # clear old waypoints - self.path_msg.poses.clear() - - for wp in self.current_trajectory: - pos = PoseStamped() - pos.header.stamp = rospy.Time.now() - pos.header.frame_id = "global" - - pos.pose.position.x = wp[0] - pos.pose.position.y = wp[1] - pos.pose.position.z = 37.6 - - # currently not used therefore zeros - pos.pose.orientation.x = 0 - pos.pose.orientation.y = 0 - pos.pose.orientation.z = 0 - pos.pose.orientation.w = 0 - - self.path_msg.poses.append(pos) - - def run(self): - """ - Control loop - :return: - """ - - def loop(timer_event=None): - # Continuously update path - self.updated_trajectory(self.initial_trajectory) - self.trajectory_publisher.publish(self.path_msg) - - self.new_timer(self.control_loop_rate, loop) - self.spin() - - -def main(args=None): - """ - main function - :param args: - :return: - """ - - roscomp.init("dummy_trajectory_pub", args=args) - try: - node = DummyTrajectoryPub() - node.run() - except KeyboardInterrupt: - pass - finally: - roscomp.shutdown() - - -if __name__ == "__main__": - main() diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index 076c2c52..65e3481a 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -10,11 +10,16 @@ from rospy import Publisher, Subscriber from std_msgs.msg import Float32 from acting.msg import Debug +import rospy +# import numpy as np from helper_functions import vector_angle from trajectory_interpolation import points_to_vector MIN_LD_V: float = 3.0 +LOOK_AHEAD_DIS = 3 +MIN_L_A_DIS = 1 +MAX_L_A_DIS = 15 class PurePursuitController(CompatibleNode): @@ -65,6 +70,16 @@ def __init__(self): f"/paf/{self.role_name}/pure_p_debug", qos_profile=1) + self.targetwp_publisher: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/current_target_wp", + qos_profile=1) + + self.currentx_publisher: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/current_x", + qos_profile=1) + self.__position: (float, float) = None # x, y self.__last_pos: (float, float) = None self.__path: Path = None @@ -73,6 +88,10 @@ def __init__(self): self.__tp_idx: int = 0 # target waypoint index # error when there are no targets + self.time_set = False + self.checker = False + self.checkpoint_time = -1 + def run(self): """ Starts the main loop of the node @@ -112,6 +131,70 @@ def loop(timer_event=None): self.new_timer(self.control_loop_rate, loop) self.spin() + def __calculate_steer(self) -> float: + """ + Calculates the steering angle based on the current information + :return: + """ + l_vehicle = 2.85 # wheelbase + k_ld = 0.1 # TODO: tune + look_ahead_dist = LOOK_AHEAD_DIS # offset so that ld is never zero + + if self.__velocity < 0: + # backwards driving is not supported, TODO why check this here? + return 0.0 + elif round(self.__velocity, 1) < MIN_LD_V: + # Offset for low velocity state + look_ahead_dist += 0.0 # no offset + else: + look_ahead_dist += k_ld * (self.__velocity - MIN_LD_V) + + # look_ahead_dist = np.clip(k_ld * self.__velocity, + # MIN_L_A_DIS, MAX_L_A_DIS) + + # Get the target position on the trajectory in look_ahead distance + self.__tp_idx = self.__get_target_point_index(look_ahead_dist) + target_wp: PoseStamped = self.__path.poses[self.__tp_idx] + # Get the vector from the current position to the target position + target_v_x, target_v_y = points_to_vector((self.__position[0], + self.__position[1]), + (target_wp.pose.position.x, + target_wp.pose.position.y)) + # Get the target heading from that vector + target_vector_heading = vector_angle(target_v_x, target_v_y) + # Get the error between current heading and target heading + alpha = target_vector_heading - self.__heading + # Steering_angle is arctan (l_vehicle / R) + # R is look_ahead_dist / 2 * sin(alpha) + # https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html + + steering_angle = atan((2 * l_vehicle * sin(alpha)) / look_ahead_dist) + + # for debugging -> + debug_msg = Debug() + debug_msg.heading = self.__heading + debug_msg.target_heading = target_vector_heading + debug_msg.l_distance = look_ahead_dist + debug_msg.steering_angle = steering_angle + self.debug_publisher.publish(debug_msg) + # <- + self.pure_pursuit_steer_target_pub.publish(target_wp.pose) + + # not beautiful but works, eliminates the first + # second because for some reason + # the positional data from the GNSS is completely broken at the start + if not self.time_set: + self.checkpoint_time = rospy.get_time() + self.time_set = True + if not self.checker and (self.checkpoint_time < rospy.get_time() - 1): + self.checker = True + + if self.checker: + self.targetwp_publisher.publish((target_wp.pose.position.x-984.5)) + self.currentx_publisher.publish(self.__position[0]-984.5) + + return steering_angle + def __set_position(self, data: PoseStamped, min_diff=0.001): """ Updates the current position of the vehicle @@ -122,6 +205,7 @@ def __set_position(self, data: PoseStamped, min_diff=0.001): the new point to be accepted :return: """ + # No position yet: always get the published position if self.__position is None: x0 = data.pose.position.x y0 = data.pose.position.y @@ -131,12 +215,14 @@ def __set_position(self, data: PoseStamped, min_diff=0.001): # check if the new position is valid dist = self.__dist_to(data.pose.position) if dist < min_diff: + # if new position is to close to current, do not accept it + # too close = closer than min_diff = 0.001 meters # for debugging purposes: self.logdebug("New position disregarded, " f"as dist ({round(dist, 3)}) to current pos " f"< min_diff ({round(min_diff, 3)})") return - + # TODO: why save the old position if it is never used again? old_x = self.__position[0] old_y = self.__position[1] self.__last_pos = (old_x, old_y) @@ -161,51 +247,6 @@ def __set_heading(self, data: Float32): def __set_velocity(self, data: CarlaSpeedometer): self.__velocity = data.speed - def __calculate_steer(self) -> float: - """ - Calculates the steering angle based on the current information - :return: - """ - l_vehicle = 2.85 # wheelbase - k_ld = 1.0 - look_ahead_dist = 3.5 # offset so that ld is never zero - - if self.__velocity < 0: - # backwards driving is not supported - return 0.0 - elif round(self.__velocity, 1) < MIN_LD_V: - # Offset for low velocity state - look_ahead_dist += 0.0 # no offset - else: - look_ahead_dist += k_ld * (self.__velocity - MIN_LD_V) - - self.__tp_idx = self.__get_target_point_index(look_ahead_dist) - - target_wp: PoseStamped = self.__path.poses[self.__tp_idx] - - target_v_x, target_v_y = points_to_vector((self.__position[0], - self.__position[1]), - (target_wp.pose.position.x, - target_wp.pose.position.y)) - - target_vector_heading = vector_angle(target_v_x, target_v_y) - - alpha = target_vector_heading - self.__heading - steering_angle = atan((2 * l_vehicle * sin(alpha)) / look_ahead_dist) - - # for debugging -> - debug_msg = Debug() - debug_msg.heading = self.__heading - debug_msg.target_heading = target_vector_heading - debug_msg.l_distance = look_ahead_dist - debug_msg.steering_angle = steering_angle - self.debug_publisher.publish(debug_msg) - # <- - - self.pure_pursuit_steer_target_pub.publish(target_wp.pose) - - return steering_angle - def __get_target_point_index(self, ld: float) -> int: """ Get the index of the target point on the current trajectory based on @@ -213,9 +254,11 @@ def __get_target_point_index(self, ld: float) -> int: :param ld: look ahead distance :return: """ + # if path has less than 2 poses, break if len(self.__path.poses) < 2: return -1 + # initialize min dist and idx very high and -1 min_dist = 10e1000 min_dist_idx = -1 # might be more elegant to only look at points diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py index 20721bcf..4d892cc5 100755 --- a/code/acting/src/acting/stanley_controller.py +++ b/code/acting/src/acting/stanley_controller.py @@ -24,7 +24,7 @@ def __init__(self): self.control_loop_rate = self.get_param('control_loop_rate', 0.05) self.role_name = self.get_param('role_name', 'ego_vehicle') - # Subscriber + # Subscribers self.position_sub: Subscriber = self.new_subscription( Path, f"/paf/{self.role_name}/trajectory", @@ -49,7 +49,7 @@ def __init__(self): self.__set_heading, qos_profile=1) - # Publisher + # Publishers self.stanley_steer_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/stanley_steer", @@ -60,8 +60,21 @@ def __init__(self): f"/paf/{self.role_name}/stanley_debug", qos_profile=1) - self.__position: (float, float) = None # x, y - self.__last_pos: (float, float) = None + # Publishers for debugging StanleyController soon + # TODO: only works with perfect current_pos + # publish the target and the current position + self.targetwp_publisher: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/current_target_wp", + qos_profile=1) + + self.currentx_publisher: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/current_x", + qos_profile=1) + + self.__position: (float, float) = None # x , y + self.__last_pos: (float, float) = None # x , y self.__path: Path = None self.__heading: float = None self.__velocity: float = None @@ -153,10 +166,10 @@ def __set_velocity(self, data: CarlaSpeedometer): def __calculate_steer(self) -> float: """ Calculates the steering angle based on the current information - using the Stanly algorithm + using the Stanley algorithm :return: steering angle """ - k_ce = 0.10 # todo: tune + k_ce = 0.10 # TODO: tune k_v = 1.0 current_velocity: float @@ -177,7 +190,7 @@ def __calculate_steer(self) -> float: current_velocity * k_v) steering_angle *= - 1 - # for debugging -> + # for debugging TODO: currently not that useful-> debug_msg = StanleyDebug() debug_msg.heading = self.__heading debug_msg.path_heading = traj_heading @@ -186,7 +199,9 @@ def __calculate_steer(self) -> float: debug_msg.steering_angle = steering_angle self.debug_publisher.publish(debug_msg) # <- - + # 2 more debugging messages: TODO: maybe put into DEBUGGER NODE? + self.targetwp_publisher.publish((closest_point.pose.position.x-984.5)) + self.currentx_publisher.publish(self.__position[0]-984.5) return steering_angle def __get_closest_point_index(self) -> int: diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 5ba38f8b..314e5bcd 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -30,15 +30,17 @@ class VehicleController(CompatibleNode): def __init__(self): super(VehicleController, self).__init__('vehicle_controller') self.loginfo('VehicleController node started') - self.control_loop_rate = self.get_param('control_loop_rate', 0.05) self.role_name = self.get_param('role_name', 'ego_vehicle') + # Publisher for Carla Vehicle Control Commands self.control_publisher: Publisher = self.new_publisher( CarlaEgoVehicleControl, f'/carla/{self.role_name}/vehicle_control_cmd', qos_profile=10 ) + + # Publisher for Status TODO: Where needed? Why carla? self.status_pub: Publisher = self.new_publisher( Bool, f"/carla/{self.role_name}/status", @@ -47,6 +49,8 @@ def __init__(self): durability=DurabilityPolicy.TRANSIENT_LOCAL) ) + # Publisher for which steering-controller is mainly used + # 1 = PurePursuit and 2 = Stanley TODO: needed? self.controller_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/controller", @@ -54,13 +58,14 @@ def __init__(self): durability=DurabilityPolicy.TRANSIENT_LOCAL) ) + # Publisher for emergency message TODO: should VC really trigger this? self.emergency_pub: Publisher = self.new_publisher( Bool, f"/paf/{self.role_name}/emergency", qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) ) - + # Subscriber for emergency TODO: who can trigger this, what happens? self.emergency_sub: Subscriber = self.new_subscription( Bool, f"/paf/{self.role_name}/emergency", @@ -93,12 +98,42 @@ def __init__(self): self.__set_stanley_steer, qos_profile=1) + # Testing / Debugging --> + self.brake_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/brake", + self.__set_brake, + qos_profile=1) + + self.target_steering_publisher: Publisher = self.new_publisher( + Float32, + f'/paf/{self.role_name}/target_steering_debug', + qos_profile=1) + + self.pidpoint_publisher: Publisher = self.new_publisher( + Float32, + f'/paf/{self.role_name}/pid_point_debug', + qos_profile=1) + + self.controller_selector_sub: Subscriber = self.new_subscription( + Float32, + f'/paf/{self.role_name}/controller_selector_debug', + self.__set_controller, + qos_profile=1) + # <-- Testing / Debugging + self.__emergency: bool = False self.__throttle: float = 0.0 self.__velocity: float = 0.0 self.__pure_pursuit_steer: float = 0.0 self.__stanley_steer: float = 0.0 - self.__current_steer: float = 0.0 # todo: check emergency behaviour + self.__current_steer: float = 0.0 + + self.__brake: float = 0.0 + + self.controller_testing: bool = False + self.controller_selected_debug: int = 1 + # TODO: check emergency behaviour def run(self): """ @@ -107,7 +142,9 @@ def run(self): """ self.status_pub.publish(True) self.loginfo('VehicleController node running') - pid = PID(0.5, 0.1, 0.1, setpoint=0) + # currently pid for steering is not used, needs fixing + pid = PID(0.5, 0.001, 0) # PID(0.5, 0.1, 0.1, setpoint=0) + # TODO: TUNE AND FIX? pid.output_limits = (-MAX_STEER_ANGLE, MAX_STEER_ANGLE) def loop(timer_event=None) -> None: @@ -119,7 +156,14 @@ def loop(timer_event=None) -> None: if self.__emergency: # emergency is already handled in # __emergency_break() return - p_stanley = self.__choose_controller() + if (self.controller_testing): + if (self.controller_selected_debug == 2): + p_stanley = 1 + elif (self.controller_selected_debug == 1): + p_stanley = 0 + else: + p_stanley = self.__choose_controller() + if p_stanley < 0.5: self.logdebug('Using PURE_PURSUIT_CONTROLLER') self.controller_pub.publish(float(PURE_PURSUIT_CONTROLLER)) @@ -131,19 +175,25 @@ def loop(timer_event=None) -> None: f_pure_p = (1 - p_stanley) * self.__pure_pursuit_steer steer = f_stanley + f_pure_p + # only use pure_pursuit controller for now, since + # stanley seems broken with the new heading-bug + # TODO: swap back if stanley is fixed + steer = self.__pure_pursuit_steer + + self.target_steering_publisher.publish(steer) # debugging + message = CarlaEgoVehicleControl() message.reverse = False - if self.__throttle > 0: # todo: driving backwards? - message.brake = 0 - message.throttle = self.__throttle - else: - message.throttle = 0 - message.brake = abs(self.__throttle) - + message.throttle = self.__throttle + message.brake = self.__brake message.hand_brake = False message.manual_gear_shift = False - pid.setpoint = self.__map_steering(steer) - message.steer = pid(self.__current_steer) + # sets target_steer to steer + # pid.setpoint = self.__map_steering(steer) + message.steer = self.__map_steering(steer) + # TEST pure steering: message.steer = self.__map_steering(steer) + # Original Code: + # message.steer = pid(self.__current_steer) message.gear = 1 message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) @@ -156,12 +206,15 @@ def __map_steering(self, steering_angle: float) -> float: """ Takes the steering angle calculated by the controller and maps it to the available steering angle + This is from (left, right): [pi/2 , -pi/2] to [-1, 1] :param steering_angle: calculated by a controller in [-pi/2 , pi/2] :return: float for steering in [-1, 1] """ - tune_k = -5 # factor for tuning todo: tune + tune_k = 1 # factor for tuning TODO: tune but why? + # negative because carla steer and our steering controllers are flipped r = 1 / (math.pi / 2) steering_float = steering_angle * r * tune_k + self.pidpoint_publisher.publish(steering_float) return steering_float def __emergency_break(self, data) -> None: @@ -219,12 +272,19 @@ def __get_velocity(self, data: CarlaSpeedometer) -> None: def __set_throttle(self, data): self.__throttle = data.data + def __set_brake(self, data): + self.__brake = data.data + def __set_pure_pursuit_steer(self, data: Float32): self.__pure_pursuit_steer = data.data def __set_stanley_steer(self, data: Float32): self.__stanley_steer = data.data + def __set_controller(self, data: Float32): + self.controller_testing = True + self.controller_selected_debug = data.data + def sigmoid(self, x: float): """ Evaluates the sigmoid function s(x) = 1 / (1+e^-25x) diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 39882333..754cbdc2 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -1,15 +1,14 @@ #!/usr/bin/env python import ros_compatibility as roscomp from carla_msgs.msg import CarlaSpeedometer -from geometry_msgs.msg import PoseStamped from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber from simple_pid import PID -from std_msgs.msg import Float32, Float32MultiArray +from std_msgs.msg import Float32 from nav_msgs.msg import Path # TODO put back to 36 when controller can handle it -SPEED_LIMIT_DEFAULT: float = 6 # 36.0 +SPEED_LIMIT_DEFAULT: float = 7 # 36.0 class VelocityController(CompatibleNode): @@ -27,10 +26,10 @@ def __init__(self): self.control_loop_rate = self.get_param('control_loop_rate', 0.05) self.role_name = self.get_param('role_name', 'ego_vehicle') - self.max_velocity_sub: Subscriber = self.new_subscription( + self.target_velocity_sub: Subscriber = self.new_subscription( Float32, - f"/paf/{self.role_name}/max_velocity", - self.__get_max_velocity, + f"/paf/{self.role_name}/target_velocity", + self.__get_target_velocity, qos_profile=1) self.velocity_sub: Subscriber = self.new_subscription( @@ -39,27 +38,14 @@ def __init__(self): self.__get_current_velocity, qos_profile=1) - self.speed_limit_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/speed_limit", - qos_profile=1) - - self.max_tree_v_sub: Subscriber = self.new_subscription( - Float32, - f"/paf/{self.role_name}/max_tree_velocity", - self.__set_max_tree_v, - qos_profile=1) - self.throttle_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/throttle", qos_profile=1) - # rqt_plot can't read the speed data provided by the rosbridge - # Therefore, the speed is published again as a float value - self.velocity_pub: Publisher = self.new_publisher( + self.brake_pub: Publisher = self.new_publisher( Float32, - f"/paf/{self.role_name}/velocity_as_float", + f"/paf/{self.role_name}/brake", qos_profile=1) # needed to prevent the car from driving before a path to follow is @@ -70,34 +56,24 @@ def __init__(self): self.__set_trajectory, qos_profile=1) - # TODO: does this replace paf/hero/speed_limit? - 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) - - self.current_pos_sub: Subscriber = self.new_subscription( - msg_type=PoseStamped, - topic="/paf/" + self.role_name + "/current_pos", - callback=self.__current_position_callback, - qos_profile=1) - self.__current_velocity: float = None - self.__max_velocity: float = None - self.__max_tree_v: float = None - self.__speed_limit: float = None + self.__target_velocity: float = None self.__trajectory: Path = None - self.__speed_limits_OD: [float] = [] - self.__current_wp_index: int = 0 def run(self): """ Starts the main loop of the node :return: """ - self.loginfo('VehicleController node running') - pid = PID(0.25, 0, 0.1) # values from paf21-2 todo: tune + self.loginfo('VelocityController node running') + # PID for throttle + pid_t = PID(0.60, 0.00076, 0.63) + pid_t.output_limits = (-1.0, 1.0) + # new PID for braking, much weaker than throttle controller! + # pid_b = PID(-0.1, -0, -0) # TODO tune? BUT current P can be good + # Kp just says "brake fully(1) until you are only Kp*speedError faster" + # so with Kp = -1.35 -> the actual braking range is hardly used + # pid_b.output_limits = (.0, 1.0) def loop(timer_event=None): """ @@ -107,48 +83,45 @@ def loop(timer_event=None): :param timer_event: Timer event from ROS :return: """ - if self.__max_velocity is None: - self.logdebug("VehicleController hasn't received max_velocity" - " yet. max_velocity has been set to" + if self.__target_velocity is None: + self.logdebug("VelocityController hasn't received target" + "_velocity yet. target_velocity has been set to" f"default value {SPEED_LIMIT_DEFAULT}") - # return - self.__max_velocity = SPEED_LIMIT_DEFAULT + self.__target_velocity = SPEED_LIMIT_DEFAULT if self.__current_velocity is None: - self.logdebug("VehicleController hasn't received " + self.logdebug("VelocityController hasn't received " "current_velocity yet and can therefore not" "publish a throttle value") return if self.__trajectory is None: - self.logdebug("VehicleController hasn't received " + self.logdebug("VelocityController hasn't received " "trajectory yet and can therefore not" "publish a throttle value (to prevent stupid)") return - if self.__speed_limit is None or self.__speed_limit < 0: - self.logdebug("VelocityController hasn't received a acceptable" - " speed_limit yet. speed_limit has been set to" - f"default value {SPEED_LIMIT_DEFAULT}") - self.__speed_limit = SPEED_LIMIT_DEFAULT - - if self.__max_tree_v is None or self.__max_tree_v < 0: - self.logdebug("VelocityController hasn't received a acceptable" - " max_tree_v yet. speed_limit has been set to" - f"default value {SPEED_LIMIT_DEFAULT}") - self.__max_tree_v = SPEED_LIMIT_DEFAULT - - if self.__max_velocity < 0: - self.logerr("Velocity controller doesn't support backward " + if self.__target_velocity < 0: + self.logerr("VelocityController doesn't support backward " "driving yet.") return - v = min(self.__max_velocity, self.__max_tree_v) - v = min(v, self.__speed_limit) - pid.setpoint = v - throttle = pid(self.__current_velocity) - throttle = max(throttle, 0) # ensures that throttle >= 0 - throttle = min(throttle, 1.0) # ensures that throttle <= 1 + v = self.__target_velocity + + pid_t.setpoint = v + throttle = pid_t(self.__current_velocity) + + # pid_b.setpoint = v + # brake = pid_b(self.__current_velocity) + + # use negative throttles as brake inputs, works OK + if throttle < 0: + brake = abs(throttle) + throttle = 0 + else: + brake = 0 + + self.brake_pub.publish(brake) self.throttle_pub.publish(throttle) self.new_timer(self.control_loop_rate, loop) @@ -156,43 +129,13 @@ def loop(timer_event=None): def __get_current_velocity(self, data: CarlaSpeedometer): self.__current_velocity = float(data.speed) - self.velocity_pub.publish(self.__current_velocity) - - def __set_max_tree_v(self, data: Float32): - self.__max_tree_v = float(data.data) - def __get_max_velocity(self, data: Float32): - self.__max_velocity = float(data.data) - - def __get_speed_limit(self, data: Float32): - self.__speed_limit = float(data.data) + def __get_target_velocity(self, data: Float32): + self.__target_velocity = float(data.data) def __set_trajectory(self, data: Path): self.__trajectory = data - def __set_speed_limits_opendrive(self, data: Float32MultiArray): - self.__speed_limits_OD = data.data - - def __current_position_callback(self, data: PoseStamped): - 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] - self.speed_limit_pub.publish(self.__speed_limit) - def main(args=None): """ diff --git a/code/agent/launch/agent.launch b/code/agent/launch/agent.launch index 4291717f..d076596d 100644 --- a/code/agent/launch/agent.launch +++ b/code/agent/launch/agent.launch @@ -7,7 +7,7 @@ - + diff --git a/code/agent/launch/agent_manual.launch b/code/agent/launch/agent_manual.launch new file mode 100644 index 00000000..32e33eb6 --- /dev/null +++ b/code/agent/launch/agent_manual.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + diff --git a/code/agent/src/agent/agent.py b/code/agent/src/agent/agent.py index e2732f00..ad8d076c 100755 --- a/code/agent/src/agent/agent.py +++ b/code/agent/src/agent/agent.py @@ -26,7 +26,7 @@ def sensors(self): 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, 'width': 300, 'height': 200, 'fov': 100}, {'type': 'sensor.lidar.ray_cast', 'id': 'LIDAR', - 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, + 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}, {'type': 'sensor.other.radar', 'id': 'RADAR', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, diff --git a/code/perception/CMakeLists.txt b/code/perception/CMakeLists.txt index daa62be3..70e340c4 100644 --- a/code/perception/CMakeLists.txt +++ b/code/perception/CMakeLists.txt @@ -52,6 +52,8 @@ add_message_files( FILES Waypoint.msg LaneChange.msg + TrafficLightState.msg + MinDistance.msg ) ## Generate services in the 'srv' folder diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 3adc596e..300976a3 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -2,7 +2,7 @@ - + - - + + + + + + + + + - - - - - - + + + + + + - + diff --git a/code/perception/msg/MinDistance.msg b/code/perception/msg/MinDistance.msg new file mode 100644 index 00000000..a62513a7 --- /dev/null +++ b/code/perception/msg/MinDistance.msg @@ -0,0 +1 @@ +float32 distance \ No newline at end of file diff --git a/code/perception/msg/TrafficLightState.msg b/code/perception/msg/TrafficLightState.msg new file mode 100644 index 00000000..9da503af --- /dev/null +++ b/code/perception/msg/TrafficLightState.msg @@ -0,0 +1,5 @@ +int8 state +int8 GREEN=1 +int8 YELLOW=4 +int8 RED=2 +int8 UNKNOWN=0 diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 71e94d31..1228c607 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -3,7 +3,18 @@ import ros_numpy import numpy as np import lidar_filter_utility -from sensor_msgs.msg import PointCloud2, Range +from sensor_msgs.msg import PointCloud2 + +from sklearn.cluster import DBSCAN +from sklearn.preprocessing import StandardScaler +import matplotlib.pyplot as plt +from perception.msg import MinDistance +# from mpl_toolkits.mplot3d import Axes3D +# from itertools import combinations +import cv2 +from sensor_msgs.msg import Image as ImageMsg +from cv_bridge import CvBridge +from matplotlib.colors import LinearSegmentedColormap class LidarDistance(): @@ -26,7 +37,7 @@ def callback(self, data): # https://stackoverflow.com/questions/44295375/how-to-slice-a-numpy- # ndarray-made-up-of-numpy-void-numbers - bit_mask = lidar_filter_utility.bounding_box( + min_dist_bit_mask = lidar_filter_utility.bounding_box( coordinates, max_x=rospy.get_param('~max_x', np.inf), min_x=rospy.get_param('~min_x', -np.inf), @@ -36,12 +47,20 @@ def callback(self, data): min_z=rospy.get_param('~min_z', -np.inf), ) + reconstruct_bit_mask = lidar_filter_utility.bounding_box( + coordinates, + max_x=rospy.get_param('~max_x', np.inf), + min_x=rospy.get_param('~min_x', -np.inf), + min_z=rospy.get_param('~min_z', -np.inf), + ) + # Filter coordinates based in generated bit_mask - coordinates = coordinates[bit_mask] + min_dist_coordinates = coordinates[min_dist_bit_mask] + reconstruct_coordinates = coordinates[reconstruct_bit_mask] # Create pointcloud from manipulated data coordinates_manipulated = ros_numpy \ - .point_cloud2.array_to_pointcloud2(coordinates) + .point_cloud2.array_to_pointcloud2(min_dist_coordinates) coordinates_manipulated.header = data.header # Publish manipulated pointCloud2 @@ -49,26 +68,39 @@ def callback(self, data): # https://stackoverflow.com/questions/1401712/how-can-the-euclidean- # distance-be-calculated-with-numpy - coordinates_xyz = np.array( - lidar_filter_utility.remove_field_name(coordinates, 'intensity') + min_dist_coordinates_xyz = np.array( + lidar_filter_utility.remove_field_name(min_dist_coordinates, + 'intensity') .tolist() ) - distances = np.array( - [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz]) - if len(distances) > 0: - range_msg = Range() - range_msg.max_range = max(distances) - range_msg.min_range = min(distances) - range_msg.range = min(distances) + reconstruct_coordinates_xyz = np.array( + lidar_filter_utility.remove_field_name(reconstruct_coordinates, + 'intensity') + .tolist() + ) - self.pub_range.publish(range_msg) + # handle minimum distance + plot = self.plot_blob(min_dist_coordinates_xyz) + img_msg = self.bridge.cv2_to_imgmsg(plot, + encoding="passthrough") + img_msg.header = data.header + self.min_dist_img_publisher.publish(img_msg) + + # handle reconstruction of lidar points + rainbow_cloud = self.reconstruct_img_from_lidar( + reconstruct_coordinates_xyz) + img_msg = self.bridge.cv2_to_imgmsg(rainbow_cloud, + encoding="passthrough") + img_msg.header = data.header + self.rainbow_publisher.publish(img_msg) def listener(self): """ Initializes the node and it's publishers """ # run simultaneously. rospy.init_node('lidar_distance') + self.bridge = CvBridge() self.pub_pointcloud = rospy.Publisher( rospy.get_param( @@ -77,20 +109,117 @@ def listener(self): ), PointCloud2 ) - self.pub_range = rospy.Publisher( + + # publisher for the closest blob in the lidar point cloud + self.pub_min_dist = rospy.Publisher( rospy.get_param( '~range_topic', - '/carla/hero/' + rospy.get_namespace() + '_range' + '/paf/hero/Center/min_distance' ), - Range + MinDistance + ) + + # publisher for reconstructed lidar image + self.rainbow_publisher = rospy.Publisher( + rospy.get_param( + '~image_distance_topic', + '/paf/hero/Center/rainbow_image' + ), + ImageMsg + ) + + # publisher for 3d blob graph + self.min_dist_img_publisher = rospy.Publisher( + rospy.get_param( + '~image_distance_topic', + '/paf/hero/Center/min_dist_image' + ), + ImageMsg ) rospy.Subscriber(rospy.get_param('~source_topic', "/carla/hero/LIDAR"), PointCloud2, self.callback) - # spin() simply keeps python from exiting until this node is stopped rospy.spin() + def plot_blob(self, xyz_coords): + # creates a 3d graph thazt highlights blobs of points + xyz_coords_standardized = StandardScaler().fit_transform(xyz_coords) + pairwise_distances_x = np.abs(np.subtract.outer(xyz_coords[:, 0], + xyz_coords[:, 0])) + min_x_distance = np.min(pairwise_distances_x[pairwise_distances_x > 0]) + self.pub_min_dist.publish(min_x_distance) + eps = 0.2 + min_samples = 5 + dbscan = DBSCAN(eps=eps, min_samples=min_samples) + labels = dbscan.fit_predict(xyz_coords_standardized) + + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + + for label in set(labels): + if label == -1: + ax.scatter(xyz_coords[labels == label][:, 0], + xyz_coords[labels == label][:, 1], + xyz_coords[labels == label][:, 2], + c='gray', marker='o', label='Noise') + else: + ax.scatter(xyz_coords[labels == label][:, 0], + xyz_coords[labels == label][:, 1], + xyz_coords[labels == label][:, 2], + label=f'Cluster {label + 1}', s=50) + + ax.set_xlabel('X') + ax.set_ylabel('Y') + ax.set_zlabel('Z') + + fig.canvas.draw() + img = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8, sep='') + img = img.reshape(fig.canvas.get_width_height()[::-1] + (3,)) + img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + return img + + def reconstruct_img_from_lidar(self, coordinates_xyz): + # reconstruct 3d LIDAR-Data and calculate 2D Pixel + # according to Camera-World + + # intrinsic matrix for camera: + # width -> 300, height -> 200, fov -> 100 (agent.py) + im = np.identity(3) + im[0, 2] = 300 / 2.0 + im[1, 2] = 200 / 2.0 + im[0, 0] = im[1, 1] = 300 / (2.0 * np.tan(100 * np.pi / 360.0)) + + # extrinsic matrix for camera + ex = np.zeros(shape=(3, 4)) + ex[0][0] = 1 + ex[1][1] = 1 + ex[2][2] = 1 + m = np.matmul(im, ex) + + # reconstruct camera image with LIDAR-Data + img = np.zeros(shape=(200, 300), dtype=np.uint8) + for c in coordinates_xyz: + point = np.array([c[1], c[2], c[0], 1]) + pixel = np.matmul(m, point) + x, y = int(pixel[0]/pixel[2]), int(pixel[1]/pixel[2]) + if x >= 0 and x <= 300 and y >= 0 and y <= 200: + img[199-y][299-x] = 255 - c[0] + + # Rainbox color mapping to highlight distances + colors = [(0, 0, 0)] + [(1, 0, 0), (1, 1, 0), + (0, 1, 0), (0, 1, 1), + (0, 0, 1)] + cmap_name = 'rainbow' + rainbow_cmap = LinearSegmentedColormap.from_list(cmap_name, + colors, + N=256) + + img_colored = (rainbow_cmap(img / np.max(img)) * 255).astype(np.uint8) + img_bgr = cv2.cvtColor(img_colored, cv2.COLOR_RGBA2BGR) + + return img_bgr + if __name__ == '__main__': lidar_distance = LidarDistance() diff --git a/code/perception/src/traffic_light_detection/.gitignore b/code/perception/src/traffic_light_detection/.gitignore index af0e7a09..d55dcb8e 100644 --- a/code/perception/src/traffic_light_detection/.gitignore +++ b/code/perception/src/traffic_light_detection/.gitignore @@ -1,2 +1 @@ /dataset -/models diff --git a/code/perception/src/traffic_light_detection/dataset.dvc b/code/perception/src/traffic_light_detection/dataset.dvc new file mode 100644 index 00000000..2f10a027 --- /dev/null +++ b/code/perception/src/traffic_light_detection/dataset.dvc @@ -0,0 +1,6 @@ +outs: +- md5: 86f14bb96e1ac5735051b8e873f07a9f.dir + size: 13745063 + nfiles: 2723 + hash: md5 + path: dataset diff --git a/code/perception/src/traffic_light_detection/dvc.lock b/code/perception/src/traffic_light_detection/dvc.lock new file mode 100644 index 00000000..6438d251 --- /dev/null +++ b/code/perception/src/traffic_light_detection/dvc.lock @@ -0,0 +1,35 @@ +schema: '2.0' +stages: + train: + cmd: python src/traffic_light_detection/traffic_light_training.py + deps: + - path: dataset + hash: md5 + md5: 86f14bb96e1ac5735051b8e873f07a9f.dir + size: 13745063 + nfiles: 2723 + - path: src + hash: md5 + md5: 34c981a61e886a858d135daee17a82ba.dir + size: 35849 + nfiles: 17 + params: + params.yaml: + train: + epochs: 20 + batch_size: 32 + outs: + - path: dvclive/metrics.json + hash: md5 + md5: 8566265bcdc76cb55d17230f82fc1517 + size: 219 + - path: dvclive/plots + hash: md5 + md5: f8f99f42fc42e0ed3c80c8e8f05c1528.dir + size: 8870 + nfiles: 4 + - path: models + hash: md5 + md5: 16f96ecc475d20123051719908af9d4d.dir + size: 11071 + nfiles: 1 diff --git a/code/perception/src/traffic_light_detection/dvc.yaml b/code/perception/src/traffic_light_detection/dvc.yaml new file mode 100644 index 00000000..d08afa7e --- /dev/null +++ b/code/perception/src/traffic_light_detection/dvc.yaml @@ -0,0 +1,21 @@ +stages: + train: + cmd: python src/traffic_light_detection/traffic_light_training.py + deps: + - dataset + - src + params: + - params.yaml: + outs: + - models + metrics: + - dvclive/metrics.json: + cache: false + plots: + - dvclive/plots: + cache: false +metrics: +- dvclive/metrics.json +plots: +- dvclive/plots/metrics: + x: step diff --git a/code/perception/src/traffic_light_detection/dvclive/metrics.json b/code/perception/src/traffic_light_detection/dvclive/metrics.json new file mode 100644 index 00000000..52d49ebd --- /dev/null +++ b/code/perception/src/traffic_light_detection/dvclive/metrics.json @@ -0,0 +1,11 @@ +{ + "train": { + "accuracy": 92.26495726495727, + "loss": 0.03128106306251298 + }, + "validation": { + "accuracy": 91.36125654450262, + "loss": 0.031184170882739323 + }, + "step": 99 +} diff --git a/code/perception/src/traffic_light_detection/models/model_acc_92.48_val_91.88.pt b/code/perception/src/traffic_light_detection/models/model_acc_92.48_val_91.88.pt new file mode 100644 index 00000000..584f75f5 Binary files /dev/null and b/code/perception/src/traffic_light_detection/models/model_acc_92.48_val_91.88.pt differ diff --git a/code/perception/src/traffic_light_detection/params.yaml b/code/perception/src/traffic_light_detection/params.yaml index 7b90494c..efce6997 100644 --- a/code/perception/src/traffic_light_detection/params.yaml +++ b/code/perception/src/traffic_light_detection/params.yaml @@ -1,3 +1,3 @@ train: - epochs: 100 + epochs: 20 batch_size: 32 diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_config.py b/code/perception/src/traffic_light_detection/src/traffic_light_config.py index 6187e901..e1c720c2 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_config.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_config.py @@ -17,7 +17,7 @@ def __init__(self): # Amount of epochs to train # One epoch: Training with all images from training dataset once self.NUM_WORKERS = 4 - self.NUM_CLASSES = 4 # Traffic light states: green, yellow, red, back + self.NUM_CLASSES = 5 # States: green, yellow, red, back, side self.NUM_CHANNELS = 3 # RGB encoded images # Inference diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/classification_model.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/classification_model.py index 9c5fb339..f44d2c31 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/classification_model.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/classification_model.py @@ -59,10 +59,10 @@ def load_model(cfg): if path is not None: try: state_dict = torch.load(path) - model.load_state_dict(state_dict).eval() + model.load_state_dict(state_dict) print(f"Pretrained model loaded from {path}") return model - except (Exception, ): - print(f"No pretrained model found at {path}. " + except Exception as e: + print(f"No pretrained model found at {path}: {e}\n" f"Created new model with random weights.") return model.eval() diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py index 40dd4c24..ada59c5f 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py @@ -1,13 +1,13 @@ import argparse -from pathlib import Path import torch.cuda import torchvision.transforms as t -from data_generation.transforms import Normalize, ResizeAndPadToSquare, \ - load_image -from traffic_light_detection.classification_model import ClassificationModel +from traffic_light_detection.src.traffic_light_detection.transforms \ + import Normalize, ResizeAndPadToSquare, load_image +from traffic_light_detection.src.traffic_light_detection.classification_model \ + import ClassificationModel from torchvision.transforms import ToTensor -from traffic_light_config import TrafficLightConfig +from traffic_light_detection.src.traffic_light_config import TrafficLightConfig def parse_args(): @@ -24,6 +24,9 @@ def parse_args(): 'model_acc_99.53_val_100.0.pt', help='path to pretrained model', type=str) + parser.add_argument('--image', default=None, + help='/dataset/val/green/green_83.png', + type=str) return parser.parse_args() @@ -47,7 +50,8 @@ def __init__(self, model_path): self.class_dict = {0: 'Backside', 1: 'Green', 2: 'Red', - 3: 'Yellow'} + 3: 'Side', + 4: 'Yellow'} def __call__(self, img): """ @@ -66,8 +70,7 @@ def __call__(self, img): # main function for testing purposes if __name__ == '__main__': args = parse_args() - image_path = str(Path(__file__).resolve().parents[2].resolve()) - image_path += "/dataset/val/green/green_83.png" + image_path = args.image image = load_image(image_path) classifier = TrafficLightInference(args.model) pred = classifier(image) diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py index 7f15e3c9..1322d024 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py @@ -11,8 +11,8 @@ import sys import os sys.path.append(os.path.abspath(sys.path[0] + '/..')) -from data_generation.transforms import Normalize, ResizeAndPadToSquare, \ - load_image # noqa: E402 +from traffic_light_detection.transforms import Normalize, \ + ResizeAndPadToSquare, load_image # noqa: E402 from data_generation.weights_organizer import WeightsOrganizer # noqa: E402 from traffic_light_detection.classification_model import ClassificationModel \ # noqa: E402 diff --git a/code/perception/src/traffic_light_detection/src/data_generation/transforms.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/transforms.py similarity index 95% rename from code/perception/src/traffic_light_detection/src/data_generation/transforms.py rename to code/perception/src/traffic_light_detection/src/traffic_light_detection/transforms.py index f1f992d2..41dc8bfc 100644 --- a/code/perception/src/traffic_light_detection/src/data_generation/transforms.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/transforms.py @@ -28,7 +28,7 @@ def __call__(self, image): image = torchvision.transforms.Pad((0, pad))(image) else: image = torchvision.transforms.Pad((pad, 0))(image) - image = torchvision.transforms.Resize(self.size)(image) + image = torchvision.transforms.Resize(self.size, antialias=True)(image) return image diff --git a/code/perception/src/traffic_light_node.py b/code/perception/src/traffic_light_node.py new file mode 100755 index 00000000..6f67b5b1 --- /dev/null +++ b/code/perception/src/traffic_light_node.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +from ros_compatibility.node import CompatibleNode +import ros_compatibility as roscomp +from rospy.numpy_msg import numpy_msg +from sensor_msgs.msg import Image as ImageMsg +from perception.msg import TrafficLightState +from cv_bridge import CvBridge +from traffic_light_detection.src.traffic_light_detection.traffic_light_inference \ + import TrafficLightInference # noqa: E501 + + +class TrafficLightNode(CompatibleNode): + def __init__(self, name, **kwargs): + super().__init__(name, **kwargs) + # general setup + self.bridge = CvBridge() + self.role_name = self.get_param("role_name", "hero") + self.side = self.get_param("side", "Center") + self.classifier = TrafficLightInference(self.get_param("model", "")) + + # publish / subscribe setup + self.setup_camera_subscriptions() + self.setup_traffic_light_publishers() + + def setup_camera_subscriptions(self): + self.new_subscription( + msg_type=numpy_msg(ImageMsg), + callback=self.handle_camera_image, + topic=f"/paf/{self.role_name}/{self.side}/segmented_traffic_light", + qos_profile=1 + ) + + def setup_traffic_light_publishers(self): + self.traffic_light_publisher = self.new_publisher( + msg_type=TrafficLightState, + topic=f"/paf/{self.role_name}/{self.side}/traffic_light_state", + qos_profile=1 + ) + + def handle_camera_image(self, image): + result = self.classifier(self.bridge.imgmsg_to_cv2(image)) + + # 1: Green, 2: Red, 4: Yellow, 0: Unknown + msg = TrafficLightState() + msg.state = result if result in [1, 2, 4] else 0 + + self.traffic_light_publisher.publish(msg) + + def run(self): + self.spin() + + +if __name__ == "__main__": + roscomp.init("TrafficLightNode") + node = TrafficLightNode("TrafficLightNode") + node.run() diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index d736253a..e9681a4c 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -18,7 +18,6 @@ from cv_bridge import CvBridge from torchvision.utils import draw_bounding_boxes, draw_segmentation_masks import numpy as np -from time import perf_counter from ultralytics import NAS, YOLO, RTDETR, SAM, FastSAM """ VisionNode: @@ -86,6 +85,7 @@ def __init__(self, name, **kwargs): # publish / subscribe setup self.setup_camera_subscriptions() self.setup_camera_publishers() + self.setup_traffic_light_publishers() self.image_msg_header = Header() self.image_msg_header.frame_id = "segmented_image_frame" @@ -128,31 +128,30 @@ def setup_camera_publishers(self): qos_profile=1 ) - def handle_camera_image(self, image): - startTime = perf_counter() + def setup_traffic_light_publishers(self): + self.traffic_light_publisher = self.new_publisher( + msg_type=numpy_msg(ImageMsg), + topic=f"/paf/{self.role_name}/{self.side}/segmented_traffic_light", + qos_profile=1 + ) + def handle_camera_image(self, image): # free up cuda memory if self.device == "cuda": torch.cuda.empty_cache() - print("Before Model: ", perf_counter() - startTime) - if self.framework == "pyTorch": vision_result = self.predict_torch(image) if self.framework == "ultralytics": vision_result = self.predict_ultralytics(image) - print("After Model: ", perf_counter() - startTime) - # publish image to rviz img_msg = self.bridge.cv2_to_imgmsg(vision_result, - encoding="passthrough") + encoding="rgb8") img_msg.header = image.header self.publisher.publish(img_msg) - pass - def predict_torch(self, image): self.model.eval() cv_image = self.bridge.imgmsg_to_cv2(img_msg=image, @@ -180,12 +179,36 @@ def predict_ultralytics(self, image): cv_image = self.bridge.imgmsg_to_cv2(img_msg=image, desired_encoding='passthrough') cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) - print(cv_image.shape) - output = self.model(cv_image) + output = self.model(cv_image, half=True, verbose=False) + + if 9 in output[0].boxes.cls: + self.process_traffic_lights(output[0], cv_image, image.header) return output[0].plot() + def process_traffic_lights(self, prediction, cv_image, image_header): + indices = (prediction.boxes.cls == 9).nonzero().squeeze().cpu().numpy() + indices = np.asarray([indices]) if indices.size == 1 else indices + + min_x = 550 + max_x = 700 + min_prob = 0.35 + + for index in indices: + box = prediction.boxes.cpu().data.numpy()[index] + + if box[0] < min_x or box[2] > max_x or box[4] < min_prob: + continue + + box = box[0:4].astype(int) + segmented = cv_image[box[1]:box[3], box[0]:box[2]] + + traffic_light_image = self.bridge.cv2_to_imgmsg(segmented, + encoding="rgb8") + traffic_light_image.header = image_header + self.traffic_light_publisher.publish(traffic_light_image) + def create_mask(self, input_image, model_output): output_predictions = torch.argmax(model_output, dim=0) for i in range(21): @@ -193,7 +216,6 @@ def create_mask(self, input_image, model_output): output_predictions = output_predictions.to(dtype=torch.bool) - print(output_predictions.shape) transposed_image = np.transpose(input_image, (2, 0, 1)) tensor_image = torch.tensor(transposed_image) tensor_image = tensor_image.to(dtype=torch.uint8) diff --git a/code/planning/global_planner/CMakeLists.txt b/code/planning/CMakeLists.txt old mode 100644 new mode 100755 similarity index 92% rename from code/planning/global_planner/CMakeLists.txt rename to code/planning/CMakeLists.txt index 3344bc47..34c3ccd0 --- a/code/planning/global_planner/CMakeLists.txt +++ b/code/planning/CMakeLists.txt @@ -7,16 +7,21 @@ project(planning) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) +find_package(catkin REQUIRED COMPONENTS + perception + rospy + roslaunch + std_msgs +) - +roslaunch_add_file_check(launch) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -44,9 +49,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 +68,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 +105,7 @@ catkin_package( # LIBRARIES planning # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib + CATKIN_DEPENDS perception rospy ) ########### @@ -113,7 +116,7 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( # include -# ${catkin_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library @@ -200,3 +203,12 @@ include_directories( ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + +# catkin_install_python( +# PROGRAMS +# src/behavior_agent/behavior_tree.py +# src/behavior_agent/__init__.py +# DESTINATION +# ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) +install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) \ No newline at end of file diff --git a/code/planning/behavior_agent/src/behavior_agent/__init__.py b/code/planning/__init__.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/__init__.py rename to code/planning/__init__.py diff --git a/code/planning/behavior_agent/CMakeLists.txt b/code/planning/behavior_agent/CMakeLists.txt deleted file mode 100644 index 7d505925..00000000 --- a/code/planning/behavior_agent/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(behavior_agent) - -find_package(catkin REQUIRED COMPONENTS rospy roslaunch) - -catkin_python_setup() - -roslaunch_add_file_check(launch) - -catkin_package(CATKIN_DEPENDS rospy) - -catkin_install_python( - PROGRAMS - src/behavior_agent/behavior_tree.py - src/behavior_agent/__init__.py - DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION} -) - - -install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) diff --git a/code/planning/behavior_agent/launch/behavior_agent.launch b/code/planning/behavior_agent/launch/behavior_agent.launch deleted file mode 100644 index 6be8134a..00000000 --- a/code/planning/behavior_agent/launch/behavior_agent.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/code/planning/behavior_agent/package.xml b/code/planning/behavior_agent/package.xml deleted file mode 100644 index 373ad321..00000000 --- a/code/planning/behavior_agent/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - behavior_agent - 0.0.0 - The decision tree - We - MIT - catkin - rospy - roslaunch - rospy - rospy - carla_msgs - sensor_msgs - std_msgs - - - diff --git a/code/planning/behavior_agent/readme.md b/code/planning/behavior_agent/readme.md deleted file mode 100644 index 9d57917f..00000000 --- a/code/planning/behavior_agent/readme.md +++ /dev/null @@ -1,190 +0,0 @@ -[//]: # () -[//]: # (""") - -[//]: # (Source: https://github.com/ll7/psaf2) - -[//]: # (""") - -# behaviour_agent - -**Disclaimer**: As we mainly built our decision tree on the previous [PAF project](https://github.com/ll7/psaf2), most part of the documentation was added here and adjusted to the changes we made. - -## About - -This Package implements a behaviour agent for our autonomous car using **Behaviour Trees**. It uses the `py_trees` Framework, that works well with ROS. -For visualization at runtime you might want to also install this [rqt-Plugin](https://wiki.ros.org/rqt_py_trees). - -## Our behaviour tree - -The following section describes the behaviour tree we use for normal driving using all functionality provided by the agent. In the actual implementation this is part of a bigger tree, that handles things like writing topics to the blackboard, starting and finishing the decision tree. -The following description is not complete, it just contains the most common behaviours and subtrees. For a complete description have a look at the [tree-description](../../../doc/07_planning/behaviortree.xml) and the [bt-specs](../../../doc/07_planning/behavior_tree_spec.md). -Note that we didn't actually implement all the behaviours from this design, due to time and functionality limitations. - -### Legend - -The following notation is used in this documentation: - -![BT Legend](../../../doc/00_assets/legend_bt.png) - -#### Behavior - -Represent an action the decision tree should execute. It has three return values representing the state of the behavior: - -* `SUCCESS`: The action has been performed successfully. -* `RUNNING`: The action is still being executed. -* `Failure`: The action couldn't be executed. - -#### Selector - -Tries to execute each of its child behaviors in turn. It has a priority hierarchy from left (high priority) to right (low priority). - -#### Sequence - -Executes all of its child behaviors sequentially after one another until all behaviors have returned `SUCCESS` or one behavior returned `FAILURE`. In that case, the sequence is aborted. - -#### Condition - -Is always the first child of a sequence. It decides if the sequence should be executed or aborted. - -#### Subtree - -Represents a specific task/scenario which is handled by the decision tree. - -### Big Picture - -![BT Big Picture](../../../doc/00_assets/top-level.png) - -This top-level tree consists mainly of subtrees that are explained below. If none of the subtrees fit the current situation, the behaviour_agent goes into `Cruising`-behaviour, where it just follows the Path at an appropriate speed. - -### Intersection - -![BT Intersection](../../../doc/00_assets/intersection.png) - -If there is an intersection coming up, the agent executes the following sequence of behaviours: - -* Approach Intersection - - Slows down and stops at line if a stop sign or a yellow or red traffic light is detected - -* Wait at Intersection - - Waits for traffic lights or higher priority traffic - -* Enter Intersection - - Enters the intersection and follows it predetermined path through the intersection - -* Leave Intersection - - Leaves the intersection in the right direction - -### Overtaking - -The Overtaking subtree is quite big to accommodate for different overtaking scenarios. Here is an overview of that subtree further refining it. - -![Overtaking](../../../doc/00_assets/overtaking_overview.png) - -Please have a look at the [tree-description](../../../doc/07_planning/behaviortree.xml) and the [bt-specs](../../../doc/07_planning/behavior_tree_spec.md) for a more detailed description. The Multi-Lane Overtaking Subtree looks like this: - -![BT Overtaking](../../../doc/00_assets/multi_lane.png) - -* Multi Lane? - - Checks the map data: does the current road have more than one lane? - -* Left Lane available? - - The lane detection checks if there is a lane to the left - -* Wait for Left Lane free - - Waits for the left lane to be free. This has a timeout. - -* Switch Lane Left - - Triggers a lane switch to the left by calling the local planner - -### Right-Hand Driving - -This subtree makes the ego vehicle switch back to the right lane, if the road ahead is free enough. It is quite similar to the Multi-Lane Overtaking Subtree, just with reversed directions. - -![BT Right Hand](../../../doc/00_assets/Right_lane.png) - -## Developing guide - -### Tree Definition - -The tree is defined in the `grow_a_tree()`-function inside `code/planning/behavior_agent/behavior_tree.py`, which is also the main node. It can be visualized using an [rqt-Plugin](https://wiki.ros.org/rqt_py_trees). This is also the place to change the execution rate of the tree: - -``` python -... -behaviour_tree.tick_tock(500) -... -``` - -### Behaviours - -`Behaviours` are implemented in the `code/planning/behavior_agent/behaviours/` directory. All the behaviours used in the current version of the tree are contained as skeletons. - -#### Blackboard - -To deal with the asynchronicity of ROS, all the topics this tree subscribes to, should be written to the Blackboard at the beginning of each tick. A node is available, that automates this task. Just add your node to the list in `src/behavior_agent/behaviours/topics2blackboard.py`: - -``` python -... -topics =[ - {'name':f"/carla/{role_name}/Speed", 'msg':CarlaSpeedoMeter, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - ... - ] -... -``` - -After that you can access them from everywhere in your Behaviour-Code using: - -``` python -... -self.blackboard = py_trees.blackboard.Blackboard() -... -speed = self.blackboard.get("/carla/hero/Speed") -... -``` - -Note that you still need to resolve the data-fields of the message (i.e. `blackboardmessage.data` for a `Float64`). - -### Guidelines - -When implementing new behaviours you should adhere to the following guidelines: - -#### Non-Blocking - -You should avoid doing complicated calculations inside the behaviours. Use asynchronous Callbacks instead, and return ```RUNNING``` while another node does the computing. - -Generally conditions should never return ```RUNNING``` and Action-Behaviours should only return ```FAILURE``` in special cases. - -#### Functions - -Behaviours generally provide five functions (you can define more of course). Short explanation when they get called and how to use them: - -##### `__init__()` - -You should probably never use this. - -##### `setup()` - -Gets called whenever the tree gets set up for the first time. Use this to setup local variables that don't need to change, like ```self.blackboard = py_trees.blackboard.Blackboard()``` or middleware like ROS-Publishers (Subscribers should be setup using the method mentioned above). - -##### `initialise()` - -Gets called every time the behaviour is entered for a new execution. Add code that only needs to be called once at the beginning of a behaviour (i.e. publishing a new target speed). - -##### `update()` - -Main function of a behaviour, that gets called everytime the behaviour is ticked. Here you need to return ```SUCCESS```, ```RUNNING``` or ```FAILURE```. - -##### `terminate()` - -This gets called, whenever a behaviour is cancelled by a higher priority branch. Use to terminate middleware connections or asynchronous Calculations, whose results are not needed anymore. - -## Authors - -Josef Kircher diff --git a/code/planning/behavior_agent/setup.py b/code/planning/behavior_agent/setup.py deleted file mode 100755 index 4ba971a2..00000000 --- a/code/planning/behavior_agent/setup.py +++ /dev/null @@ -1,15 +0,0 @@ -""" -Source: https://github.com/ll7/psaf2 - -behavior_agent -""" - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['behavior_agent'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py deleted file mode 100755 index f8984b65..00000000 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -from . import topics2blackboard, road_features -from . import intersection, traffic_objects, maneuvers, meta, lane_change \ No newline at end of file diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch deleted file mode 100644 index d26b4873..00000000 --- a/code/planning/global_planner/launch/global_planner.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch new file mode 100644 index 00000000..c6377c79 --- /dev/null +++ b/code/planning/launch/planning.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/planning/local_planner/CMakeLists.txt b/code/planning/local_planner/CMakeLists.txt deleted file mode 100644 index 32a5947b..00000000 --- a/code/planning/local_planner/CMakeLists.txt +++ /dev/null @@ -1,202 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(local_planner) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES planning -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include -# ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/planning.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/planning_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_planning.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch deleted file mode 100644 index c469c05e..00000000 --- a/code/planning/local_planner/launch/local_planner.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/code/planning/local_planner/package.xml b/code/planning/local_planner/package.xml deleted file mode 100644 index 6dc4269a..00000000 --- a/code/planning/local_planner/package.xml +++ /dev/null @@ -1,59 +0,0 @@ - - - local_planner - 0.0.0 - The local planning package - - - - - carla - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - - - - - - diff --git a/code/planning/local_planner/setup.py b/code/planning/local_planner/setup.py deleted file mode 100644 index e5a88c1c..00000000 --- a/code/planning/local_planner/setup.py +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -setup_args = generate_distutils_setup(packages=['local_planner'], - package_dir={'': 'src'}) -setup(**setup_args) diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py deleted file mode 100755 index 34bcdbb4..00000000 --- a/code/planning/local_planner/src/collision_check.py +++ /dev/null @@ -1,107 +0,0 @@ -#!/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 Float32MultiArray - - -class CollisionCheck(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(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 - # TODO: Add Subscriber for Speed and Obstacles - self.logdebug("CollisionCheck started") - - def update(self, speed): - self.current_speed = speed - - def time_to_collision(self, obstacle_speed, distance): - return distance / (self.current_speed - obstacle_speed) - - def meters_to_collision(self, obstacle_speed, distance): - return self.time_to_collision(obstacle_speed, distance) * \ - self.current_speed - - # PAF 22 - def calculate_safe_dist(self) -> float: - """ - 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 - """ - 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 - if emergency: - return reaction_distance + braking_distance / 2 - else: - return reaction_distance + braking_distance - - 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) - - 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 - 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}") - else: - print("Ego slower then car in front") - - def run(self): - """ - Control loop - :return: - """ - - def loop(timer_event=None): - pass - - self.new_timer(self.control_loop_rate, loop) - self.spin() - - -if __name__ == "__main__": - """ - main function starts the CollisionCheck node - :param args: - """ - roscomp.init('CollisionCheck') - - try: - node = CollisionCheck() - node.run() - except KeyboardInterrupt: - pass - finally: - roscomp.shutdown() diff --git a/code/planning/global_planner/package.xml b/code/planning/package.xml old mode 100644 new mode 100755 similarity index 85% rename from code/planning/global_planner/package.xml rename to code/planning/package.xml index 9a2f8896..a321a109 --- a/code/planning/global_planner/package.xml +++ b/code/planning/package.xml @@ -32,7 +32,6 @@ - @@ -48,12 +47,25 @@ + rospy + roslaunch + + rospy + perception + + rospy + carla_msgs + sensor_msgs + std_msgs + perception + + perception + catkin - diff --git a/code/planning/planning_runner/CMakeLists.txt b/code/planning/planning_runner/CMakeLists.txt deleted file mode 100644 index 5111471e..00000000 --- a/code/planning/planning_runner/CMakeLists.txt +++ /dev/null @@ -1,205 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(planning_runner) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES planning_runner -# CATKIN_DEPENDS rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/planning_runner.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/planning_runner_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_planning_runner.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/code/planning/planning_runner/launch/planning_runner.launch b/code/planning/planning_runner/launch/planning_runner.launch deleted file mode 100755 index b5597b71..00000000 --- a/code/planning/planning_runner/launch/planning_runner.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/code/planning/planning_runner/package.xml b/code/planning/planning_runner/package.xml deleted file mode 100644 index 4efbae8d..00000000 --- a/code/planning/planning_runner/package.xml +++ /dev/null @@ -1,65 +0,0 @@ - - - planning_runner - 0.0.0 - The planning_runner package - - - - - imech039 - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - std_msgs - rospy - std_msgs - rospy - std_msgs - - - - - - - - diff --git a/code/planning/global_planner/setup.py b/code/planning/setup.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/setup.py rename to code/planning/setup.py diff --git a/code/planning/global_planner/__init__.py b/code/planning/src/behavior_agent/__init__.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/__init__.py rename to code/planning/src/behavior_agent/__init__.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py similarity index 57% rename from code/planning/behavior_agent/src/behavior_agent/behavior_tree.py rename to code/planning/src/behavior_agent/behavior_tree.py index 4df7e287..cacf8619 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -1,21 +1,19 @@ #!/usr/bin/env python import functools -import behavior_agent -import py_trees from py_trees.behaviours import Running import py_trees_ros -import py_trees.console as console import rospy import sys -from behavior_agent import behaviours +import behaviours from py_trees.composites import Parallel, Selector, Sequence -from py_trees.decorators import Inverter """ Source: https://github.com/ll7/psaf2 """ +# flake8: noqa: E501 + def grow_a_tree(role_name): @@ -61,47 +59,7 @@ def grow_a_tree(role_name): ("Leave Change") ]) ]), - Inverter(Selector("Overtaking", children=[ - behaviours.traffic_objects.NotSlowedByCarInFront - ("Not Slowed By Car in Front?"), - Selector("Number of Lanes", children=[ - Sequence("Multi Lane", children=[ - behaviours.road_features.MultiLane("Multi Lane?"), - behaviours.road_features.LeftLaneAvailable - ("Left Lane Available?"), - behaviours.traffic_objects.WaitLeftLaneFree - ("Wait for Left Lane Free"), - behaviours.maneuvers.SwitchLaneLeft("Switch Lane Left") - ]), - Sequence("Single Lane", children=[ - behaviours.road_features.SingleLineDotted - ("Single Lane with dotted Line?"), - behaviours.traffic_objects.WaitLeftLaneFree - ("Wait for Left Lane Free"), - behaviours.maneuvers.SwitchLaneLeft - ("Switch Lane Left"), - Selector("Driving on Left Side", children=[ - Sequence("Overtake", children=[ - behaviours.traffic_objects.OvertakingPossible - ("Overtaking Possible?"), - behaviours.maneuvers.Overtake("Overtake"), - behaviours.maneuvers.SwitchLaneRight - ("Switch Lane Right") - ]), - behaviours.maneuvers.SwitchLaneRight("Switch Lane Right") - ]) - ]) - ]), - Running("Can't Overtake") - ])), - Sequence("Back to Right Lane", children=[ - behaviours.road_features.RightLaneAvailable("Right Lane Available"), - behaviours.traffic_objects.NotSlowedByCarInFrontRight - ("Not Slowed By Car in Front Right?"), - behaviours.traffic_objects. - WaitRightLaneFree("Wait for Right Lane Free"), - behaviours.maneuvers.SwitchLaneRight("Switch Lane Right") - ]) + ]), behaviours.maneuvers.Cruise("Cruise") ]) @@ -132,7 +90,7 @@ def main(): rospy.on_shutdown(functools.partial(shutdown, behaviour_tree)) if not behaviour_tree.setup(timeout=15): - rospy.loginfo("Tree Setup failed") + rospy.logerr("Tree Setup failed") sys.exit(1) rospy.loginfo("tree setup worked") r = rospy.Rate(5) diff --git a/code/planning/local_planner/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/local_planner/__init__.py rename to code/planning/src/behavior_agent/behaviours/__init__.py diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py new file mode 100755 index 00000000..4eb9cf47 --- /dev/null +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -0,0 +1,66 @@ + +from collections import namedtuple + + +def convert_to_ms(speed): + return speed / 3.6 + + +Behavior = namedtuple("Behavior", ("name", "speed")) + +# Changed target_speed_pub to curr_behavior_pub + +# Intersection - Behaviors + +# Approach + +int_app_init = Behavior("int_app_init", convert_to_ms(30.0)) + +# No Traffic Light or Sign -> stop dynamically at Stopline +int_app_no_sign = Behavior("int_app_no_sign", -2) + +int_app_green = Behavior("int_app_green", convert_to_ms(30.0)) + +# Wait + +int_wait = Behavior("int_wait", 0) + +# Enter + +int_enter_no_light = Behavior("int_enter_no_light", convert_to_ms(50.0)) + +int_enter_empty_str = Behavior("int_enter_empty_string", convert_to_ms(18.0)) + +int_enter_light = Behavior("int_enter_light", convert_to_ms(50.0)) + +# Exit + +int_exit = Behavior("int_exit", -1) # Get SpeedLimit dynamically + + +# Lane Change + +# Approach + +lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0)) + + +# TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128) +lc_app_blocked = Behavior("lc_app_blocked", 0.5) + +# Wait + +# Has a publisher but doesnt publish anything ?? + +# Enter + +lc_enter_init = Behavior("lc_enter_init", convert_to_ms(20.0)) + +# Exit + +lc_exit = Behavior("lc_exit", -1) # Get SpeedLimit dynamically + + +# Cruise + +cruise = Behavior("Cruise", -1) diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py similarity index 90% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py rename to code/planning/src/behavior_agent/behaviours/intersection.py index b8d7e8db..22697f27 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/src/behavior_agent/behaviours/intersection.py @@ -1,9 +1,11 @@ import py_trees import numpy as np -from std_msgs.msg import Float32 +from std_msgs.msg import String import rospy +from .import behavior_speed as bs + """ Source: https://github.com/ll7/psaf2 """ @@ -41,9 +43,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.target_speed_pub = rospy.Publisher("/paf/hero/" - "max_tree_velocity", - Float32, queue_size=1) + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", + String, queue_size=1) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -66,7 +68,7 @@ def initialise(self): self.traffic_light_distance = np.inf self.traffic_light_status = '' self.virtual_stopline_distance = np.inf - self.target_speed_pub.publish(convert_to_ms(30.0)) + self.curr_behavior_pub.publish(bs.int_app_init.name) self.last_virtual_distance = np.inf def update(self): @@ -117,16 +119,7 @@ def update(self): self.virtual_stopline_distance = 0.0 rospy.loginfo(f"Stopline distance: {self.virtual_stopline_distance}") - target_distance = 3.0 - # calculate speed needed for stopping - v_stop = max(convert_to_ms(10.), - convert_to_ms((self.virtual_stopline_distance / 30) - * 50)) - if v_stop > convert_to_ms(50.0): - v_stop = convert_to_ms(50.0) - if self.virtual_stopline_distance < target_distance: - v_stop = 0.0 # stop when there is no or red/yellow traffic light or a stop sign is # detected if self.traffic_light_status == '' \ @@ -135,13 +128,13 @@ def update(self): or (self.stop_sign_detected and not self.traffic_light_detected): - rospy.loginfo(f"slowing down: {v_stop}") - self.target_speed_pub.publish(v_stop) + rospy.loginfo("slowing down!") + self.curr_behavior_pub.publish(bs.int_app_no_sign.name) # approach slowly when traffic light is green as traffic lights are # higher priority than traffic signs this behavior is desired if self.traffic_light_status == 'green': - self.target_speed_pub.publish(convert_to_ms(30)) + self.curr_behavior_pub.publish(bs.int_app_green.name) # get speed speedometer = self.blackboard.get("/carla/hero/Speed") @@ -224,9 +217,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.target_speed_pub = rospy.Publisher("/paf/hero/" - "max_tree_velocity", Float32, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", String, + queue_size=1) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -279,7 +272,7 @@ def update(self): if traffic_light_status == "red" or \ traffic_light_status == "yellow": rospy.loginfo(f"Light Status: {traffic_light_status}") - self.target_speed_pub.publish(0) + self.curr_behavior_pub.publish(bs.int_wait.name) return py_trees.common.Status.RUNNING else: rospy.loginfo(f"Light Status: {traffic_light_status}") @@ -333,9 +326,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.target_speed_pub = rospy.Publisher("/paf/hero/" - "max_tree_velocity", Float32, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", String, + queue_size=1) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -352,16 +345,16 @@ def initialise(self): rospy.loginfo("Enter Intersection") light_status_msg = self.blackboard.get("/paf/hero/traffic_light") if light_status_msg is None: - self.target_speed_pub.publish(convert_to_ms(50.0)) + self.curr_behavior_pub.publish(bs.int_enter_no_light.name) return True else: traffic_light_status = light_status_msg.color if traffic_light_status == "": - self.target_speed_pub.publish(convert_to_ms(18.0)) + self.curr_behavior_pub.publish(bs.int_enter_empty_str.name) else: rospy.loginfo(f"Light Status: {traffic_light_status}") - self.target_speed_pub.publish(convert_to_ms(50.0)) + self.curr_behavior_pub.publish(bs.int_enter_light.name) def update(self): """ @@ -433,9 +426,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.target_speed_pub = rospy.Publisher("/paf/hero/" - "max_tree_velocity", Float32, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", String, + queue_size=1) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -453,16 +446,15 @@ def initialise(self): rospy.loginfo("Leave Intersection") street_speed_msg = self.blackboard.get("/paf/hero/speed_limit") if street_speed_msg is not None: - self.target_speed_pub.publish(street_speed_msg.data) + # self.curr_behavior_pub.publish(street_speed_msg.data) + self.curr_behavior_pub.publish(bs.int_exit.name) return True def update(self): """ When is this called? Every time your behaviour is ticked. - What to do here? - - Triggering, checking, monitoring. Anything...but do not block! - - Set a feedback message + What to do here?exit_int - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] Abort this subtree :return: py_trees.common.Status.FAILURE, to exit this subtree diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py old mode 100644 new mode 100755 similarity index 92% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py rename to code/planning/src/behavior_agent/behaviours/lane_change.py index d4b84d19..4433cc2e --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py +++ b/code/planning/src/behavior_agent/behaviours/lane_change.py @@ -1,11 +1,13 @@ import py_trees import numpy as np -from std_msgs.msg import Float32 +from std_msgs.msg import String # from nav_msgs.msg import Odometry # from custom_carla_msgs.srv import UpdateLocalPath import rospy +from . import behavior_speed as bs + """ Source: https://github.com/ll7/psaf2 """ @@ -43,9 +45,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.target_speed_pub = rospy.Publisher("/paf/hero/" - "max_tree_velocity", - Float32, queue_size=1) + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", + String, queue_size=1) # rospy.wait_for_service('update_local_path') # TODO is this necessary? # self.update_local_path = # rospy.ServiceProxy("update_local_path", UpdateLocalPath) @@ -68,7 +70,7 @@ def initialise(self): self.change_detected = False self.change_distance = np.inf self.virtual_change_distance = np.inf - self.target_speed_pub.publish(convert_to_ms(30.0)) + self.curr_behavior_pub.publish(bs.lc_init.name) def update(self): """ @@ -98,12 +100,6 @@ def update(self): if self.change_distance != np.inf and self.change_detected: self.virtual_change_distance = self.change_distance - # calculate speed needed for stopping - v_stop = max(convert_to_ms(5.), - convert_to_ms((self.virtual_change_distance / 30) ** 1.5 - * 50)) - if v_stop > convert_to_ms(50.0): - v_stop = convert_to_ms(30.0) # slow down before lane change if self.virtual_change_distance < 15.0: if self.change_option == 5: @@ -120,9 +116,8 @@ def update(self): # self.update_local_path(leave_intersection=True) return py_trees.common.Status.SUCCESS else: - v_stop = 0.5 - rospy.loginfo(f"Change blocked slowing down: {v_stop}") - self.target_speed_pub.publish(v_stop) + rospy.loginfo("Change blocked slowing down") + self.curr_behavior_pub.publish(bs.lc_app_blocked.name) # get speed speedometer = self.blackboard.get("/carla/hero/Speed") @@ -194,9 +189,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.target_speed_pub = rospy.Publisher("/paf/hero/" - "max_tree_velocity", Float32, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", String, + queue_size=1) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -303,9 +298,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.target_speed_pub = rospy.Publisher("/paf/hero/" - "max_tree_velocity", Float32, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", String, + queue_size=1) # rospy.wait_for_service('update_local_path') # self.update_local_path = rospy.ServiceProxy("update_local_path", # UpdateLocalPath) @@ -323,7 +318,7 @@ def initialise(self): the intersection. """ rospy.loginfo("Enter next Lane") - self.target_speed_pub.publish(convert_to_ms(20.0)) + self.curr_behavior_pub.publish(bs.lc_enter_init.name) def update(self): """ @@ -396,9 +391,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.target_speed_pub = rospy.Publisher("/paf/hero/" - "max_tree_velocity", Float32, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", String, + queue_size=1) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -415,7 +410,8 @@ def initialise(self): rospy.loginfo("Leave Change") street_speed_msg = self.blackboard.get("/paf/hero/speed_limit") if street_speed_msg is not None: - self.target_speed_pub.publish(street_speed_msg.data) + # self.curr_behavior_pub.publish(street_speed_msg.data) + self.curr_behavior_pub.publish(bs.lc_exit.name) return True def update(self): diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py similarity index 96% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py rename to code/planning/src/behavior_agent/behaviours/maneuvers.py index ce672563..5a17c25a 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -1,4 +1,9 @@ import py_trees +import rospy +from std_msgs.msg import String + +from . import behavior_speed as bs +# from behavior_agent.msg import BehaviorSpeed """ Source: https://github.com/ll7/psaf2 @@ -264,6 +269,7 @@ def __init__(self, name): :param name: name of the behaviour """ super(Cruise, self).__init__(name) + rospy.loginfo("Cruise started") def setup(self, timeout): """ @@ -277,6 +283,11 @@ def setup(self, timeout): successful :return: True, as there is nothing to set up. """ + + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", + String, queue_size=1) + self.blackboard = py_trees.blackboard.Blackboard() return True @@ -290,6 +301,7 @@ def initialise(self): Any initialisation you need before putting your behaviour to work. :return: True """ + rospy.loginfo("Starting Cruise") return True def update(self): @@ -308,6 +320,7 @@ def update(self): :return: py_trees.common.Status.RUNNING, keeps the decision tree from finishing """ + self.curr_behavior_pub.publish(bs.cruise.name) return py_trees.common.Status.RUNNING def terminate(self, new_status): diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/meta.py b/code/planning/src/behavior_agent/behaviours/meta.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/meta.py rename to code/planning/src/behavior_agent/behaviours/meta.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/road_features.py b/code/planning/src/behavior_agent/behaviours/road_features.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/road_features.py rename to code/planning/src/behavior_agent/behaviours/road_features.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/topics2blackboard.py rename to code/planning/src/behavior_agent/behaviours/topics2blackboard.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/traffic_objects.py b/code/planning/src/behavior_agent/behaviours/traffic_objects.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/traffic_objects.py rename to code/planning/src/behavior_agent/behaviours/traffic_objects.py diff --git a/code/planning/global_planner/src/dev_global_route.py b/code/planning/src/global_planner/dev_global_route.py similarity index 100% rename from code/planning/global_planner/src/dev_global_route.py rename to code/planning/src/global_planner/dev_global_route.py diff --git a/code/planning/global_planner/src/global_planner.py b/code/planning/src/global_planner/global_planner.py similarity index 99% rename from code/planning/global_planner/src/global_planner.py rename to code/planning/src/global_planner/global_planner.py index fdc542d6..a113c787 100755 --- a/code/planning/global_planner/src/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -77,7 +77,7 @@ def __init__(self): msg_type=Float32MultiArray, topic=f"/paf/{self.role_name}/speed_limits_OpenDrive", qos_profile=1) - self.loginfo('PrePlanner-Node started') + self.logdebug('PrePlanner-Node started') def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/global_planner/src/global_route.txt b/code/planning/src/global_planner/global_route.txt old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/src/global_route.txt rename to code/planning/src/global_planner/global_route.txt diff --git a/code/planning/global_planner/src/help_functions.py b/code/planning/src/global_planner/help_functions.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/src/help_functions.py rename to code/planning/src/global_planner/help_functions.py diff --git a/code/planning/global_planner/src/preplanning_trajectory.py b/code/planning/src/global_planner/preplanning_trajectory.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/src/preplanning_trajectory.py rename to code/planning/src/global_planner/preplanning_trajectory.py diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py new file mode 100755 index 00000000..3849dc6a --- /dev/null +++ b/code/planning/src/local_planner/ACC.py @@ -0,0 +1,209 @@ +#!/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 + + self.logdebug("ACC initialized") + + 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/src/local_planner/behavior_speed.py b/code/planning/src/local_planner/behavior_speed.py new file mode 100755 index 00000000..4eb9cf47 --- /dev/null +++ b/code/planning/src/local_planner/behavior_speed.py @@ -0,0 +1,66 @@ + +from collections import namedtuple + + +def convert_to_ms(speed): + return speed / 3.6 + + +Behavior = namedtuple("Behavior", ("name", "speed")) + +# Changed target_speed_pub to curr_behavior_pub + +# Intersection - Behaviors + +# Approach + +int_app_init = Behavior("int_app_init", convert_to_ms(30.0)) + +# No Traffic Light or Sign -> stop dynamically at Stopline +int_app_no_sign = Behavior("int_app_no_sign", -2) + +int_app_green = Behavior("int_app_green", convert_to_ms(30.0)) + +# Wait + +int_wait = Behavior("int_wait", 0) + +# Enter + +int_enter_no_light = Behavior("int_enter_no_light", convert_to_ms(50.0)) + +int_enter_empty_str = Behavior("int_enter_empty_string", convert_to_ms(18.0)) + +int_enter_light = Behavior("int_enter_light", convert_to_ms(50.0)) + +# Exit + +int_exit = Behavior("int_exit", -1) # Get SpeedLimit dynamically + + +# Lane Change + +# Approach + +lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0)) + + +# TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128) +lc_app_blocked = Behavior("lc_app_blocked", 0.5) + +# Wait + +# Has a publisher but doesnt publish anything ?? + +# Enter + +lc_enter_init = Behavior("lc_enter_init", convert_to_ms(20.0)) + +# Exit + +lc_exit = Behavior("lc_exit", -1) # Get SpeedLimit dynamically + + +# Cruise + +cruise = Behavior("Cruise", -1) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py new file mode 100755 index 00000000..3cc107d3 --- /dev/null +++ b/code/planning/src/local_planner/collision_check.py @@ -0,0 +1,207 @@ +#!/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 rospy import Subscriber +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 +from perception.msg import MinDistance +import time + + +class CollisionCheck(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(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.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 + self.logdebug("CollisionCheck started") + + 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): + """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): + """Calculates the meters until collision with the obstacle in front + + 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 + """ + 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 + """ + 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_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: + # 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: + # 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: + """ + self.spin() + + +if __name__ == "__main__": + """ + main function starts the CollisionCheck node + :param args: + """ + roscomp.init('CollisionCheck') + + try: + node = CollisionCheck() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() diff --git a/code/planning/src/local_planner/dev_collision_publisher.py b/code/planning/src/local_planner/dev_collision_publisher.py new file mode 100755 index 00000000..9bbd267a --- /dev/null +++ b/code/planning/src/local_planner/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/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py new file mode 100755 index 00000000..92949cad --- /dev/null +++ b/code/planning/src/local_planner/motion_planning.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python +# import rospy +# import tf.transformations +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +from rospy import Publisher, Subscriber +from std_msgs.msg import String, Float32 +import numpy as np + +# from behavior_agent.msg import BehaviorSpeed +from perception.msg import Waypoint, LaneChange +import behavior_speed as bs + +# 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 Float32MultiArray + + +def convert_to_ms(speed): + return speed / 3.6 + + +class MotionPlanning(CompatibleNode): + """ + This node selects speeds according to the behavior in the Decision Tree + and the ACC. + Later this Node should compute a local Trajectory and forward + it to the Acting. + """ + + def __init__(self): + super(MotionPlanning, self).__init__('MotionPlanning') + self.role_name = self.get_param("role_name", "hero") + self.control_loop_rate = self.get_param("control_loop_rate", 0.5) + + self.target_speed = 0.0 + self.__curr_behavior = None + self.__acc_speed = 0.0 + self.__stopline = None # (Distance, isStopline) + self.__change_point = None # (Distance, isLaneChange, roadOption) + + # Subscriber + self.curr_behavior_sub: Subscriber = self.new_subscription( + String, + f"/paf/{self.role_name}/curr_behavior", + self.__set_curr_behavior, + qos_profile=1) + + self.acc_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/acc_velocity", + self.__set_acc_speed, + qos_profile=1) + + self.stopline_sub: Subscriber = self.new_subscription( + Waypoint, + f"/paf/{self.role_name}/waypoint_distance", + self.__set_stopline, + qos_profile=1) + + self.change_point_sub: Subscriber = self.new_subscription( + LaneChange, + f"/paf/{self.role_name}/lane_change_distance", + self.__set_change_point, + qos_profile=1) + + # Publisher + self.velocity_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/target_velocity", + qos_profile=1) + + self.logdebug("MotionPlanning started") + + def update_target_speed(self, acc_speed, behavior): + be_speed = self.get_speed_by_behavior(behavior) + + self.target_speed = min(be_speed, acc_speed) + self.velocity_pub.publish(self.target_speed) + + def __set_acc_speed(self, data: Float32): + self.__acc_speed = data.data + + def __set_curr_behavior(self, data: String): + self.__curr_behavior = data.data + + def __set_stopline(self, data: Waypoint) -> float: + if data is not None: + self.__stopline = (data.distance, data.isStopLine) + + def __set_change_point(self, data: LaneChange): + if data is not None: + self.__change_point = \ + (data.distance, data.isLaneChange, data.roadOption) + + def get_speed_by_behavior(self, behavior: str) -> float: + speed = 0.0 + split = "_" + short_behavior = behavior.partition(split)[0] + if short_behavior == "int": + speed = self.__get_speed_intersection(behavior) + elif short_behavior == "lc": + speed = self.__get_speed_lanechange(behavior) + else: + speed = self.__get_speed_cruise() + + return speed + + def __get_speed_intersection(self, behavior: str) -> float: + speed = 0.0 + if behavior == bs.int_app_init.name: + speed = bs.int_app_init.speed + elif behavior == bs.int_app_green.name: + speed = bs.int_app_green.speed + elif behavior == bs.int_app_no_sign.name: + speed = self.__calc_speed_to_stop_intersection() + elif behavior == bs.int_wait.name: + speed == bs.int_wait.speed + elif behavior == bs.int_enter_no_light: + speed = bs.int_enter_no_light.speed + elif behavior == bs.int_enter_empty_str.name: + speed = bs.int_enter_empty_str.speed + elif behavior == bs.int_enter_light.name: + speed == bs.int_enter_light.speed + elif behavior == bs.int_exit: + speed = bs.int_exit.speed + + return speed + + def __get_speed_lanechange(self, behavior: str) -> float: + speed = 0.0 + if behavior == bs.lc_app_init.name: + speed = bs.lc_app_init.speed + elif behavior == bs.lc_app_blocked.name: + speed = bs.lc_app_blocked.speed # calc_speed_to_stop_lanechange() + elif behavior == bs.lc_enter_init.name: + speed = bs.lc_enter_init.speed + elif behavior == bs.lc_exit.name: + speed = bs.lc_exit.speed + + return speed + + def __get_speed_cruise(self) -> float: + return self.__acc_speed + + def __calc_speed_to_stop_intersection(self) -> float: + target_distance = 3.0 + virtual_stopline_distance = self.__calc_virtual_stopline() + # calculate speed needed for stopping + v_stop = max(convert_to_ms(10.), + convert_to_ms((virtual_stopline_distance / 30) + * 50)) + if v_stop > convert_to_ms(50.0): + v_stop = convert_to_ms(50.0) + if virtual_stopline_distance < target_distance: + v_stop = 0.0 + + # TODO: Find out purpose + def __calc_speed_to_stop_lanechange(self) -> float: + if self.__change_point[0] != np.inf and self.__change_point[1]: + stopline = self.__change_point[0] + else: + return 100 + + v_stop = max(convert_to_ms(5.), + convert_to_ms((stopline / 30) ** 1.5 + * 50)) + if v_stop > convert_to_ms(50.0): + v_stop = convert_to_ms(30.0) + return v_stop + + def __calc_virtual_stopline(self) -> float: + if self.__stopline[0] != np.inf and self.__stopline[1]: + return self.__stopline[0] + elif self.traffic_light_detected: + return self.traffic_light_distance + else: + return 0.0 + + def run(self): + """ + Control loop + :return: + """ + + def loop(timer_event=None): + self.update_target_speed(self.__acc_speed, self.__curr_behavior) + + self.new_timer(self.control_loop_rate, loop) + self.spin() + + +if __name__ == "__main__": + """ + main function starts the MotionPlanning node + :param args: + """ + roscomp.init('MotionPlanning') + + try: + node = MotionPlanning() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() diff --git a/code/requirements.txt b/code/requirements.txt index c0e55aca..1263bdf7 100644 --- a/code/requirements.txt +++ b/code/requirements.txt @@ -13,3 +13,4 @@ xmltodict==0.13.0 py-trees==2.2.3 numpy==1.23.5 ultralytics==8.0.220 +scikit-learn>=0.18 \ No newline at end of file diff --git a/doc/00_assets/3d_2d_formula.png b/doc/00_assets/3d_2d_formula.png new file mode 100644 index 00000000..2fc1e902 Binary files /dev/null and b/doc/00_assets/3d_2d_formula.png differ diff --git a/doc/00_assets/3d_2d_projection.png b/doc/00_assets/3d_2d_projection.png new file mode 100644 index 00000000..fe0b6ae6 Binary files /dev/null and b/doc/00_assets/3d_2d_projection.png differ diff --git a/doc/03_research/03_planning/00_paf23/BT_paper.png b/doc/00_assets/planning/BT_paper.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/BT_paper.png rename to doc/00_assets/planning/BT_paper.png diff --git a/doc/03_research/03_planning/00_paf23/BehaviorTree_medium.png b/doc/00_assets/planning/BehaviorTree_medium.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/BehaviorTree_medium.png rename to doc/00_assets/planning/BehaviorTree_medium.png diff --git a/doc/03_research/03_planning/00_paf23/Globalplan.png b/doc/00_assets/planning/Globalplan.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/Globalplan.png rename to doc/00_assets/planning/Globalplan.png diff --git a/doc/03_research/03_planning/00_paf23/Planning.png b/doc/00_assets/planning/Planning.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/Planning.png rename to doc/00_assets/planning/Planning.png diff --git a/doc/03_research/03_planning/00_paf23/Planning_paf21.png b/doc/00_assets/planning/Planning_paf21.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/Planning_paf21.png rename to doc/00_assets/planning/Planning_paf21.png diff --git a/doc/03_research/03_planning/00_paf23/intersection_scenario.png b/doc/00_assets/planning/intersection_scenario.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/intersection_scenario.png rename to doc/00_assets/planning/intersection_scenario.png diff --git a/doc/03_research/03_planning/00_paf23/localplan.png b/doc/00_assets/planning/localplan.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/localplan.png rename to doc/00_assets/planning/localplan.png diff --git a/doc/03_research/03_planning/00_paf23/overtaking_scenario.png b/doc/00_assets/planning/overtaking_scenario.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/overtaking_scenario.png rename to doc/00_assets/planning/overtaking_scenario.png diff --git a/doc/03_research/03_planning/00_paf23/overview.jpg b/doc/00_assets/planning/overview.jpg similarity index 100% rename from doc/03_research/03_planning/00_paf23/overview.jpg rename to doc/00_assets/planning/overview.jpg diff --git a/doc/03_research/03_planning/00_paf23/overview.png b/doc/00_assets/planning/overview.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/overview.png rename to doc/00_assets/planning/overview.png diff --git a/doc/03_research/03_planning/00_paf23/overview_paper1.png b/doc/00_assets/planning/overview_paper1.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/overview_paper1.png rename to doc/00_assets/planning/overview_paper1.png diff --git a/doc/03_research/03_planning/00_paf23/prios.png b/doc/00_assets/planning/prios.png similarity index 100% rename from doc/03_research/03_planning/00_paf23/prios.png rename to doc/00_assets/planning/prios.png diff --git a/doc/06_perception/09_paf_21_1_perception.md b/doc/03_research/02_perception/06_paf_21_1_perception.md similarity index 100% rename from doc/06_perception/09_paf_21_1_perception.md rename to doc/03_research/02_perception/06_paf_21_1_perception.md diff --git a/doc/03_research/02_perception/Readme.md b/doc/03_research/02_perception/Readme.md index eef46a2e..051c9dd0 100644 --- a/doc/03_research/02_perception/Readme.md +++ b/doc/03_research/02_perception/Readme.md @@ -7,4 +7,5 @@ This folder contains all the results of research on perception: * [First implementation plan](./03_first_implementation_plan.md) * **PAF23** * [Pylot Perception](./04_pylot.md) - * [PAF_21_1 Perception](./05_Research_PAF21-Perception.md) + * [PAF_21_2 Perception](./05_Research_PAF21-Perception.md) + * [PAF_21_1_Perception](./06_paf_21_1_perception.md) diff --git a/doc/03_research/03_planning/00_paf23/01_Planning.md b/doc/03_research/03_planning/00_paf23/01_Planning.md index da344b00..a36283ac 100644 --- a/doc/03_research/03_planning/00_paf23/01_Planning.md +++ b/doc/03_research/03_planning/00_paf23/01_Planning.md @@ -6,7 +6,7 @@ Finding the optimal path from start to goal, taking into account the static and ### [PAF21 - 2](https://github.com/ll7/paf21-2) -![Planning](Planning_paf21.png) +![Planning](../../../00_assets/planning/Planning_paf21.png) Input: @@ -55,7 +55,7 @@ Map Manager ### [Autoware](https://github.com/autowarefoundation/autoware) -![https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/](Planning.png) +![https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/](../../../00_assets/planning/Planning.png) Input: diff --git a/doc/03_research/03_planning/00_paf23/02_PlanningPaf22.md b/doc/03_research/03_planning/00_paf23/02_PlanningPaf22.md index a28289f8..605605e7 100644 --- a/doc/03_research/03_planning/00_paf23/02_PlanningPaf22.md +++ b/doc/03_research/03_planning/00_paf23/02_PlanningPaf22.md @@ -4,7 +4,7 @@ ## Architecture -![overview](overview.jpg) +![overview](../../../00_assets/planning/overview.jpg) ### Preplanning diff --git a/doc/03_research/03_planning/00_paf23/architecture.md b/doc/03_research/03_planning/00_paf23/03_PlannedArchitecture.md similarity index 91% rename from doc/03_research/03_planning/00_paf23/architecture.md rename to doc/03_research/03_planning/00_paf23/03_PlannedArchitecture.md index e85c7947..172cb9ca 100644 --- a/doc/03_research/03_planning/00_paf23/architecture.md +++ b/doc/03_research/03_planning/00_paf23/03_PlannedArchitecture.md @@ -4,7 +4,7 @@ Provide an overview for a possible planning architecture consisting of Global P ## Overview -![overview](overview.png) +![overview](../../../00_assets/planning/overview.png) The **Global Plan** gathers all data relevant to build a copy of the town the car is driving in. It also computes an optimal global path, which includes all waypoints. The Decision Making can order a recalculation of the global path. @@ -19,7 +19,7 @@ Motions like lane changing must be approved by the decision making and they get ### Global Plan -![overview](Globalplan.png) +![overview](../../../00_assets/planning/Globalplan.png) *Map Generator:* Gathers map data from Carla and prepares it for the PrePlanner @@ -69,7 +69,7 @@ See Behaviour Tree. ### Local Plan -![Local Plan](localplan.png) +![Local Plan](../../../00_assets/planning/localplan.png) *Local Preplan:* Segements the global path and calculates the middle of the lane. Is not called in every cycle. @@ -128,4 +128,4 @@ See Behaviour Tree. Red must have for next Milestone, Orange needed for future milestones, Green can already be used or is not that important -![prios](prios.png) +![prios](../../../00_assets/planning/prios.png) diff --git a/doc/03_research/03_planning/00_paf23/Local_planning_for_first_milestone.md b/doc/03_research/03_planning/00_paf23/04_Local_planning_for_first_milestone.md similarity index 82% rename from doc/03_research/03_planning/00_paf23/Local_planning_for_first_milestone.md rename to doc/03_research/03_planning/00_paf23/04_Local_planning_for_first_milestone.md index e47f05eb..431ffee9 100644 --- a/doc/03_research/03_planning/00_paf23/Local_planning_for_first_milestone.md +++ b/doc/03_research/03_planning/00_paf23/04_Local_planning_for_first_milestone.md @@ -16,7 +16,7 @@ Julius Miller Paper: [Behavior Planning for Autonomous Driving: Methodologies, Applications, and Future Orientation](https://www.researchgate.net/publication/369181112_Behavior_Planning_for_Autonomous_Driving_Methodologies_Applications_and_Future_Orientation) -![Overview_interfaces](overview_paper1.png) +![Overview_interfaces](../../../00_assets/planning/overview_paper1.png) Rule-based planning @@ -49,7 +49,7 @@ Leader, Track-Speed Github: [Decision Making with Behaviour Tree](https://github.com/kirilcvetkov92/Path-planning?source=post_page-----8db1575fec2c--------------------------------) -![github_tree](BehaviorTree_medium.png) +![github_tree](../../../00_assets/planning/BehaviorTree_medium.png) - No Intersection - Collision Detection in behaviour Tree @@ -58,7 +58,7 @@ Paper: [Behavior Trees for decision-making in Autonomous Driving](https://www.diva-portal.org/smash/get/diva2:907048/FULLTEXT01.pdf) -![Behaviour Tree](BT_paper.png) +![Behaviour Tree](../../../00_assets/planning/BT_paper.png) - simple simulation - Car only drives straight @@ -81,17 +81,17 @@ Low Level Decision: - Emergency Brake - ACC -![localplan](localplan.png) +![localplan](../../../00_assets/planning/localplan.png) Scenarios: -![Intersection](intersection_scenario.png) +![Intersection](../../../00_assets/planning/intersection_scenario.png) Left: Behaviour Intersection is triggered for motion planning, acc publishes speed. -> Lower speed is used to approach intersection Right: Behaviour Intersection is used for motion planning, acc is ignored (no object in front) -![Overtake](overtaking_scenario.png) +![Overtake](../../../00_assets/planning/overtaking_scenario.png) Left: Overtake gets triggered to maintain speed, acc is ignored 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/06_perception/6-autoware-perception.md b/doc/06_perception/05-autoware-perception.md similarity index 100% rename from doc/06_perception/6-autoware-perception.md rename to doc/06_perception/05-autoware-perception.md diff --git a/doc/06_perception/07_vision_node.md b/doc/06_perception/06_vision_node.md similarity index 100% rename from doc/06_perception/07_vision_node.md rename to doc/06_perception/06_vision_node.md diff --git a/doc/06_perception/10_sensor_filter_debug.md b/doc/06_perception/07_sensor_filter_debug.md similarity index 95% rename from doc/06_perception/10_sensor_filter_debug.md rename to doc/06_perception/07_sensor_filter_debug.md index 83307e48..d3ab8932 100644 --- a/doc/06_perception/10_sensor_filter_debug.md +++ b/doc/06_perception/07_sensor_filter_debug.md @@ -1,6 +1,6 @@ # sensor_filter_debug.py -**Summary:** [sensor_filter_debug.py](.../code/perception/src/sensor_filter_debug.py): +**Summary:** [sensor_filter_debug.py](../../code/perception/src/sensor_filter_debug.py): The sensor_filter_debug node is responsible for collecting sensor data from the IMU and GNSS and process the data in such a way, that it shows the errors between the real is-state and the measured state. The data is the shown in multiple rqt_plots. @@ -33,9 +33,9 @@ Robert Fischer ## Getting started -Uncomment the sensor_filter_debug.py node in the [perception.launch](.../code/perception/launch/perception.launch) to start the node. +Uncomment the sensor_filter_debug.py node in the [perception.launch](../../code/perception/launch/perception.launch) to start the node. You can also uncomment the rqt_plots that seem useful to you, or create your own ones from the data published. -You have to add the following sensors to the sensors inside the [dev_objects.json](.../code/agent/config/dev_objects.json): +You have to add the following sensors to the sensors inside the [dev_objects.json](../../code/agent/config/dev_objects.json): ```json { @@ -101,7 +101,7 @@ Right now only the IMU and the GNSS sensor are available for debug. Debug for the RADAR and LIDAR hasn't been implemented yet. An Example of Location Error Output can be seen here: -![Distance from current_pos to ideal_gps_pos (blue) and to carla_pos (red)](.../doc/00_assets/gnss_ohne_rolling_average.png) +![Distance from current_pos to ideal_gps_pos (blue) and to carla_pos (red)](../00_assets/gnss_ohne_rolling_average.png) ### Inputs diff --git a/doc/06_perception/11_kalman_filter.md b/doc/06_perception/08_kalman_filter.md similarity index 95% rename from doc/06_perception/11_kalman_filter.md rename to doc/06_perception/08_kalman_filter.md index 8eadb14b..7ba78703 100644 --- a/doc/06_perception/11_kalman_filter.md +++ b/doc/06_perception/08_kalman_filter.md @@ -1,6 +1,6 @@ # Kalman Filter -**Summary:** [kalman_filter.py](.../code/perception/src/kalman_filter.py): +**Summary:** [kalman_filter.py](../../code/perception/src/kalman_filter.py): The Kalman Filter node is responsible for filtering the location and heading data, by using an IMU and GNSS sensor together with the carla speedometer. @@ -38,7 +38,7 @@ Robert Fischer Right now the Node does not work correctly. It creates topics to publish to, but doesn't yet. This will be fixed in [#106](https://github.com/una-auxme/paf23/issues/106) -Uncomment the kalman_filter.py node in the [perception.launch](.../code/perception/launch/perception.launch) to start the node. +Uncomment the kalman_filter.py node in the [perception.launch](../../code/perception/launch/perception.launch) to start the node. You can also uncomment the rqt_plots that seem useful to you. No extra installation needed. @@ -60,7 +60,7 @@ Stackoverflow and other useful sites: [4](https://github.com/Janudis/Extended-Kalman-Filter-GPS_IMU) This script implements a Kalman Filter. It is a recursive algorithm used to estimate the state of a system that can be modeled with linear equations. -This Kalman Filter uses the location provided by a GNSS sensor (by using the current_pos provided by the [Position Publisher Node](.../code/perception/src/Position_Publisher_Node.py)) +This Kalman Filter uses the location provided by a GNSS sensor (by using the current_pos provided by the [Position Publisher Node](../../code/perception/src/Position_Publisher_Node.py)) the orientation and angular velocity provided by the IMU sensor and the current speed in the headed direction by the Carla Speedometer. ```Python diff --git a/doc/06_perception/12_distance_to_objects.md b/doc/06_perception/12_distance_to_objects.md new file mode 100644 index 00000000..59863390 --- /dev/null +++ b/doc/06_perception/12_distance_to_objects.md @@ -0,0 +1,87 @@ +# Getting the Distance to Objects + +Using the vision node and the lidar distance node we can calculate the distance of detected objects. +We can solve this problem from two directions mapping either pixel into the 3D-World or mapping 3D-LidarPoints into Pixel. + +This file will first discuss a simple Minimum Distance Publisher and will than explain the mapping of 3D-Points into 2D. + +## Minimum Distance + +This part of the node analysis the LIDAR-Data in a small corridor right in front of the vehicle. +This corridor is similar to what is explained in 03_lidar_distance_utility.md + +We retrieve the (X, Y, Z) coordinates of every LIDAR-Point in this corridor. +We than check for the nearest cluster of Points, where there are at least 5 LIDAR-Points within a Range of 0.2 Meters. +That way we dont accidently classify a sensor error or a tiny object as an obstacle. + +We publish a 3D-Graph of the analysis on the topic: "/paf/hero/Center/min_dist_image" + +## Converting 3D-Points into 2D-Camera-Space + +As said before we can try this from both directions. I chose to do this starting with the LIDAR-Data. +There are two reaons for this. + +**1. Resolution** +The resolution of the LIDAR-Sensor is not as high as the Camera Resolution. +This means, that not every Pixel in the Camera-Image will have a corresponding Data-Point in the LIDAR-Pointcloud. + +**2. Math** +The literature for Camera-Calibration proposes a way to calculate Pixel from a 3D-World Coordinate System. +You can only partially invert this formula, since it involves calculation the invers matrix for a matrix that isnt square. + +I found ways online, that seemed to solve this issue though. + +### Concept + +![3d_2d_porjection](../00_assets/3d_2d_projection.png) + +The goal is to calculate the projection of point P and find its Pixl-Coordinates (u,v) on the Image-Plain. +To do this you need a couple of thins: + +1. Camera-Calibration + 1. Width + 2. Height + 3. Field-of-View +2. LIDAR-Sensor-Position + 1. Origin of 3D-World-Coordinates +3. Camera-Sensor + 1. Position + 2. Orientation + +The formula for this projection proposed by the literature looks like this: + +![3d_2d_formula](../00_assets/3d_2d_formula.png) + +To get the camera-intrinsic matrix we need the width, height and fov of the image produced by the camera. +Luckily we cn easly get these values from the sensor configuration in (agent.py) + +In our case we use the following configuration: Width: 300, Height: 200, FOV: 100 + +The Intrinsic Matrix is calculated within the code using these values and a piece of code from pyLot (insert link) + +Next up we need the extrinsic Camera-Matrix. We can set this Matrix to the Identity-Matrix, if both LIDAR and Camera are in the exact same position (e.g (0,0, 0)) in the world. + +### Purpose + +**Why is this usefull?** + +Using this projection we can reconstruct the camera image using the lidar points. The result is a 2D-Image that has the exact same Dimensions as the Camera-Output. +We can now use this Image in Combination with the Object-Detection to find the distance for every Object. + +U can imageine putting both images on top of each other. + +### Implementation + +All that is left now is the calculation of the Pixel-Coordinates for every LIDAR-Point in front of the vehicle and some nice Visualization. + +LIDAR-Points that are not within the field of view of the camera would be projected to (u, v)-Points that dont match the Camera-Image +(for example: (-100, 23)) + +To visualize the reconstruced Image we create a cv2 Image where the RGB-Values a mapped to a rainbox-color scheme according to the distance at each pixel. + +insert image + +## Next Steps + +- Combine Object-Detection with LIDAR-Reconstruction to get a depth image +- Provide List of Objects with distance in a publisher diff --git a/doc/06_perception/13_traffic_light_detection.md b/doc/06_perception/13_traffic_light_detection.md new file mode 100644 index 00000000..c627e3cf --- /dev/null +++ b/doc/06_perception/13_traffic_light_detection.md @@ -0,0 +1,37 @@ +# Traffic Light Detection + +## Vision Node + +For each analyzed image, it is checked whether an object with the ID=9 (traffic light) is detected. +If that is the case, `process_traffic_lights()` is called which applies the bounding box of the predicition to cut out the found object (e.g. traffic light). + +Only if the object is within `min_x`, `max_x` and `min_prob` (probability), it will be published to `"/paf/{self.role_name}/{self.side}/segmented_traffic_light"`. + +## TrafficLightNode + +The `traffic_light_node.py` file is part of a larger system that handles traffic light detection. It contains a class `TrafficLightNode` that extends from `CompatibleNode`. + +This class is responsible for setting up the traffic light detection system and handling the incoming camera images. + +### Initialization + +The `TrafficLightNode` class is initialized with a name and additional keyword arguments. During initialization, it sets up the following: + +- A `CvBridge` instance for converting between ROS image messages and OpenCV images. +- The role name and side, which are parameters that can be set externally. +- A `TrafficLightInference` instance for performing traffic light detection. + +### Methods + +#### `setup_camera_subscriptions()` + +This method sets up a subscription to the camera images. It subscribes to the topic `"/paf/{self.role_name}/{self.side}/segmented_traffic_light"` and calls the `handle_camera_image` method whenever a new image message is received. + +#### `setup_traffic_light_publishers()` + +This method sets up a publisher for the traffic light state. It publishes to the topic `"/paf/{self.role_name}/{self.side}/traffic_light_state"` in the format of `TrafficLightState.msg` which uses an int8-based enum for the traffic light state. + +#### `handle_camera_image(image)` + +This method is called whenever a new image message is received. It performs traffic light detection by using `traffic_light_inference.py` on the image and publishes the result. +The result is a `TrafficLightState` message where the state is set to the detected traffic light state (1 for green, 2 for red, 4 for yellow, 0 for unknown). diff --git a/doc/06_perception/Readme.md b/doc/06_perception/Readme.md index f92978c3..4147f1dc 100644 --- a/doc/06_perception/Readme.md +++ b/doc/06_perception/Readme.md @@ -4,6 +4,16 @@ This folder contains further documentation of the perception components. ## 1. [Dataset generator](./01_dataset_generator.md) -## 10. [sensor_filter_debug.py](./10_sensor_filter_debug.md) +## 2. [Dataset Structure](./02_dataset_structure.md) -## 11. [Kalman Filter](./11_kalman_filter.md) +## 3. [Lidar Distance Utility](./03_lidar_distance_utility.md) + +## 4. [Efficient PS](./04_efficientps.md) + +## 5. [Autoware Perception](./05-autoware-perception.md) + +## 6. [Vision Node](./06_vision_node.md) + +## 7. [sensor_filter_debug.py](./07_sensor_filter_debug.md) + +## 8. [Kalman Filter](./08_kalman_filter.md) diff --git a/doc/06_perception/experiments/model_evaluation/README.md b/doc/06_perception/experiments/object-detection-model_evaluation/README.md similarity index 100% rename from doc/06_perception/experiments/model_evaluation/README.md rename to doc/06_perception/experiments/object-detection-model_evaluation/README.md diff --git a/doc/06_perception/experiments/model_evaluation/asset-copies/1619_PT_fasterrcnn_resnet50_fpn_v2.jpg b/doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_PT_fasterrcnn_resnet50_fpn_v2.jpg similarity index 100% rename from doc/06_perception/experiments/model_evaluation/asset-copies/1619_PT_fasterrcnn_resnet50_fpn_v2.jpg rename to doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_PT_fasterrcnn_resnet50_fpn_v2.jpg diff --git a/doc/06_perception/experiments/model_evaluation/asset-copies/1619_TF_faster-rcnn.jpg b/doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_TF_faster-rcnn.jpg similarity index 100% rename from doc/06_perception/experiments/model_evaluation/asset-copies/1619_TF_faster-rcnn.jpg rename to doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_TF_faster-rcnn.jpg diff --git a/doc/06_perception/experiments/model_evaluation/asset-copies/1619_yolo_nas_l.jpg b/doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_yolo_nas_l.jpg similarity index 100% rename from doc/06_perception/experiments/model_evaluation/asset-copies/1619_yolo_nas_l.jpg rename to doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_yolo_nas_l.jpg diff --git a/doc/06_perception/experiments/model_evaluation/asset-copies/1619_yolo_rtdetr_x.jpg b/doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_yolo_rtdetr_x.jpg similarity index 100% rename from doc/06_perception/experiments/model_evaluation/asset-copies/1619_yolo_rtdetr_x.jpg rename to doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_yolo_rtdetr_x.jpg diff --git a/doc/06_perception/experiments/model_evaluation/asset-copies/1619_yolov8x.jpg b/doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_yolov8x.jpg similarity index 100% rename from doc/06_perception/experiments/model_evaluation/asset-copies/1619_yolov8x.jpg rename to doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_yolov8x.jpg diff --git a/doc/06_perception/experiments/model_evaluation/asset-copies/1619_yolov8x_seg.jpg b/doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_yolov8x_seg.jpg similarity index 100% rename from doc/06_perception/experiments/model_evaluation/asset-copies/1619_yolov8x_seg.jpg rename to doc/06_perception/experiments/object-detection-model_evaluation/asset-copies/1619_yolov8x_seg.jpg diff --git a/doc/06_perception/experiments/model_evaluation/globals.py b/doc/06_perception/experiments/object-detection-model_evaluation/globals.py similarity index 100% rename from doc/06_perception/experiments/model_evaluation/globals.py rename to doc/06_perception/experiments/object-detection-model_evaluation/globals.py diff --git a/doc/06_perception/experiments/model_evaluation/pt.py b/doc/06_perception/experiments/object-detection-model_evaluation/pt.py similarity index 100% rename from doc/06_perception/experiments/model_evaluation/pt.py rename to doc/06_perception/experiments/object-detection-model_evaluation/pt.py diff --git a/doc/06_perception/experiments/model_evaluation/pylot.py b/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py similarity index 100% rename from doc/06_perception/experiments/model_evaluation/pylot.py rename to doc/06_perception/experiments/object-detection-model_evaluation/pylot.py diff --git a/doc/06_perception/experiments/model_evaluation/requirements.txt b/doc/06_perception/experiments/object-detection-model_evaluation/requirements.txt similarity index 100% rename from doc/06_perception/experiments/model_evaluation/requirements.txt rename to doc/06_perception/experiments/object-detection-model_evaluation/requirements.txt diff --git a/doc/06_perception/experiments/model_evaluation/yolo.py b/doc/06_perception/experiments/object-detection-model_evaluation/yolo.py similarity index 81% rename from doc/06_perception/experiments/model_evaluation/yolo.py rename to doc/06_perception/experiments/object-detection-model_evaluation/yolo.py index f7ff342d..39d727b7 100644 --- a/doc/06_perception/experiments/model_evaluation/yolo.py +++ b/doc/06_perception/experiments/object-detection-model_evaluation/yolo.py @@ -1,5 +1,8 @@ ''' -Docs: https://docs.ultralytics.com/modes/predict/, https://docs.ultralytics.com/tasks/detect/#models, https://docs.ultralytics.com/models/yolo-nas +Docs: +https://docs.ultralytics.com/modes/predict/ +https://docs.ultralytics.com/tasks/detect/#models +https://docs.ultralytics.com/models/yolo-nas ''' import os @@ -35,6 +38,7 @@ image_path = os.path.join(IMAGE_BASE_FOLDER, IMAGES_FOR_TEST[p]) img = Image.open(image_path) - _ = model.predict(source=img, save=True, save_conf=True, line_width=1, half=True) + _ = model.predict(source=img, save=True, save_conf=True, + line_width=1, half=True) del model diff --git a/doc/06_perception/experiments/traffic-light-detection_evaluation/README.md b/doc/06_perception/experiments/traffic-light-detection_evaluation/README.md new file mode 100644 index 00000000..46e8e0af --- /dev/null +++ b/doc/06_perception/experiments/traffic-light-detection_evaluation/README.md @@ -0,0 +1,43 @@ +# Evaluation of the PAF22 Traffic Light Detection + +In this experiment, the existing Traffic Light Detection from PAF22 has been tested. +The goals was to be able to verify, that it is suitable for PAF23. + +## Model + +The architecture of the model is a Convolutional Neural Network (CNN) and it consists of the following layers: + +1. **Convolutional Layer 1**: This layer uses a 2D convolution over an input signal composed of several input planes, with in_channels input channels, 4 output channels, a kernel size of 5, and padding set to 'same'. This means the output size is the same as the input size. +2. **Batch Normalization Layer**: This layer applies Batch Normalization over a 4D input (a mini-batch of 2D inputs with additional channel dimension) as described in the paper Batch Normalization: Accelerating Deep Network Training by Reducing Internal Covariate Shift. +3. **Convolutional Layer 2**: This layer is similar to the first convolutional layer but it takes the output of the first layer (4 channels) as input. +4. **Max Pooling Layer 1**: This layer uses a 2D max pooling over an input signal composed of several input planes, with a kernel size of (2, 2). +5. **Convolutional Layer 3**: This layer is similar to the previous convolutional layers but it has a kernel size of 3. +6. **Max Pooling Layer 2**: This layer is similar to the first max pooling layer. +7. **Convolutional Layer 4**: This layer is similar to the previous convolutional layers. +8. **Max Pooling Layer 3**: This layer is similar to the previous max pooling layers. +9. **Flatten Layer**: This layer flattens the input by removing the spatial dimensions. +10. **Dropout Layer**: This layer randomly zeroes some of the elements of the input tensor with probability p=0.3 using samples from a Bernoulli distribution. +11. **Linear Layer**: This layer applies a linear transformation to the incoming data. It has 64 input features and num_classes output features. + +## Dataset + +The existing dataset of PAF22 consists of 2340 images (combined) of the categories red, yellow, green, backside. There are also 382 validation images (combined). + +The data can be accessed through DVC. + +## Training + +Running the training with `dvc exp run` in the traffic light detection directory, results in a trained model with >99% accuracy & validation. + +## Examples + +Result | Large | Small | +|-----------|----------|----------| +Green | ![Green-Large](assets/green_4.png) | ![Green-Small](assets/green_22.jpg) +Yellow | ![Yellow-Large](assets/yellow_1.png) | ![Yellow-Small](assets/yellow_18.jpg) +Red | ![Red-Large](assets/red_10.png) | ![Red-Small](assets/red_20.png) +Back | ![Back-Large](assets/back_1.png) | ![Back-Small](assets/back_14.jpg) + +## Verdict + +The high accuracy and manual testing of the above example images verified, that the existing PAF22 traffic light detection model can be used for PAF23. diff --git a/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/back_1.png b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/back_1.png new file mode 100644 index 00000000..b0a6a2f6 Binary files /dev/null and b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/back_1.png differ diff --git a/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/back_14.jpg b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/back_14.jpg new file mode 100644 index 00000000..8fa865b1 Binary files /dev/null and b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/back_14.jpg differ diff --git a/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/green_22.jpg b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/green_22.jpg new file mode 100644 index 00000000..5df96d68 Binary files /dev/null and b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/green_22.jpg differ diff --git a/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/green_4.png b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/green_4.png new file mode 100644 index 00000000..a65fccf8 Binary files /dev/null and b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/green_4.png differ diff --git a/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/red_10.png b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/red_10.png new file mode 100644 index 00000000..c7192ae2 Binary files /dev/null and b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/red_10.png differ diff --git a/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/red_20.png b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/red_20.png new file mode 100644 index 00000000..365e7529 Binary files /dev/null and b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/red_20.png differ diff --git a/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/yellow_1.png b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/yellow_1.png new file mode 100644 index 00000000..b39e5182 Binary files /dev/null and b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/yellow_1.png differ diff --git a/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/yellow_18.jpg b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/yellow_18.jpg new file mode 100644 index 00000000..12881d31 Binary files /dev/null and b/doc/06_perception/experiments/traffic-light-detection_evaluation/assets/yellow_18.jpg differ 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. diff --git a/doc/07_planning/motion_planning.md b/doc/07_planning/motion_planning.md new file mode 100644 index 00000000..175fbb25 --- /dev/null +++ b/doc/07_planning/motion_planning.md @@ -0,0 +1,56 @@ +# Motion Planning + +**Summary:** [motion_planning.py](.../code/planning/local_planner/src/motion_planning.py): +The motion planning is responsible for collecting all the speeds from the different components and choosing the optimal one to be fowarded into the acting. + +--- + +## Author + +Julius Miller + +## Date + +17.12.2023 + +## Prerequisite + +--- + +- [Motion Planning](#motion-planning) + - [Author](#author) + - [Date](#date) + - [Prerequisite](#prerequisite) + - [Description](#description) + - [Inputs](#inputs) + - [Outputs](#outputs) + + +--- + +## Description + +This node currently gathers the behavior speed and the acc_speed and chooses the lower one to be forwarded to the acting. +At the moment this is enough since the only present behaviors are Intersection, Lane Change and Cruise. + +When the Overtaking behavior will be added, choosing the minimum speed will not be sufficient. + +### Inputs + +This node subscribes to the following topics: + +- Current Behavior: + - `/paf/{role_name}/curr_behavior` (String) +- ACC Velocity: + - `/paf/{role_name}/acc_velocity` (Float32) +- Waypoint: + - `/paf/{role_name}/waypoint_distance` (Waypoint) +- Lane Change: + - `/paf/{role_name}/lane_change_distance` (LaneChange) + +### Outputs + +This node publishes the following topics: + +- Target Velocity + - `/paf/{role_name}/target_velocity` (Float32)