From a2e9cfa87347937adcd34119619b9b4abe1d7d9c Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Tue, 21 Nov 2023 15:21:05 +0100 Subject: [PATCH 01/53] feat: Set world to developer-enviroment --- build/docker-compose.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index baf7d008..fd0e7069 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -54,8 +54,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && 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/ && 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: From b585e473090dc4635a7efbffcc0673c143613889 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Tue, 21 Nov 2023 18:03:59 +0100 Subject: [PATCH 02/53] feat: Modified project for Acting testing --- code/acting/launch/acting.launch | 20 +++-- code/acting/src/acting/max_velocity_Dummy.py | 73 +++++++++++++++++++ code/acting/src/acting/velocity_controller.py | 10 ++- code/agent/config/rviz_config.rviz | 2 +- code/perception/launch/perception.launch | 3 +- 5 files changed, 96 insertions(+), 12 deletions(-) create mode 100755 code/acting/src/acting/max_velocity_Dummy.py diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index dc276494..0a647e96 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -2,7 +2,7 @@ - + @@ -45,13 +45,21 @@ - + - + - + + + + + + + diff --git a/code/acting/src/acting/max_velocity_Dummy.py b/code/acting/src/acting/max_velocity_Dummy.py new file mode 100755 index 00000000..18475c46 --- /dev/null +++ b/code/acting/src/acting/max_velocity_Dummy.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python +import math +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +from rospy import Publisher, Subscriber +from std_msgs.msg import Float32 + +MAX_VELOCITY: float = 20.0 +STEERING: float = 0.0 + +class DummyVelocityPublisher(CompatibleNode): + """ + This node publishes a constant max_velocity for Debugging and Parametertuning. + """ + + def __init__(self): + super(DummyVelocityPublisher, self).__init__('dummy_const_vel_pub') + self.control_loop_rate = self.get_param('control_loop_rate', 0.1) + self.role_name = self.get_param('role_name', 'ego_vehicle') + + self.velocity_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/max_velocity", + qos_profile=1) + + self.stanley_steer_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/stanley_steer", + qos_profile=1) + + self.pure_pursuit_steer_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/pure_pursuit_steer", + qos_profile=1) + + + def run(self): + """ + Starts the main loop of the node + :return: + """ + self.loginfo('DUMMY_constant-velocity_publisher node running') + def loop(timer_event=None): + """ + Publishes velocity limits calculated in acting based on + upcoming curves + :param timer_event: Timer event from ROS + :return: + """ + self.velocity_pub.publish(MAX_VELOCITY) + self.stanley_steer_pub.publish(STEERING) + self.pure_pursuit_steer_pub.publish(STEERING) + + self.new_timer(self.control_loop_rate, loop) + self.spin() + +def main(args=None): + """ + Main function starts the node + :param args: + """ + roscomp.init('dummy_const_vel_pub', args=args) + try: + node = DummyVelocityPublisher() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() + + +if __name__ == '__main__': + main() diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 39882333..8b334336 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -113,13 +113,13 @@ def loop(timer_event=None): f"default value {SPEED_LIMIT_DEFAULT}") # return self.__max_velocity = SPEED_LIMIT_DEFAULT - + if self.__current_velocity is None: self.logdebug("VehicleController 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 " "trajectory yet and can therefore not" @@ -142,8 +142,10 @@ def loop(timer_event=None): self.logerr("Velocity controller doesn't support backward " "driving yet.") return - v = min(self.__max_velocity, self.__max_tree_v) - v = min(v, self.__speed_limit) + """ + #v = min(self.__max_velocity, self.__max_tree_v) + #v = min(v, self.__speed_limit) + v = self.__max_velocity pid.setpoint = v throttle = pid(self.__current_velocity) diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 60da1674..b6d02247 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -149,7 +149,7 @@ Visualization Manager: Offset: X: 0 Y: 0 - Z: 39 + Z: 0 #39 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index bc8c8f9c..2b5f69d8 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -2,7 +2,7 @@ - + From 2c6c37d16cba38997a6d63e1eb5ce7383baa04f1 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Tue, 21 Nov 2023 18:06:25 +0100 Subject: [PATCH 03/53] feat: Fixes for Lintercheck --- code/acting/src/acting/max_velocity_Dummy.py | 26 +++++++++++-------- code/acting/src/acting/velocity_controller.py | 6 ++--- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/code/acting/src/acting/max_velocity_Dummy.py b/code/acting/src/acting/max_velocity_Dummy.py index 18475c46..5f073756 100755 --- a/code/acting/src/acting/max_velocity_Dummy.py +++ b/code/acting/src/acting/max_velocity_Dummy.py @@ -1,16 +1,18 @@ #!/usr/bin/env python -import math + import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode -from rospy import Publisher, Subscriber +from rospy import Publisher from std_msgs.msg import Float32 MAX_VELOCITY: float = 20.0 STEERING: float = 0.0 + class DummyVelocityPublisher(CompatibleNode): """ - This node publishes a constant max_velocity for Debugging and Parametertuning. + This node publishes a constant max_velocity + for Debugging and Parametertuning. """ def __init__(self): @@ -22,17 +24,16 @@ def __init__(self): Float32, f"/paf/{self.role_name}/max_velocity", qos_profile=1) - + self.stanley_steer_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/stanley_steer", - qos_profile=1) + Float32, + f"/paf/{self.role_name}/stanley_steer", + qos_profile=1) self.pure_pursuit_steer_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/pure_pursuit_steer", - qos_profile=1) - + Float32, + f"/paf/{self.role_name}/pure_pursuit_steer", + qos_profile=1) def run(self): """ @@ -40,6 +41,7 @@ def run(self): :return: """ self.loginfo('DUMMY_constant-velocity_publisher node running') + def loop(timer_event=None): """ Publishes velocity limits calculated in acting based on @@ -54,11 +56,13 @@ def loop(timer_event=None): self.new_timer(self.control_loop_rate, loop) self.spin() + def main(args=None): """ Main function starts the node :param args: """ + roscomp.init('dummy_const_vel_pub', args=args) try: node = DummyVelocityPublisher() diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 8b334336..6fb44d72 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -113,7 +113,7 @@ def loop(timer_event=None): f"default value {SPEED_LIMIT_DEFAULT}") # return self.__max_velocity = SPEED_LIMIT_DEFAULT - + if self.__current_velocity is None: self.logdebug("VehicleController hasn't received " "current_velocity yet and can therefore not" @@ -143,8 +143,8 @@ def loop(timer_event=None): "driving yet.") return """ - #v = min(self.__max_velocity, self.__max_tree_v) - #v = min(v, self.__speed_limit) + # v = min(self.__max_velocity, self.__max_tree_v) + # v = min(v, self.__speed_limit) v = self.__max_velocity pid.setpoint = v From 3af1313962e66f54f8ed39c935edbc7c58658bb6 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Wed, 22 Nov 2023 17:13:40 +0100 Subject: [PATCH 04/53] feat: added new testability to be finished --- code/acting/launch/acting.launch | 9 ++-- .../src/acting/DummyTrajectoryPublisher.py | 42 ++++++++++++++++++- code/acting/src/acting/max_velocity_Dummy.py | 30 ++++++++++--- code/acting/src/acting/stanley_controller.py | 6 +++ code/acting/src/acting/velocity_controller.py | 2 + code/perception/launch/perception.launch | 4 +- .../launch/behavior_agent.launch | 3 +- 7 files changed, 83 insertions(+), 13 deletions(-) diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 0a647e96..b24f78f7 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -2,7 +2,7 @@ - + @@ -28,12 +28,11 @@ - @@ -62,4 +61,6 @@ + + diff --git a/code/acting/src/acting/DummyTrajectoryPublisher.py b/code/acting/src/acting/DummyTrajectoryPublisher.py index 61a8e8ae..d46ac8f3 100755 --- a/code/acting/src/acting/DummyTrajectoryPublisher.py +++ b/code/acting/src/acting/DummyTrajectoryPublisher.py @@ -2,6 +2,8 @@ """ This node publishes a dummy trajectory between predefined points. +TODO: Implement this dummy to post a trajectory of serpentine-lines +to check if the steering controllers work """ import ros_compatibility as roscomp @@ -34,6 +36,7 @@ def __init__(self): 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), @@ -59,6 +62,22 @@ def __init__(self): (1464.6, -5580.0), (1664.6, -5580.0) ] + """ + startx = 986.0 + starty = -5442.0 + self.initial_trajectory = [ + (startx, starty), + (startx, starty-10), + (startx-2, starty-20), + (startx, starty-30), + (startx+2, starty-40), + (startx, starty-50), + (startx-2, starty-60), + (startx, starty-70), + (startx+2, starty-80), + (startx, starty-90), + (startx, starty-1000), + ] self.updated_trajectory(self.initial_trajectory) @@ -68,6 +87,16 @@ def __init__(self): "/paf/" + self.role_name + "/trajectory", qos_profile=1) + self.current_pos_sub = self.new_subscription( + msg_type=PoseStamped, + topic="/paf/" + self.role_name + "/current_pos", + callback=self.__current_position_callback, + qos_profile=1) + + self.x = 0 + self.y = 0 + self.z = 0 + def updated_trajectory(self, target_trajectory): """ Updates the published Path message with the new target trajectory. @@ -88,7 +117,7 @@ def updated_trajectory(self, target_trajectory): pos.pose.position.x = wp[0] pos.pose.position.y = wp[1] - pos.pose.position.z = 37.6 + pos.pose.position.z = 37.6 # why?? # currently not used therefore zeros pos.pose.orientation.x = 0 @@ -96,8 +125,19 @@ def updated_trajectory(self, target_trajectory): pos.pose.orientation.z = 0 pos.pose.orientation.w = 0 + # print(pos) + 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 + # print("x: "+ str(agent.x)) + # print("y: "+ str(agent.y)) + # print("z: "+ str(agent.z)) + def run(self): """ Control loop diff --git a/code/acting/src/acting/max_velocity_Dummy.py b/code/acting/src/acting/max_velocity_Dummy.py index 5f073756..55e1d417 100755 --- a/code/acting/src/acting/max_velocity_Dummy.py +++ b/code/acting/src/acting/max_velocity_Dummy.py @@ -3,10 +3,12 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Publisher +import rospy from std_msgs.msg import Float32 -MAX_VELOCITY: float = 20.0 -STEERING: float = 0.0 +MAX_VELOCITY_HIGH: float = 5.0 +MAX_VELOCITY_LOW: float = 5.0 +STEERING: float = 0.0 # TODO: NO NEED IF TRAJECTORY DUMMY WORKS class DummyVelocityPublisher(CompatibleNode): @@ -35,6 +37,10 @@ def __init__(self): f"/paf/{self.role_name}/pure_pursuit_steer", qos_profile=1) + self.checkpoint_time = rospy.get_time() + self.switchVelocity = False + self.driveVel = MAX_VELOCITY_HIGH + def run(self): """ Starts the main loop of the node @@ -49,9 +55,23 @@ def loop(timer_event=None): :param timer_event: Timer event from ROS :return: """ - self.velocity_pub.publish(MAX_VELOCITY) - self.stanley_steer_pub.publish(STEERING) - self.pure_pursuit_steer_pub.publish(STEERING) + + # letzter timecheck < aktuelle Zeit - 20 Sekunden + # = mehr Zeit vergangen als 20 Sekunden + if (self.checkpoint_time < rospy.get_time() - 40.0): + self.checkpoint_time = rospy.get_time() + + if (self.switchVelocity): + self.switchVelocity = False + self.driveVel = MAX_VELOCITY_HIGH + + else: + self.switchVelocity = True + self.driveVel = MAX_VELOCITY_LOW + + self.velocity_pub.publish(self.driveVel) + # self.stanley_steer_pub.publish(STEERING) + # self.pure_pursuit_steer_pub.publish(STEERING) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py index 20721bcf..0c67bbeb 100755 --- a/code/acting/src/acting/stanley_controller.py +++ b/code/acting/src/acting/stanley_controller.py @@ -60,6 +60,11 @@ def __init__(self): f"/paf/{self.role_name}/stanley_debug", qos_profile=1) + self.poserror_publisher: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/position_error", + qos_profile=1) + self.__position: (float, float) = None # x, y self.__last_pos: (float, float) = None self.__path: Path = None @@ -186,6 +191,7 @@ def __calculate_steer(self) -> float: debug_msg.steering_angle = steering_angle self.debug_publisher.publish(debug_msg) # <- + self.poserror_publisher.publish(cross_err) return steering_angle diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 6fb44d72..ea040835 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -176,6 +176,7 @@ 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 @@ -194,6 +195,7 @@ def __current_position_callback(self, data: PoseStamped): 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/perception/launch/perception.launch b/code/perception/launch/perception.launch index 2b5f69d8..983a45fd 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -2,12 +2,12 @@ - - + From b5552147d4a2fc899df4587ca7b5278f0e397fb5 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 23 Nov 2023 16:51:03 +0100 Subject: [PATCH 05/53] feat: added testability for both steering controllers seperately and for the steering pid in vehicle control (steering behavior) --- code/acting/launch/acting.launch | 4 +- .../src/acting/DummyTrajectoryPublisher.py | 98 ++++++++++--------- code/acting/src/acting/stanley_controller.py | 13 ++- code/acting/src/acting/vehicle_controller.py | 23 ++++- 4 files changed, 85 insertions(+), 53 deletions(-) diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index b24f78f7..c069b09e 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -61,6 +61,8 @@ - + + + diff --git a/code/acting/src/acting/DummyTrajectoryPublisher.py b/code/acting/src/acting/DummyTrajectoryPublisher.py index d46ac8f3..a9c93e1e 100755 --- a/code/acting/src/acting/DummyTrajectoryPublisher.py +++ b/code/acting/src/acting/DummyTrajectoryPublisher.py @@ -12,6 +12,9 @@ from ros_compatibility.node import CompatibleNode from trajectory_interpolation import interpolate_route import rospy +import numpy as np + +TRAJECTORY_TYPE = 1 # 0 = Straight ; 1 = SineWave ; 2 = (old) Curve class DummyTrajectoryPub(CompatibleNode): @@ -35,50 +38,59 @@ def __init__(self): self.path_msg = Path() self.path_msg.header.stamp = rospy.Time.now() self.path_msg.header.frame_id = "global" + if (TRAJECTORY_TYPE == 2): + # Static trajectory of a curve for testing purposes + self.curve_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) + ] - """ - # 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) - ] - """ startx = 986.0 starty = -5442.0 - self.initial_trajectory = [ - (startx, starty), - (startx, starty-10), - (startx-2, starty-20), - (startx, starty-30), - (startx+2, starty-40), - (startx, starty-50), - (startx-2, starty-60), - (startx, starty-70), - (startx+2, starty-80), - (startx, starty-90), - (startx, starty-1000), - ] - + # 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.initial_trajectory = trajectory_wave self.updated_trajectory(self.initial_trajectory) # publisher for the current trajectory @@ -93,10 +105,6 @@ def __init__(self): callback=self.__current_position_callback, qos_profile=1) - self.x = 0 - self.y = 0 - self.z = 0 - def updated_trajectory(self, target_trajectory): """ Updates the published Path message with the new target trajectory. diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py index 0c67bbeb..29987ce4 100755 --- a/code/acting/src/acting/stanley_controller.py +++ b/code/acting/src/acting/stanley_controller.py @@ -60,9 +60,14 @@ def __init__(self): f"/paf/{self.role_name}/stanley_debug", qos_profile=1) - self.poserror_publisher: Publisher = self.new_publisher( + self.targetwp_publisher: Publisher = self.new_publisher( Float32, - f"/paf/{self.role_name}/position_error", + 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 @@ -191,8 +196,8 @@ def __calculate_steer(self) -> float: debug_msg.steering_angle = steering_angle self.debug_publisher.publish(debug_msg) # <- - self.poserror_publisher.publish(cross_err) - + self.targetwp_publisher.publish((closest_point.pose.position.x)) + self.currentx_publisher.publish(self.__position[0]) 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..f3eac9f4 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -9,6 +9,9 @@ from std_msgs.msg import Bool, Float32 from simple_pid import PID +STEERING_CONTROLLERTEST: bool = True # Steering-Controllers tested alone +CONTROLLER_TESTED: int = 1 # 1 = PPC alone ; 2 = SC alone + PURE_PURSUIT_CONTROLLER: int = 1 STANLEY_CONTROLLER: int = 2 STANLEY_CONTROLLER_MIN_V: float = 4.0 # ~14kph @@ -93,6 +96,11 @@ def __init__(self): self.__set_stanley_steer, qos_profile=1) + self.target_steering_publisher: Publisher = self.new_publisher( + Float32, + f'/paf/{self.role_name}/target_steering', + qos_profile=1) + self.__emergency: bool = False self.__throttle: float = 0.0 self.__velocity: float = 0.0 @@ -107,7 +115,7 @@ def run(self): """ self.status_pub.publish(True) self.loginfo('VehicleController node running') - pid = PID(0.5, 0.1, 0.1, setpoint=0) + pid = PID(0.5, 0.1, 0.1, setpoint=0) # TODO: tune parameters pid.output_limits = (-MAX_STEER_ANGLE, MAX_STEER_ANGLE) def loop(timer_event=None) -> None: @@ -119,7 +127,13 @@ def loop(timer_event=None) -> None: if self.__emergency: # emergency is already handled in # __emergency_break() return - p_stanley = self.__choose_controller() + if (STEERING_CONTROLLERTEST): + if (CONTROLLER_TESTED == 2): + p_stanley = 1 + elif (CONTROLLER_TESTED == 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)) @@ -148,6 +162,9 @@ def loop(timer_event=None) -> None: message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) self.control_publisher.publish(message) + print(steer) + print(message.steer) + self.target_steering_publisher.publish(steer) self.new_timer(self.control_loop_rate, loop) self.spin() @@ -159,7 +176,7 @@ def __map_steering(self, steering_angle: float) -> float: :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 = -5 # factor for tuning TODO: tune r = 1 / (math.pi / 2) steering_float = steering_angle * r * tune_k return steering_float From 32e7d105163332a9c76db594abe57d825fee9a95 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Fri, 24 Nov 2023 15:08:10 +0100 Subject: [PATCH 06/53] feat: All Testability for Acting in one class Still not perfect and needs alot more finetuning. But selecting a testcase, a trajectory and which steeringcontroller to test seem to work already. --- code/acting/launch/acting.launch | 14 +- code/acting/src/acting/Acting_DebuggerNode.py | 271 ++++++++++++++++++ .../src/acting/DummyTrajectoryPublisher.py | 182 ------------ code/acting/src/acting/max_velocity_Dummy.py | 97 ------- code/acting/src/acting/stanley_controller.py | 6 +- code/acting/src/acting/vehicle_controller.py | 35 ++- code/acting/src/acting/velocity_controller.py | 2 +- .../launch/global_planner.launch | 2 + 8 files changed, 304 insertions(+), 305 deletions(-) create mode 100755 code/acting/src/acting/Acting_DebuggerNode.py delete mode 100755 code/acting/src/acting/DummyTrajectoryPublisher.py delete mode 100755 code/acting/src/acting/max_velocity_Dummy.py diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index c069b09e..3ebea296 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -29,7 +29,7 @@ - + @@ -39,11 +39,6 @@ - - @@ -56,13 +51,8 @@ --> - - - - - - + diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py new file mode 100755 index 00000000..b7de5e2a --- /dev/null +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -0,0 +1,271 @@ +#!/usr/bin/env python + +""" +This node will (soon TODO) provide full testability +for all Acting components with different testcases +to hopefully fully implement and tune Acting without +the need of working Perception and Planning components. +""" + +import ros_compatibility as roscomp +import numpy as np +from nav_msgs.msg import Path +from std_msgs.msg import Float32 +from geometry_msgs.msg import PoseStamped +from ros_compatibility.node import CompatibleNode +import rospy +from rospy import Publisher, Subscriber + +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 +# 0: c. Velocity Only (publishes const. velocity,const. +# steering = 0, no trajectory) +# TURN OFF stanley and PP Controllers in acting.launch! +# 1: changing velocity (TODO: implement good changing velocity-curve, +# CURRENTLY alternates between two constant velocities) +# TURN OFF stanley and PP Controllers in acting.launch! +# 2: c. Velocity + trajectory (publishes const. velocity +# and chosen trajectory (see TRAJECTORY_TYPE)) +# 3: TODO: implement more test cases as needed, trajectory + changing vel, +# trajectory and actual calculated vel from acc or other etc +# TODO: implement acc test, implement more tests as needed in general +TEST_TYPE = 2 # aka. TT + +STEERING: float = 0.0 # for TT0: steering -> always straight +MAX_VELOCITY_LOW: float = 4.0 # for TT0/TT1: low velocity +MAX_VELOCITY_HIGH: float = 12.0 # for TT1: high velocity + +STEERING_CONTROLLER_USED = 2 # for TT1/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 (soon TODO) full testability for all acting components + without the need of working perception or planning. + This hopefully allows us to fully finish Acting for later merging with the + other two big modules. + """ + + 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 TT0 and TT1 on max_velocity + self.velocity_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/max_velocity", + qos_profile=1) + + # Steer-SC: Publisher for TT0, constant Steer to stanley_steer + self.stanley_steer_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/stanley_steer", + qos_profile=1) + + # Steer-PPC: Publisher for TT0, constant Steer to pure_pursuit_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 ?? + 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) + + # Initialize all needed "global" variables here + self.current_trajectory = [] + self.checkpoint_time = rospy.get_time() + self.switchVelocity = False + self.driveVel = MAX_VELOCITY_LOW + 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 of the car 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 what is happening here? + + def run(self): + """ + Control loop + :return: + """ + + def loop(timer_event=None): + """ + Publishes different speeds, trajectories ... + depending on the selected TEST_TYPE + """ + if (TEST_TYPE == 0): + self.drive_Vel = MAX_VELOCITY_LOW + self.stanley_steer_pub.publish(STEERING) + self.pure_pursuit_steer_pub.publish(STEERING) + self.velocity_pub.publish(self.driveVel) + + elif (TEST_TYPE == 1): + self.drive_Vel = MAX_VELOCITY_LOW + if (self.vel_switch_timer < rospy.get_time() - 40.0): + self.checkpoint_time = rospy.get_time() + if (self.switchVelocity): + self.driveVel = MAX_VELOCITY_HIGH + else: + self.driveVel = MAX_VELOCITY_LOW + self.switchVelocity = not self.switchVelocity + self.stanley_steer_pub.publish(STEERING) + self.pure_pursuit_steer_pub.publish(STEERING) + self.velocity_pub.publish(self.driveVel) + + elif (TEST_TYPE == 2): + # Continuously update path and publish it + self.drive_Vel = MAX_VELOCITY_LOW + self.updated_trajectory(self.current_trajectory) + self.trajectory_pub.publish(self.path_msg) + self.velocity_pub.publish(self.driveVel) + + if (STEERING_CONTROLLER_USED == 1): + self.controller_selector_pub.publish(1) + elif (STEERING_CONTROLLER_USED == 2): + self.controller_selector_pub.publish(2) + + 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 a9c93e1e..00000000 --- a/code/acting/src/acting/DummyTrajectoryPublisher.py +++ /dev/null @@ -1,182 +0,0 @@ -#!/usr/bin/env python - -""" -This node publishes a dummy trajectory between predefined points. -TODO: Implement this dummy to post a trajectory of serpentine-lines -to check if the steering controllers work -""" - -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 -import numpy as np - -TRAJECTORY_TYPE = 1 # 0 = Straight ; 1 = SineWave ; 2 = (old) Curve - - -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" - if (TRAJECTORY_TYPE == 2): - # Static trajectory of a curve for testing purposes - self.curve_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) - ] - - startx = 986.0 - starty = -5442.0 - # 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.initial_trajectory = trajectory_wave - 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) - - self.current_pos_sub = self.new_subscription( - msg_type=PoseStamped, - topic="/paf/" + self.role_name + "/current_pos", - callback=self.__current_position_callback, - 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 # 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 - - # print(pos) - - 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 - # print("x: "+ str(agent.x)) - # print("y: "+ str(agent.y)) - # print("z: "+ str(agent.z)) - - 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/max_velocity_Dummy.py b/code/acting/src/acting/max_velocity_Dummy.py deleted file mode 100755 index 55e1d417..00000000 --- a/code/acting/src/acting/max_velocity_Dummy.py +++ /dev/null @@ -1,97 +0,0 @@ -#!/usr/bin/env python - -import ros_compatibility as roscomp -from ros_compatibility.node import CompatibleNode -from rospy import Publisher -import rospy -from std_msgs.msg import Float32 - -MAX_VELOCITY_HIGH: float = 5.0 -MAX_VELOCITY_LOW: float = 5.0 -STEERING: float = 0.0 # TODO: NO NEED IF TRAJECTORY DUMMY WORKS - - -class DummyVelocityPublisher(CompatibleNode): - """ - This node publishes a constant max_velocity - for Debugging and Parametertuning. - """ - - def __init__(self): - super(DummyVelocityPublisher, self).__init__('dummy_const_vel_pub') - self.control_loop_rate = self.get_param('control_loop_rate', 0.1) - self.role_name = self.get_param('role_name', 'ego_vehicle') - - self.velocity_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/max_velocity", - qos_profile=1) - - self.stanley_steer_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/stanley_steer", - qos_profile=1) - - self.pure_pursuit_steer_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/pure_pursuit_steer", - qos_profile=1) - - self.checkpoint_time = rospy.get_time() - self.switchVelocity = False - self.driveVel = MAX_VELOCITY_HIGH - - def run(self): - """ - Starts the main loop of the node - :return: - """ - self.loginfo('DUMMY_constant-velocity_publisher node running') - - def loop(timer_event=None): - """ - Publishes velocity limits calculated in acting based on - upcoming curves - :param timer_event: Timer event from ROS - :return: - """ - - # letzter timecheck < aktuelle Zeit - 20 Sekunden - # = mehr Zeit vergangen als 20 Sekunden - if (self.checkpoint_time < rospy.get_time() - 40.0): - self.checkpoint_time = rospy.get_time() - - if (self.switchVelocity): - self.switchVelocity = False - self.driveVel = MAX_VELOCITY_HIGH - - else: - self.switchVelocity = True - self.driveVel = MAX_VELOCITY_LOW - - self.velocity_pub.publish(self.driveVel) - # self.stanley_steer_pub.publish(STEERING) - # self.pure_pursuit_steer_pub.publish(STEERING) - - self.new_timer(self.control_loop_rate, loop) - self.spin() - - -def main(args=None): - """ - Main function starts the node - :param args: - """ - - roscomp.init('dummy_const_vel_pub', args=args) - try: - node = DummyVelocityPublisher() - node.run() - except KeyboardInterrupt: - pass - finally: - roscomp.shutdown() - - -if __name__ == '__main__': - main() diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py index 29987ce4..d5cae8fe 100755 --- a/code/acting/src/acting/stanley_controller.py +++ b/code/acting/src/acting/stanley_controller.py @@ -196,8 +196,10 @@ def __calculate_steer(self) -> float: debug_msg.steering_angle = steering_angle self.debug_publisher.publish(debug_msg) # <- - self.targetwp_publisher.publish((closest_point.pose.position.x)) - self.currentx_publisher.publish(self.__position[0]) + + # 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 f3eac9f4..f2bd5d17 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -9,9 +9,6 @@ from std_msgs.msg import Bool, Float32 from simple_pid import PID -STEERING_CONTROLLERTEST: bool = True # Steering-Controllers tested alone -CONTROLLER_TESTED: int = 1 # 1 = PPC alone ; 2 = SC alone - PURE_PURSUIT_CONTROLLER: int = 1 STANLEY_CONTROLLER: int = 2 STANLEY_CONTROLLER_MIN_V: float = 4.0 # ~14kph @@ -96,17 +93,29 @@ def __init__(self): self.__set_stanley_steer, qos_profile=1) + # Testing / Debugging --> self.target_steering_publisher: Publisher = self.new_publisher( Float32, - f'/paf/{self.role_name}/target_steering', + f'/paf/{self.role_name}/target_steering_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.controller_testing: bool = False + self.controller_selected_debug: int = 1 + # TODO: check emergency behaviour def run(self): """ @@ -127,13 +136,15 @@ def loop(timer_event=None) -> None: if self.__emergency: # emergency is already handled in # __emergency_break() return - if (STEERING_CONTROLLERTEST): - if (CONTROLLER_TESTED == 2): + if (self.controller_testing): + if (self.controller_selected_debug == 2): p_stanley = 1 - elif (CONTROLLER_TESTED == 1): + elif (self.controller_selected_debug == 1): p_stanley = 0 else: + print("hier??!") p_stanley = self.__choose_controller() + print(p_stanley) if p_stanley < 0.5: self.logdebug('Using PURE_PURSUIT_CONTROLLER') self.controller_pub.publish(float(PURE_PURSUIT_CONTROLLER)) @@ -162,9 +173,7 @@ def loop(timer_event=None) -> None: message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) self.control_publisher.publish(message) - print(steer) - print(message.steer) - self.target_steering_publisher.publish(steer) + self.target_steering_publisher.publish(steer) # debugging self.new_timer(self.control_loop_rate, loop) self.spin() @@ -242,6 +251,10 @@ def __set_pure_pursuit_steer(self, data: Float32): 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 ea040835..219dd23c 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -9,7 +9,7 @@ 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 = 10 # 36.0 class VelocityController(CompatibleNode): diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index d26b4873..38440559 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -7,9 +7,11 @@ --> + From a895d140c5aa2912dc3594133d9c2bacd7c236a3 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Wed, 29 Nov 2023 16:21:52 +0100 Subject: [PATCH 07/53] feat: Ready for velocity testing --- code/acting/launch/acting.launch | 12 +- code/acting/src/acting/Acting_DebuggerNode.py | 116 ++++++++++++++---- code/acting/src/acting/vehicle_controller.py | 3 +- code/acting/src/acting/velocity_controller.py | 9 +- 4 files changed, 109 insertions(+), 31 deletions(-) diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 3ebea296..c3dfab4e 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -2,7 +2,7 @@ - + @@ -30,7 +30,7 @@ - + @@ -51,8 +51,8 @@ --> - - - + + + diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index b7de5e2a..5950b24d 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -1,10 +1,13 @@ #!/usr/bin/env python """ -This node will (soon TODO) provide full testability -for all Acting components with different testcases +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 ros_compatibility as roscomp @@ -15,38 +18,44 @@ from ros_compatibility.node import CompatibleNode import rospy from rospy import Publisher, Subscriber +from carla_msgs.msg import CarlaSpeedometer 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 -# 0: c. Velocity Only (publishes const. velocity,const. -# steering = 0, no trajectory) + +# TEST_TYPE to choose which kind of Test to run: +# 0: Test Velocity Controller with constant one velocity +# const. velocity = MAX_VELOCITY_LOW +# const. steering = 0 +# no trajectory # TURN OFF stanley and PP Controllers in acting.launch! -# 1: changing velocity (TODO: implement good changing velocity-curve, -# CURRENTLY alternates between two constant velocities) + +# 1: Test Velocity Controller with changing velocity +# velocity = alternate all 20 secs: MAX_VELOCITY_LOW/_HIGH +# const. steering = 0 +# no trajectory # TURN OFF stanley and PP Controllers in acting.launch! -# 2: c. Velocity + trajectory (publishes const. velocity -# and chosen trajectory (see TRAJECTORY_TYPE)) -# 3: TODO: implement more test cases as needed, trajectory + changing vel, -# trajectory and actual calculated vel from acc or other etc -# TODO: implement acc test, implement more tests as needed in general -TEST_TYPE = 2 # aka. TT + +# 2: Test Steering Controller on chooseable trajectory +# velocity = MAX_VELOCITY_LOW TODO: maybe use velocity publisher? +# steering = STEERING_CONTROLLER_USED (see below) +# trajectory = TRAJECTORY_TYPE (see below) +TEST_TYPE = 0 # aka. TT STEERING: float = 0.0 # for TT0: steering -> always straight -MAX_VELOCITY_LOW: float = 4.0 # for TT0/TT1: low velocity -MAX_VELOCITY_HIGH: float = 12.0 # for TT1: high velocity +MAX_VELOCITY_LOW: float = 10.0 # for TT0/TT1: low velocity +MAX_VELOCITY_HIGH: float = 20.0 # for TT1: high velocity STEERING_CONTROLLER_USED = 2 # for TT1/TT2: 0 = both ; 1 = PP ; 2 = Stanley -TRAJECTORY_TYPE = 0 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve +TRAJECTORY_TYPE = 0 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve class Acting_Debugger(CompatibleNode): """ - Creates a node with (soon TODO) full testability for all acting components - without the need of working perception or planning. - This hopefully allows us to fully finish Acting for later merging with the - other two big modules. + Creates a node with testability for all acting components + without the need of working/running perception or planning. """ def __init__(self): @@ -90,18 +99,43 @@ def __init__(self): f"/paf/{self.role_name}/controller_selector_debug", qos_profile=1) - # Subscriber of current_pos, used for TODO ?? + # Subscriber of current_pos, used for 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) + # SUBSCRIBER FOR EVALUATION LISTS FOR TUNING + self.max_velocity_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/max_velocity", + self.__get_max_velocity, + qos_profile=1) + + self.current_velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + + self.current_throttle_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/throttle", + self.__get_throttle, + qos_profile=1) + # Initialize all needed "global" variables here self.current_trajectory = [] self.checkpoint_time = rospy.get_time() self.switchVelocity = False self.driveVel = MAX_VELOCITY_LOW + + self.__current_velocities = [] + self.__max_velocities = [] + self.__throttles = [] + self.time_set = False + self.path_msg = Path() self.path_msg.header.stamp = rospy.Time.now() self.path_msg.header.frame_id = "global" @@ -202,13 +236,23 @@ def __current_position_callback(self, data: PoseStamped): self.x = agent.x self.y = agent.y self.z = agent.z - # TODO what is happening here? + # TODO use this to get spawnpoint? necessary? + + def __get_max_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 run(self): """ Control loop :return: """ + self.checkpoint_time = rospy.get_time() def loop(timer_event=None): """ @@ -223,7 +267,7 @@ def loop(timer_event=None): elif (TEST_TYPE == 1): self.drive_Vel = MAX_VELOCITY_LOW - if (self.vel_switch_timer < rospy.get_time() - 40.0): + if (self.checkpoint_time < rospy.get_time() - 20.0): self.checkpoint_time = rospy.get_time() if (self.switchVelocity): self.driveVel = MAX_VELOCITY_HIGH @@ -246,6 +290,34 @@ def loop(timer_event=None): elif (STEERING_CONTROLLER_USED == 2): self.controller_selector_pub.publish(2) + # set starttime to when the 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() + print(self.checkpoint_time) + self.time_set = True + + if (self.checkpoint_time < rospy.get_time() - 10.0): + self.checkpoint_time = rospy.get_time() + print(">>>>>>>>>>>> HIER <<<<<<<<<<<<<<") + print(self.checkpoint_time) + print(self.__max_velocities) + print(self.__current_velocities) + print(self.__throttles) + print(len(self.__max_velocities)) + print(len(self.__current_velocities)) + print(len(self.__throttles)) + """ + TODO: THIS DOES NOT WORK WITHOUT SUDO PERMISSION + doc_folder = os.path.join(os.path.expanduser('~'), 'Documents') + data_folder = os.path.join(doc_folder, 'data') + os.makedirs(data_folder, exist_ok=True) + output_file = os.path.join(data_folder, 'cur_vel.txt') + with open (output_file, "w") as f: + for wert in self.__current_velocities: + f.write(str(wert) + "; ") + """ + self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index f2bd5d17..e7057626 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -142,9 +142,8 @@ def loop(timer_event=None) -> None: elif (self.controller_selected_debug == 1): p_stanley = 0 else: - print("hier??!") p_stanley = self.__choose_controller() - print(p_stanley) + if p_stanley < 0.5: self.logdebug('Using PURE_PURSUIT_CONTROLLER') self.controller_pub.publish(float(PURE_PURSUIT_CONTROLLER)) diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 219dd23c..323a2636 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -62,6 +62,11 @@ def __init__(self): f"/paf/{self.role_name}/velocity_as_float", qos_profile=1) + self.veldiff_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/vel_diff", + qos_profile=1) + # needed to prevent the car from driving before a path to follow is # available. Might be needed later to slow down in curves self.trajectory_sub: Subscriber = self.new_subscription( @@ -97,7 +102,7 @@ def run(self): :return: """ self.loginfo('VehicleController node running') - pid = PID(0.25, 0, 0.1) # values from paf21-2 todo: tune + pid = PID(0.47, 0, 0) # PID(0.25, 0, 0.1) values from paf21-2 TUNE! def loop(timer_event=None): """ @@ -152,6 +157,8 @@ def loop(timer_event=None): throttle = max(throttle, 0) # ensures that throttle >= 0 throttle = min(throttle, 1.0) # ensures that throttle <= 1 self.throttle_pub.publish(throttle) + vel_diff = self.__current_velocity - v + self.veldiff_pub.publish(vel_diff) self.new_timer(self.control_loop_rate, loop) self.spin() From 3adc42aefcdc883814bd13421132c8cab638ae22 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 30 Nov 2023 14:53:15 +0100 Subject: [PATCH 08/53] feat: Acting_DebuggerNode and Acting commitable --- build/docker-compose.yml | 4 +- code/acting/launch/acting.launch | 14 ++-- code/acting/src/acting/Acting_DebuggerNode.py | 76 +++++++++++++------ code/acting/src/acting/stanley_controller.py | 18 +++-- code/acting/src/acting/vehicle_controller.py | 24 ++++-- code/acting/src/acting/velocity_controller.py | 39 +++++----- code/agent/config/rviz_config.rviz | 2 +- code/perception/launch/perception.launch | 16 +++- .../launch/behavior_agent.launch | 3 +- .../launch/global_planner.launch | 4 +- 10 files changed, 124 insertions(+), 76 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index fd0e7069..baf7d008 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -54,8 +54,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - #command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && 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/ && 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 c3dfab4e..8033851c 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -1,8 +1,8 @@ - + - + @@ -44,15 +44,15 @@ - - + + - + + diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index 5950b24d..6d38ed9e 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -42,14 +42,20 @@ # velocity = MAX_VELOCITY_LOW TODO: maybe use velocity publisher? # steering = STEERING_CONTROLLER_USED (see below) # trajectory = TRAJECTORY_TYPE (see below) -TEST_TYPE = 0 # aka. TT + +# 3: Test Emergency Breaks on TestType 1 +# TODO IMPLEMENT THIS TODO + +# 4: Test Steering-PID in vehicleController +# TODO TODO +TEST_TYPE = 2 # aka. TT STEERING: float = 0.0 # for TT0: steering -> always straight -MAX_VELOCITY_LOW: float = 10.0 # for TT0/TT1: low velocity +MAX_VELOCITY_LOW: float = 5.0 # for TT0/TT1: low velocity MAX_VELOCITY_HIGH: float = 20.0 # for TT1: high velocity -STEERING_CONTROLLER_USED = 2 # for TT1/TT2: 0 = both ; 1 = PP ; 2 = Stanley -TRAJECTORY_TYPE = 0 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve +STEERING_CONTROLLER_USED = 0 # for TT1/TT2: 0 = both ; 1 = PP ; 2 = Stanley +TRAJECTORY_TYPE = 1 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve class Acting_Debugger(CompatibleNode): @@ -68,25 +74,25 @@ def __init__(self): 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 + # Publisher for Dummy Trajectory self.trajectory_pub: Publisher = self.new_publisher( Path, "/paf/" + self.role_name + "/trajectory", qos_profile=1) - # Publisher for TT0 and TT1 on max_velocity + # Publisher for Dummy Velocity self.velocity_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/max_velocity", qos_profile=1) - # Steer-SC: Publisher for TT0, constant Steer to stanley_steer + # 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) - # Steer-PPC: Publisher for TT0, constant Steer to pure_pursuit_steer + # PurePursuit: Publisher for Dummy PP-Steer self.pure_pursuit_steer_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/pure_pursuit_steer", @@ -99,32 +105,49 @@ def __init__(self): f"/paf/{self.role_name}/controller_selector_debug", qos_profile=1) - # Subscriber of current_pos, used for + # 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) - # SUBSCRIBER FOR EVALUATION LISTS FOR TUNING + # ---> EVALUATION/TUNING: Subscribers for plotting + # Subscriber for max_velocity for plotting self.max_velocity_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/max_velocity", self.__get_max_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) + # Initialize all needed "global" variables here self.current_trajectory = [] self.checkpoint_time = rospy.get_time() @@ -136,12 +159,15 @@ def __init__(self): self.__throttles = [] self.time_set = False + self.__purepursuit_steers = [] + self.__stanley_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 of the car at the simulationstart TODO: get from position + # Spawncoords at the simulationstart TODO: get from position startx = 984.5 starty = -5442.0 @@ -247,6 +273,12 @@ def __get_current_velocity(self, data: CarlaSpeedometer): 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): + self.__purepursuit_steers.append(float(data.data)) + def run(self): """ Control loop @@ -285,38 +317,32 @@ def loop(timer_event=None): self.trajectory_pub.publish(self.path_msg) self.velocity_pub.publish(self.driveVel) + elif (TEST_TYPE == 4): + self.drive_Vel = MAX_VELOCITY_LOW + self.stanley_steer_pub.publish(STEERING) + self.pure_pursuit_steer_pub.publish(STEERING) + 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 the simulation is actually starting to run + """# 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() - print(self.checkpoint_time) self.time_set = True if (self.checkpoint_time < rospy.get_time() - 10.0): self.checkpoint_time = rospy.get_time() - print(">>>>>>>>>>>> HIER <<<<<<<<<<<<<<") - print(self.checkpoint_time) + print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") print(self.__max_velocities) print(self.__current_velocities) print(self.__throttles) print(len(self.__max_velocities)) print(len(self.__current_velocities)) print(len(self.__throttles)) - """ - TODO: THIS DOES NOT WORK WITHOUT SUDO PERMISSION - doc_folder = os.path.join(os.path.expanduser('~'), 'Documents') - data_folder = os.path.join(doc_folder, 'data') - os.makedirs(data_folder, exist_ok=True) - output_file = os.path.join(data_folder, 'cur_vel.txt') - with open (output_file, "w") as f: - for wert in self.__current_velocities: - f.write(str(wert) + "; ") - """ + print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<")""" self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py index d5cae8fe..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,6 +60,9 @@ def __init__(self): f"/paf/{self.role_name}/stanley_debug", qos_profile=1) + # 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", @@ -70,8 +73,8 @@ def __init__(self): f"/paf/{self.role_name}/current_x", qos_profile=1) - self.__position: (float, float) = None # x, y - self.__last_pos: (float, float) = None + 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 @@ -163,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 @@ -187,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 @@ -196,7 +199,6 @@ 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) diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index e7057626..a9901608 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", @@ -99,6 +104,11 @@ def __init__(self): 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', @@ -124,7 +134,7 @@ def run(self): """ self.status_pub.publish(True) self.loginfo('VehicleController node running') - pid = PID(0.5, 0.1, 0.1, setpoint=0) # TODO: tune parameters + pid = 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: @@ -155,6 +165,8 @@ def loop(timer_event=None) -> None: f_pure_p = (1 - p_stanley) * self.__pure_pursuit_steer steer = f_stanley + f_pure_p + self.target_steering_publisher.publish(steer) # debugging + message = CarlaEgoVehicleControl() message.reverse = False if self.__throttle > 0: # todo: driving backwards? @@ -166,13 +178,13 @@ def loop(timer_event=None) -> None: message.hand_brake = False message.manual_gear_shift = False + # sets target_steer to steer pid.setpoint = self.__map_steering(steer) message.steer = pid(self.__current_steer) message.gear = 1 message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) self.control_publisher.publish(message) - self.target_steering_publisher.publish(steer) # debugging self.new_timer(self.control_loop_rate, loop) self.spin() @@ -182,11 +194,13 @@ def __map_steering(self, steering_angle: float) -> float: Takes the steering angle calculated by the controller and maps it to the available steering angle :param steering_angle: calculated by a controller in [-pi/2 , pi/2] + TODO IS IT CALCULATED THAT WAY?? :return: float for steering in [-1, 1] """ - tune_k = -5 # factor for tuning TODO: tune + tune_k = -5 # factor for tuning TODO: tune WHAT IS THIS FOR? 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: diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 323a2636..183d8e5d 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -57,16 +57,12 @@ def __init__(self): # rqt_plot can't read the speed data provided by the rosbridge # Therefore, the speed is published again as a float value + # TODO not true anymore? -> obsolete? self.velocity_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/velocity_as_float", qos_profile=1) - self.veldiff_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/vel_diff", - qos_profile=1) - # needed to prevent the car from driving before a path to follow is # available. Might be needed later to slow down in curves self.trajectory_sub: Subscriber = self.new_subscription( @@ -82,6 +78,8 @@ def __init__(self): self.__set_speed_limits_opendrive, qos_profile=1) + # TODO: currently used to determine position on OpenDrive Map + # to set speed_limits correctly. Planning soon? -> Obsolete self.current_pos_sub: Subscriber = self.new_subscription( msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", @@ -101,8 +99,10 @@ def run(self): Starts the main loop of the node :return: """ - self.loginfo('VehicleController node running') - pid = PID(0.47, 0, 0) # PID(0.25, 0, 0.1) values from paf21-2 TUNE! + self.loginfo('VelocityController node running') + # PID(0.25, 0, 0.1) values from paf22 + # now newly tuned, not yet perfect, but highly stable + pid = PID(0.154, 0.001, 0.01) def loop(timer_event=None): """ @@ -113,20 +113,18 @@ def loop(timer_event=None): :return: """ if self.__max_velocity is None: - self.logdebug("VehicleController hasn't received max_velocity" + self.logdebug("VelocityController hasn't received max_velocity" " yet. max_velocity has been set to" f"default value {SPEED_LIMIT_DEFAULT}") - # return self.__max_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 @@ -144,12 +142,12 @@ def loop(timer_event=None): self.__max_tree_v = SPEED_LIMIT_DEFAULT if self.__max_velocity < 0: - self.logerr("Velocity controller doesn't support backward " + 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) + # TODO: soon Planning wants to calculate and publish max_velocity + v = min(self.__max_velocity, self.__max_tree_v) + v = min(v, self.__speed_limit) v = self.__max_velocity pid.setpoint = v @@ -157,8 +155,6 @@ def loop(timer_event=None): throttle = max(throttle, 0) # ensures that throttle >= 0 throttle = min(throttle, 1.0) # ensures that throttle <= 1 self.throttle_pub.publish(throttle) - vel_diff = self.__current_velocity - v - self.veldiff_pub.publish(vel_diff) self.new_timer(self.control_loop_rate, loop) self.spin() @@ -183,6 +179,12 @@ def __set_speed_limits_opendrive(self, data: Float32MultiArray): self.__speed_limits_OD = data.data def __current_position_callback(self, data: PoseStamped): + """ Use the current position to determine where + on the map the agent currently is. + This is needed to determine the Speed limits from + the OpenDrive Map. + :param data (PoseStamped): the Position-Message recieved + :return: """ if len(self.__speed_limits_OD) < 1 or self.__trajectory is None: return @@ -202,7 +204,6 @@ def __current_position_callback(self, data: PoseStamped): 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/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index b6d02247..60da1674 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -149,7 +149,7 @@ Visualization Manager: Offset: X: 0 Y: 0 - Z: 0 #39 + Z: 39 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 983a45fd..f04c9fa6 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -7,10 +7,19 @@ - + @@ -47,5 +56,4 @@ - --> - + \ No newline at end of file diff --git a/code/planning/behavior_agent/launch/behavior_agent.launch b/code/planning/behavior_agent/launch/behavior_agent.launch index 5118b697..6be8134a 100644 --- a/code/planning/behavior_agent/launch/behavior_agent.launch +++ b/code/planning/behavior_agent/launch/behavior_agent.launch @@ -1,8 +1,7 @@ - diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index 38440559..12ede0ad 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -7,11 +7,9 @@ --> - - + \ No newline at end of file From 903fd73a1a4ad1fe03c41372c18f354cc4d68a40 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 30 Nov 2023 15:49:05 +0100 Subject: [PATCH 09/53] feat: added emergency break testtype --- code/acting/src/acting/Acting_DebuggerNode.py | 29 +++++++++++++++++-- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index 6d38ed9e..2a53fc79 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -13,7 +13,7 @@ import ros_compatibility as roscomp import numpy as np from nav_msgs.msg import Path -from std_msgs.msg import Float32 +from std_msgs.msg import Float32, Bool from geometry_msgs.msg import PoseStamped from ros_compatibility.node import CompatibleNode import rospy @@ -44,11 +44,15 @@ # trajectory = TRAJECTORY_TYPE (see below) # 3: Test Emergency Breaks on TestType 1 -# TODO IMPLEMENT THIS TODO +# const velocity = MAX_VELOCITY_LOW +# 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 +TEST_TYPE = 3 # aka. TT STEERING: float = 0.0 # for TT0: steering -> always straight MAX_VELOCITY_LOW: float = 5.0 # for TT0/TT1: low velocity @@ -148,6 +152,12 @@ def __init__(self): self.__get_stanley_steer, qos_profile=1) + # 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.checkpoint_time = rospy.get_time() @@ -317,6 +327,19 @@ def loop(timer_event=None): self.trajectory_pub.publish(self.path_msg) self.velocity_pub.publish(self.driveVel) + elif (TEST_TYPE == 3): + # Continuously update path and publish it + self.drive_Vel = MAX_VELOCITY_LOW + 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) + elif (TEST_TYPE == 4): self.drive_Vel = MAX_VELOCITY_LOW self.stanley_steer_pub.publish(STEERING) From 46c15da0babfdd83d99f6f576ed9f318d191eba8 Mon Sep 17 00:00:00 2001 From: Robert Date: Tue, 5 Dec 2023 21:18:49 +0100 Subject: [PATCH 10/53] feat(#130): renamed files and linked them properly --- .../02_perception/06_paf_21_1_perception.md} | 0 doc/03_research/02_perception/Readme.md | 3 ++- ...are-perception.md => 05-autoware-perception.md} | 0 .../{07_vision_node.md => 06_vision_node.md} | 0 ...r_filter_debug.md => 07_sensor_filter_debug.md} | 8 ++++---- .../{11_kalman_filter.md => 08_kalman_filter.md} | 6 +++--- doc/06_perception/Readme.md | 14 ++++++++++++-- 7 files changed, 21 insertions(+), 10 deletions(-) rename doc/{06_perception/09_paf_21_1_perception.md => 03_research/02_perception/06_paf_21_1_perception.md} (100%) rename doc/06_perception/{6-autoware-perception.md => 05-autoware-perception.md} (100%) rename doc/06_perception/{07_vision_node.md => 06_vision_node.md} (100%) rename doc/06_perception/{10_sensor_filter_debug.md => 07_sensor_filter_debug.md} (95%) rename doc/06_perception/{11_kalman_filter.md => 08_kalman_filter.md} (95%) 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/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/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) From 2c680c78e7044de0ad9a0acddf54e670f11fb6f4 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 7 Dec 2023 14:39:58 +0100 Subject: [PATCH 11/53] feat: improved tested velocity_controller --- code/acting/launch/acting.launch | 5 +- code/acting/src/acting/Acting_DebuggerNode.py | 50 +++++----- .../src/acting/pure_pursuit_controller.py | 97 ++++++++++--------- code/acting/src/acting/vehicle_controller.py | 25 +++-- code/acting/src/acting/velocity_controller.py | 28 ++++-- 5 files changed, 117 insertions(+), 88 deletions(-) diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 8033851c..65c4fc35 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -50,9 +50,10 @@ - + + - + diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index 2a53fc79..65d2799c 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -52,13 +52,13 @@ # 4: Test Steering-PID in vehicleController # TODO TODO -TEST_TYPE = 3 # aka. TT +TEST_TYPE = 2 # aka. TT -STEERING: float = 0.0 # for TT0: steering -> always straight -MAX_VELOCITY_LOW: float = 5.0 # for TT0/TT1: low velocity -MAX_VELOCITY_HIGH: float = 20.0 # for TT1: high velocity +STEERING: float = 0.0 # for TT0: steering -> always straight +MAX_VELOCITY_LOW: float = 7 # for TT0/TT1: low velocity +MAX_VELOCITY_HIGH: float = 14 # for TT1: high velocity -STEERING_CONTROLLER_USED = 0 # for TT1/TT2: 0 = both ; 1 = PP ; 2 = Stanley +STEERING_CONTROLLER_USED = 1 # for TT2: 0 = both ; 1 = PP ; 2 = Stanley TRAJECTORY_TYPE = 1 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve @@ -160,14 +160,18 @@ def __init__(self): # Initialize all needed "global" variables here self.current_trajectory = [] - self.checkpoint_time = rospy.get_time() self.switchVelocity = False self.driveVel = MAX_VELOCITY_LOW + 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.time_set = False self.__purepursuit_steers = [] self.__stanley_steers = [] @@ -308,14 +312,17 @@ def loop(timer_event=None): self.velocity_pub.publish(self.driveVel) elif (TEST_TYPE == 1): - self.drive_Vel = MAX_VELOCITY_LOW - if (self.checkpoint_time < rospy.get_time() - 20.0): - self.checkpoint_time = rospy.get_time() + if not self.time_set: + self.drive_Vel = MAX_VELOCITY_LOW + self.switch_checkpoint_time = rospy.get_time() + self.switch_time_set = True + if (self.switch_checkpoint_time < rospy.get_time() - 10.0): + self.switch_checkpoint_time = rospy.get_time() + self.switchVelocity = not self.switchVelocity if (self.switchVelocity): self.driveVel = MAX_VELOCITY_HIGH else: self.driveVel = MAX_VELOCITY_LOW - self.switchVelocity = not self.switchVelocity self.stanley_steer_pub.publish(STEERING) self.pure_pursuit_steer_pub.publish(STEERING) self.velocity_pub.publish(self.driveVel) @@ -350,23 +357,22 @@ def loop(timer_event=None): elif (STEERING_CONTROLLER_USED == 2): self.controller_selector_pub.publish(2) - """# set starttime to when simulation is actually starting to run + # 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 - - if (self.checkpoint_time < rospy.get_time() - 10.0): + """ + if (self.checkpoint_time < rospy.get_time() - 20.0): self.checkpoint_time = rospy.get_time() print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") - print(self.__max_velocities) - print(self.__current_velocities) - print(self.__throttles) - print(len(self.__max_velocities)) - print(len(self.__current_velocities)) - print(len(self.__throttles)) - print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<")""" - + # print(self.__max_velocities) + # print(self.__current_velocities) + # print(self.__throttles) + # print(self.__purepursuit_steers) + # print(self.__stanley_steers) + print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") + """ self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index 076c2c52..38e36258 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -112,6 +112,53 @@ 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 = 1.0 # TODO: tune + look_ahead_dist = 3.5 # 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) + + # 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) + + return steering_angle + def __set_position(self, data: PoseStamped, min_diff=0.001): """ Updates the current position of the vehicle @@ -122,6 +169,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 +179,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 +211,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 diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index a9901608..b5b5a91b 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -99,6 +99,12 @@ def __init__(self): 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', @@ -123,6 +129,8 @@ def __init__(self): self.__stanley_steer: float = 0.0 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 @@ -169,13 +177,8 @@ def loop(timer_event=None) -> None: 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 # sets target_steer to steer @@ -193,11 +196,12 @@ 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] - TODO IS IT CALCULATED THAT WAY?? :return: float for steering in [-1, 1] """ - tune_k = -5 # factor for tuning TODO: tune WHAT IS THIS FOR? + tune_k = -5 # 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) @@ -258,6 +262,9 @@ 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 diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 183d8e5d..9035ba2f 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -9,7 +9,7 @@ from nav_msgs.msg import Path # TODO put back to 36 when controller can handle it -SPEED_LIMIT_DEFAULT: float = 10 # 36.0 +SPEED_LIMIT_DEFAULT: float = 36 # 36.0 class VelocityController(CompatibleNode): @@ -55,6 +55,11 @@ def __init__(self): f"/paf/{self.role_name}/throttle", qos_profile=1) + self.brake_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/brake", + 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 # TODO not true anymore? -> obsolete? @@ -100,9 +105,12 @@ def run(self): :return: """ self.loginfo('VelocityController node running') - # PID(0.25, 0, 0.1) values from paf22 - # now newly tuned, not yet perfect, but highly stable - pid = PID(0.154, 0.001, 0.01) + # PID for throttle + pid_t = PID(0.60, 0.00076, 0.63) + pid_t.output_limits = (0.0, 1.0) + # new PID for braking, much weaker than throttle controller! + pid_b = PID(-0.3, -0.00002, -0) # TODO TUNE ALOT MORE NO GOOD YET + pid_b.output_limits = (0.0, 1.0) def loop(timer_event=None): """ @@ -140,7 +148,6 @@ def loop(timer_event=None): " 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("VelocityController doesn't support backward " "driving yet.") @@ -150,10 +157,13 @@ def loop(timer_event=None): v = min(v, self.__speed_limit) v = self.__max_velocity - 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 + pid_t.setpoint = v + throttle = pid_t(self.__current_velocity) + + pid_b.setpoint = v + brake = pid_b(self.__current_velocity) + + self.brake_pub.publish(brake) self.throttle_pub.publish(throttle) self.new_timer(self.control_loop_rate, loop) From 3082103d32ea901321eb7250de1389ecfb98c05a Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 7 Dec 2023 14:50:59 +0100 Subject: [PATCH 12/53] feat: deactivate Testnode --- code/acting/launch/acting.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index b1262c4b..975d0b0e 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -29,10 +29,10 @@ - + From f784a69cf311ef0cc6322c1e8a5efdd02f874c9f Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Thu, 7 Dec 2023 16:14:05 +0100 Subject: [PATCH 13/53] fix: Delete unused behaviors --- .../src/behavior_agent/behavior_tree.py | 42 +------------------ 1 file changed, 1 insertion(+), 41 deletions(-) diff --git a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py b/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py index 4df7e287..1ac702c1 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py +++ b/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py @@ -61,47 +61,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") ]) From 9a8789b1a7b38f592ad322c4501e8e276abf93aa Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Thu, 7 Dec 2023 16:35:39 +0100 Subject: [PATCH 14/53] docs: changed directory for assets --- .../00_paf23 => 00_assets/planning}/BT_paper.png | Bin .../planning}/BehaviorTree_medium.png | Bin .../00_paf23 => 00_assets/planning}/Globalplan.png | Bin .../00_paf23 => 00_assets/planning}/Planning.png | Bin .../planning}/Planning_paf21.png | Bin .../planning}/intersection_scenario.png | Bin .../00_paf23 => 00_assets/planning}/localplan.png | Bin .../planning}/overtaking_scenario.png | Bin .../00_paf23 => 00_assets/planning}/overview.jpg | Bin .../00_paf23 => 00_assets/planning}/overview.png | Bin .../planning}/overview_paper1.png | Bin .../00_paf23 => 00_assets/planning}/prios.png | Bin doc/03_research/03_planning/00_paf23/01_Planning.md | 4 ++-- .../03_planning/00_paf23/02_PlanningPaf22.md | 2 +- .../{architecture.md => 03_PlannedArchitecture.md} | 8 ++++---- ....md => 04_Local_planning_for_first_milestone.md} | 12 ++++++------ 16 files changed, 13 insertions(+), 13 deletions(-) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/BT_paper.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/BehaviorTree_medium.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/Globalplan.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/Planning.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/Planning_paf21.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/intersection_scenario.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/localplan.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/overtaking_scenario.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/overview.jpg (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/overview.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/overview_paper1.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/prios.png (100%) rename doc/03_research/03_planning/00_paf23/{architecture.md => 03_PlannedArchitecture.md} (91%) rename doc/03_research/03_planning/00_paf23/{Local_planning_for_first_milestone.md => 04_Local_planning_for_first_milestone.md} (82%) 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/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 From c694f51e31838f5355abe6f348b20aceec765f57 Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Thu, 7 Dec 2023 16:48:32 +0100 Subject: [PATCH 15/53] fix: Delete unused behaviors --- .../src/behavior_agent/behavior_tree.py | 42 +----------------- .../planning}/BT_paper.png | Bin .../planning}/BehaviorTree_medium.png | Bin .../planning}/Globalplan.png | Bin .../planning}/Planning.png | Bin .../planning}/Planning_paf21.png | Bin .../planning}/intersection_scenario.png | Bin .../planning}/localplan.png | Bin .../planning}/overtaking_scenario.png | Bin .../planning}/overview.jpg | Bin .../planning}/overview.png | Bin .../planning}/overview_paper1.png | Bin .../00_paf23 => 00_assets/planning}/prios.png | Bin .../03_planning/00_paf23/01_Planning.md | 4 +- .../03_planning/00_paf23/02_PlanningPaf22.md | 2 +- ...hitecture.md => 03_PlannedArchitecture.md} | 8 ++-- ... 04_Local_planning_for_first_milestone.md} | 12 ++--- 17 files changed, 14 insertions(+), 54 deletions(-) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/BT_paper.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/BehaviorTree_medium.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/Globalplan.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/Planning.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/Planning_paf21.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/intersection_scenario.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/localplan.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/overtaking_scenario.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/overview.jpg (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/overview.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/overview_paper1.png (100%) rename doc/{03_research/03_planning/00_paf23 => 00_assets/planning}/prios.png (100%) rename doc/03_research/03_planning/00_paf23/{architecture.md => 03_PlannedArchitecture.md} (91%) rename doc/03_research/03_planning/00_paf23/{Local_planning_for_first_milestone.md => 04_Local_planning_for_first_milestone.md} (82%) diff --git a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py b/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py index 4df7e287..1ac702c1 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py +++ b/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py @@ -61,47 +61,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") ]) 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/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 From d7a5daa6fdcf0018df146874666b7c5ad14e54b7 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 7 Dec 2023 17:02:31 +0100 Subject: [PATCH 16/53] feat: added speed calculation for Object --- .../local_planner/src/collision_check.py | 121 ++++++++++++++---- 1 file changed, 98 insertions(+), 23 deletions(-) diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 34bcdbb4..72173567 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -1,14 +1,15 @@ #!/usr/bin/env python -# import rospy +import rospy +import numpy as np # import tf.transformations import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode - -# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion -# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo -# from nav_msgs.msg import Path +from rospy import Publisher, Subscriber, Duration +from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion +from carla_msgs.msg import CarlaRoute, CarlaSpeedometer # , CarlaWorldInfo +from nav_msgs.msg import Path # from std_msgs.msg import String -# from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Float32MultiArray class CollisionCheck(CompatibleNode): @@ -25,6 +26,81 @@ def __init__(self): # TODO: Add Subscriber for Speed and Obstacles self.logdebug("CollisionCheck started") + # self.obstacle_sub: Subscriber = self.new_subscription( + # ) + self.velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + + 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) + + # needed to prevent the car from driving before a path to follow is + # available. Might be needed later to slow down in curves + self.trajectory_sub: Subscriber = self.new_subscription( + Path, + f"/paf/{self.role_name}/trajectory", + self.__set_trajectory, + 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.__speed_limits_OD: [float] = [] + self.__trajectory: Path = None + self.__current_wp_index: int = 0 + self.__current_velocity: float = None + self.__object_last_position: tuple = None + + def calculate_obstacle_speed(self, new_position): + # Calculate time since last position update + current_time = rospy.get_rostime() + time_difference = current_time-self.__object_last_position[0] + + # Calculate distance (in m) - Euclidian distance is used as approx + distance = np.linalg.norm( + new_position - self.__object_last_position[1]) + + # Speed is distance/time (m/s) + return distance/time_difference + + def __get_current_velocity(self, data: CarlaSpeedometer): + self.__current_velocity = float(data.speed) + self.velocity_pub.publish(self.__current_velocity) + + 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] + def update(self, speed): self.current_speed = speed @@ -35,20 +111,6 @@ 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 @@ -63,7 +125,6 @@ def check_crash(self, 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) @@ -73,7 +134,6 @@ def check_crash(self, obstacle): 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") @@ -85,7 +145,22 @@ def run(self): """ def loop(timer_event=None): - pass + if self.__velocity is None: + self.logdebug("ACC hasn't received the velocity of the ego " + "vehicle yet and can therefore not publish a " + "velocity") + return + + if self.__dist < 0.5: + self.velocity_pub.publish(0) + self.logwarn("ACC off") + self.__on = False + self.__dist = None # to check if new dist was published + return + + # Use for testing + # self.d_dist_pub.publish(self.calculate_optimal_dist()-self.__dist) + # self.velocity_pub.publish(v) self.new_timer(self.control_loop_rate, loop) self.spin() From cda33e0c028d23f347e05c4ccd16b41b42eb8142 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 7 Dec 2023 17:03:03 +0100 Subject: [PATCH 17/53] fix: unused imports --- code/planning/local_planner/src/collision_check.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 72173567..7368ec23 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -4,9 +4,9 @@ # import tf.transformations import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode -from rospy import Publisher, Subscriber, Duration -from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion -from carla_msgs.msg import CarlaRoute, CarlaSpeedometer # , CarlaWorldInfo +from rospy import Subscriber +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 From 35bc1048a0feea9730d6d17d229b7676808574e2 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 8 Dec 2023 17:42:13 +0100 Subject: [PATCH 18/53] feat: ACC Node added, Collision check cleared --- .../local_planner/launch/local_planner.launch | 4 + code/planning/local_planner/src/ACC.py | 186 ++++++++++++++++++ .../local_planner/src/collision_check.py | 62 +----- 3 files changed, 193 insertions(+), 59 deletions(-) create mode 100644 code/planning/local_planner/src/ACC.py diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index c469c05e..8cf61195 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -3,4 +3,8 @@ + + + + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py new file mode 100644 index 00000000..46b3167a --- /dev/null +++ b/code/planning/local_planner/src/ACC.py @@ -0,0 +1,186 @@ +#!/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, 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 + + +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 + + self.logdebug("ACC started") + # TODO: Add Subscriber for Obsdacle from Collision Check + # self.obstacle_sub: Subscriber = self.new_subscription( + # ) + + # Get current speed + self.velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + 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) + + # Get current position to determine current speed limit + 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) + + # Publish desiored 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 + # Is an obstacle ahead where we would collide with? + self.collision_ahead: bool = False + # Distnace and speed from possible collsion object + self.__obstacle: tuple = None + # Current speed limit + self.speed_limit: float = np.Inf + + + def calculate_safe_speed(self, distance, object_speed, own_speed): + """calculates the speed to meet the desired distance to the object + + Args: + distance (float): Distance to the object in front + object_speed (float): Speed from object in front + own_speed (float): Current speed of the ego vehicle + + Returns: + float: safe speed tp meet the desired distance + """ + safety_distance = own_speed/2 + if distance < safety_distance: + # If safety distance is reached, we want to reduce the speed to meet the desired distance + # The speed is reduced by the factor of the distance to the safety distance + # Another solution could be object_speed - (safety_distance-distance) + + safe_speed = object_speed * (distance / safety_distance) + return safe_speed + else: + # If safety distance is reached, drive with same speed as Object in front + # TODO: Incooperate overtaking -> Communicate with decision tree about overtaking + return object_speed + + def __get_current_velocity(self, data: CarlaSpeedometer): + """_summary_ + + Args: + data (CarlaSpeedometer): _description_ + """ + self.__current_velocity = float(data.speed) + self.velocity_pub.publish(self.__current_velocity) + + 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] + + def run(self): + """ + Control loop + :return: + """ + + def loop(timer_event=None): + if self.__velocity is None: + self.logdebug("ACC hasn't received the velocity of the ego " + "vehicle yet and can therefore not publish a " + "velocity") + return + + # check if collision is ahead + if self.collision_ahead: + # collision is ahead + # check if object moves + if self.obstacle_speed > 0: + # Object is moving + # caluculate safe speed + speed = self.calculate_safe_speed() + self.velocity_pub.publish(speed) + else: + # If object doesnt move, behaviour tree will handle overtaking or emergency stop was done by collision check + pass + else: + # no collisoion ahead -> publish 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() + + print("ACC") \ No newline at end of file diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 7368ec23..e0f0c25e 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -34,31 +34,15 @@ def __init__(self): self.__get_current_velocity, qos_profile=1) - 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) - - # needed to prevent the car from driving before a path to follow is - # available. Might be needed later to slow down in curves - self.trajectory_sub: Subscriber = self.new_subscription( - Path, - f"/paf/{self.role_name}/trajectory", - self.__set_trajectory, - 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.__speed_limits_OD: [float] = [] - self.__trajectory: Path = None - self.__current_wp_index: int = 0 self.__current_velocity: float = None self.__object_last_position: tuple = None + self._current_position: tuple= None def calculate_obstacle_speed(self, new_position): # Calculate time since last position update @@ -76,33 +60,8 @@ def __get_current_velocity(self, data: CarlaSpeedometer): self.__current_velocity = float(data.speed) self.velocity_pub.publish(self.__current_velocity) - 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] - - def update(self, speed): - self.current_speed = speed + self._current_position = (data.pose.position.x, data.pose.position.y) def time_to_collision(self, obstacle_speed, distance): return distance / (self.current_speed - obstacle_speed) @@ -145,22 +104,7 @@ def run(self): """ def loop(timer_event=None): - if self.__velocity is None: - self.logdebug("ACC hasn't received the velocity of the ego " - "vehicle yet and can therefore not publish a " - "velocity") - return - - if self.__dist < 0.5: - self.velocity_pub.publish(0) - self.logwarn("ACC off") - self.__on = False - self.__dist = None # to check if new dist was published - return - - # Use for testing - # self.d_dist_pub.publish(self.calculate_optimal_dist()-self.__dist) - # self.velocity_pub.publish(v) + pass self.new_timer(self.control_loop_rate, loop) self.spin() From 91e519e91a0be68e73150717bddce059d377e62c Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Mon, 11 Dec 2023 17:06:46 +0100 Subject: [PATCH 19/53] feat: Collision Check node finished and ACC publishers and subscribers created --- code/acting/launch/acting.launch | 4 +- code/planning/local_planner/src/ACC.py | 97 ++++++++------ .../local_planner/src/collision_check.py | 123 +++++++++++++++--- 3 files changed, 166 insertions(+), 58 deletions(-) diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 5a90b88a..586101e7 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -29,10 +29,10 @@ - + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 46b3167a..08f74cf3 100644 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -1,8 +1,7 @@ #!/usr/bin/env python -import rospy import numpy as np +import roscomp # import tf.transformations -import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Subscriber, Publisher from geometry_msgs.msg import PoseStamped @@ -14,7 +13,7 @@ class ACC(CompatibleNode): """ - This node recieves a possible collision and + This node recieves a possible collision and """ def __init__(self): @@ -22,11 +21,14 @@ def __init__(self): self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 1) self.current_speed = 50 / 3.6 # m/ss - + self.logdebug("ACC started") # TODO: Add Subscriber for Obsdacle from Collision Check - # self.obstacle_sub: Subscriber = self.new_subscription( - # ) + self.collision_sub = self.new_subscription( + Float32MultiArray, + f"/paf/{self.role_name}/collision", + self.__get_collision, + qos_profile=1) # Get current speed self.velocity_sub: Subscriber = self.new_subscription( @@ -72,35 +74,55 @@ def __init__(self): self.__current_velocity: float = None # Is an obstacle ahead where we would collide with? self.collision_ahead: bool = False - # Distnace and speed from possible collsion object - self.__obstacle: tuple = None + # Distance and speed from possible collsion object + self.obstacle: tuple = None # Current speed limit self.speed_limit: float = np.Inf - - def calculate_safe_speed(self, distance, object_speed, own_speed): - """calculates the speed to meet the desired distance to the object + def __get_collision(self, data: Float32MultiArray): + """Check if collision is ahead Args: - distance (float): Distance to the object in front - object_speed (float): Speed from object in front - own_speed (float): Current speed of the ego vehicle + data (Float32MultiArray): Distance and speed from possible + collsion object + """ + if data.data[1] == np.Inf: + # No collision ahead + self.collision_ahead = False + else: + # Collision ahead + self.collision_ahead = True + self.obstacle = (data.data[0], data.data[1]) + self.calculate_safe_speed() + + def calculate_safe_speed(self): + """calculates the speed to meet the desired distance to the object Returns: float: safe speed tp meet the desired distance """ - safety_distance = own_speed/2 - if distance < safety_distance: - # If safety distance is reached, we want to reduce the speed to meet the desired distance - # The speed is reduced by the factor of the distance to the safety distance - # Another solution could be object_speed - (safety_distance-distance) - - safe_speed = object_speed * (distance / safety_distance) + # 1s * m/s = reaction distance + reaction_distance = self.__current_velocity + safety_distance = reaction_distance + \ + (self.__current_velocity * 0.36)**2 + if self.obstacle[0] < safety_distance: + # If safety distance is reached, we want to reduce the speed to + # meet the desired distance + # Speed is reduced by the factor of the distance to the safety + # distance + # Another solution could be + # object_speed - (safety_distance-distance) + + safe_speed = self.obstacle[1] * (self.obstacle[0] / + safety_distance) return safe_speed else: - # If safety distance is reached, drive with same speed as Object in front - # TODO: Incooperate overtaking -> Communicate with decision tree about overtaking - return object_speed + # If safety distance is reached, drive with same speed as + # Object in front + # TODO: + # Incooperate overtaking -> + # Communicate with decision tree about overtaking + return self.obstacle[1] def __get_current_velocity(self, data: CarlaSpeedometer): """_summary_ @@ -148,8 +170,8 @@ def loop(timer_event=None): "vehicle yet and can therefore not publish a " "velocity") return - - # check if collision is ahead + + # check if collision is ahead if self.collision_ahead: # collision is ahead # check if object moves @@ -159,7 +181,8 @@ def loop(timer_event=None): speed = self.calculate_safe_speed() self.velocity_pub.publish(speed) else: - # If object doesnt move, behaviour tree will handle overtaking or emergency stop was done by collision check + # If object doesnt move, behaviour tree will handle + # overtaking or emergency stop was done by collision check pass else: # no collisoion ahead -> publish speed limit @@ -173,14 +196,12 @@ def loop(timer_event=None): main function starts the ACC node :param args: """ - # roscomp.init('ACC') - - # try: - # node = ACC() - # node.run() - # except KeyboardInterrupt: - # pass - # finally: - # roscomp.shutdown() - - print("ACC") \ No newline at end of file + roscomp.init('ACC') + + try: + node = ACC() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index e0f0c25e..9da23271 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -7,9 +7,9 @@ from rospy import Subscriber 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 +from std_msgs.msg import Float32, Float32MultiArray +from std_msgs.msg import Bool class CollisionCheck(CompatibleNode): @@ -28,49 +28,124 @@ def __init__(self): # 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 current position 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) - + # Subscriber for lidar distance + self.lidar_dist = self.new_subscription( + Float32, + f"/carla/{self.role_name}/lidar_dist", + 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) + # Variables to save vehicle data self.__current_velocity: float = None self.__object_last_position: tuple = None - self._current_position: tuple= None + self._current_position: tuple = None + + def calculate_obstacle_speed(self, new_position: Float32): + """Caluclate the speed of the obstacle in front of the ego vehicle + based on the distance between to timestamps - def calculate_obstacle_speed(self, new_position): + Args: + new_position (Float32): new position received from the lidar + """ + # Check if this is the first time the callback is called + if self.__object_last_position is None: + self.__object_last_position = (rospy.get_rostime(), + new_position.data) + return + + # If speed is np.inf no car is in front + if new_position.data == np.inf: + self.__object_last_position = None + return + # Check if too much time has passed since last position update + if self.__object_last_position[0] + \ + rospy.Duration(0.5) < rospy.get_rostime(): + self.__object_last_position = None + return # Calculate time since last position update current_time = rospy.get_rostime() time_difference = current_time-self.__object_last_position[0] - # Calculate distance (in m) - Euclidian distance is used as approx - distance = np.linalg.norm( - new_position - self.__object_last_position[1]) + # Calculate distance (in m) + distance = new_position.data - self.__object_last_position[1] # Speed is distance/time (m/s) - return distance/time_difference + speed = distance/time_difference + self.check_crash((distance, speed)) 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) self.velocity_pub.publish(self.__current_velocity) def __current_position_callback(self, data: PoseStamped): + """Saves current position of the ego vehicle + + Args: + data (PoseStamped): Message from Perception with current position + """ self._current_position = (data.pose.position.x, data.pose.position.y) 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 + """ return distance / (self.current_speed - 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_speed + self.self.__current_velocity def calculate_rule_of_thumb(self, emergency): + """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 = self.current_speed braking_distance = (self.current_speed * 0.36)**2 if emergency: @@ -79,23 +154,35 @@ def calculate_rule_of_thumb(self, emergency): 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) + # 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 Thumb: {safe_distance2:.2f}") + # 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=[collision_meter, obstacle_speed]) + self.collision_pub.publish(data) + # print(f"Safe Distance Thumb: {safe_distance2:.2f}") else: - print("Ego slower then car in front") + # If no collision is ahead publish np.Inf + data = Float32MultiArray(data=[np.Inf, -1]) + self.collision_pub(data) def run(self): """ From 4c22dcc9276c284d9aeb70cd398c7d6dd29e02ab Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Mon, 11 Dec 2023 17:07:35 +0100 Subject: [PATCH 20/53] feat: Brake implemented, Steertests started --- code/acting/src/acting/Acting_DebuggerNode.py | 38 +++++++++++----- .../src/acting/pure_pursuit_controller.py | 45 +++++++++++++++++-- code/acting/src/acting/vehicle_controller.py | 9 ++-- code/acting/src/acting/velocity_controller.py | 8 +++- 4 files changed, 80 insertions(+), 20 deletions(-) diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index 65d2799c..3509aa31 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -10,6 +10,7 @@ TODO: emergency brake behavior """ +import math import ros_compatibility as roscomp import numpy as np from nav_msgs.msg import Path @@ -18,7 +19,7 @@ from ros_compatibility.node import CompatibleNode import rospy from rospy import Publisher, Subscriber -from carla_msgs.msg import CarlaSpeedometer +from carla_msgs.msg import CarlaSpeedometer, CarlaEgoVehicleControl from trajectory_interpolation import interpolate_route @@ -52,14 +53,14 @@ # 4: Test Steering-PID in vehicleController # TODO TODO -TEST_TYPE = 2 # aka. TT +TEST_TYPE = 1 # aka. TT STEERING: float = 0.0 # for TT0: steering -> always straight -MAX_VELOCITY_LOW: float = 7 # for TT0/TT1: low velocity +MAX_VELOCITY_LOW: float = 3 # for TT0/TT1: low velocity MAX_VELOCITY_HIGH: float = 14 # for TT1: high velocity STEERING_CONTROLLER_USED = 1 # for TT2: 0 = both ; 1 = PP ; 2 = Stanley -TRAJECTORY_TYPE = 1 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve +TRAJECTORY_TYPE = 1 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve class Acting_Debugger(CompatibleNode): @@ -152,6 +153,13 @@ def __init__(self): 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, @@ -175,6 +183,7 @@ def __init__(self): self.__purepursuit_steers = [] self.__stanley_steers = [] + self.__vehicle_steers = [] self.path_msg = Path() self.path_msg.header.stamp = rospy.Time.now() @@ -291,7 +300,12 @@ def __get_stanley_steer(self, data: Float32): self.__stanley_steers.append(float(data.data)) def __get_purepursuit_steer(self, data: Float32): - self.__purepursuit_steers.append(float(data.data)) + 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): """ @@ -316,7 +330,7 @@ def loop(timer_event=None): self.drive_Vel = MAX_VELOCITY_LOW self.switch_checkpoint_time = rospy.get_time() self.switch_time_set = True - if (self.switch_checkpoint_time < rospy.get_time() - 10.0): + if (self.switch_checkpoint_time < rospy.get_time() - 7.5): self.switch_checkpoint_time = rospy.get_time() self.switchVelocity = not self.switchVelocity if (self.switchVelocity): @@ -362,17 +376,19 @@ def loop(timer_event=None): if not self.time_set: self.checkpoint_time = rospy.get_time() self.time_set = True - """ - if (self.checkpoint_time < rospy.get_time() - 20.0): + + # Uncomment the prints of the data you want to plot + if (self.checkpoint_time < rospy.get_time() - 22.5): self.checkpoint_time = rospy.get_time() print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") - # print(self.__max_velocities) - # print(self.__current_velocities) + 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() diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index 38e36258..6296c6cf 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 @@ -118,18 +137,20 @@ def __calculate_steer(self) -> float: :return: """ l_vehicle = 2.85 # wheelbase - k_ld = 1.0 # TODO: tune - look_ahead_dist = 3.5 # offset so that ld is never zero + k_ld = 0.1 # TODO: tune + look_ahead_dist = LOOK_AHEAD_DIS # offset so that ld is never zero - if self.__velocity < 0: + """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 += 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] @@ -145,6 +166,7 @@ def __calculate_steer(self) -> float: # 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 -> @@ -157,6 +179,19 @@ def __calculate_steer(self) -> float: # <- 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): @@ -218,9 +253,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/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index b5b5a91b..67d6a443 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -142,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) # TODO: TUNE AND FIX? + # currently pid for steering is not used, needs fixing + pid = PID(0.5, 0.01, 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: @@ -183,7 +185,8 @@ def loop(timer_event=None) -> None: message.manual_gear_shift = False # sets target_steer to steer pid.setpoint = self.__map_steering(steer) - message.steer = pid(self.__current_steer) + message.steer = self.__map_steering(steer) + # message.steer = pid(self.__current_steer) message.gear = 1 message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) @@ -200,7 +203,7 @@ def __map_steering(self, steering_angle: float) -> float: :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 but why? + tune_k = 1 # -5 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 diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 9035ba2f..b026bc7e 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -9,7 +9,7 @@ from nav_msgs.msg import Path # TODO put back to 36 when controller can handle it -SPEED_LIMIT_DEFAULT: float = 36 # 36.0 +SPEED_LIMIT_DEFAULT: float = 10 # 36.0 class VelocityController(CompatibleNode): @@ -109,7 +109,9 @@ def run(self): pid_t = PID(0.60, 0.00076, 0.63) pid_t.output_limits = (0.0, 1.0) # new PID for braking, much weaker than throttle controller! - pid_b = PID(-0.3, -0.00002, -0) # TODO TUNE ALOT MORE NO GOOD YET + pid_b = PID(-0, -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.0, 1.0) def loop(timer_event=None): @@ -131,6 +133,7 @@ def loop(timer_event=None): "current_velocity yet and can therefore not" "publish a throttle value") return + if self.__trajectory is None: self.logdebug("VelocityController hasn't received " "trajectory yet and can therefore not" @@ -152,6 +155,7 @@ def loop(timer_event=None): self.logerr("VelocityController doesn't support backward " "driving yet.") return + # TODO: soon Planning wants to calculate and publish max_velocity v = min(self.__max_velocity, self.__max_tree_v) v = min(v, self.__speed_limit) From aeb38352e5322e5622923c28385670f27d8e84b4 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Mon, 11 Dec 2023 22:41:44 +0100 Subject: [PATCH 21/53] fix: Comments and run function --- code/planning/local_planner/src/ACC.py | 25 ++++++++++++++++++- .../local_planner/src/collision_check.py | 5 ---- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 08f74cf3..c8202ff8 100644 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -93,7 +93,9 @@ def __get_collision(self, data: Float32MultiArray): # Collision ahead self.collision_ahead = True self.obstacle = (data.data[0], data.data[1]) - self.calculate_safe_speed() + target_speed = self.calculate_safe_speed() + if target_speed is not None: + self.velocity_pub.publish(target_speed) def calculate_safe_speed(self): """calculates the speed to meet the desired distance to the object @@ -101,6 +103,11 @@ def calculate_safe_speed(self): Returns: float: safe speed tp meet the desired distance """ + # No speed or obstacle recieved yet + if self.__current_velocity is None: + return None + if self.obstacle is None: + return None # 1s * m/s = reaction distance reaction_distance = self.__current_velocity safety_distance = reaction_distance + \ @@ -134,12 +141,28 @@ def __get_current_velocity(self, data: CarlaSpeedometer): self.velocity_pub.publish(self.__current_velocity) 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 diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 9da23271..f52b6b34 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -189,11 +189,6 @@ def run(self): Control loop :return: """ - - def loop(timer_event=None): - pass - - self.new_timer(self.control_loop_rate, loop) self.spin() From 273f7466374f3cc75a28b55c5fd9c15c624112d0 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 12 Dec 2023 16:44:38 +0100 Subject: [PATCH 22/53] feat: dev env created --- build/docker-compose.yml | 4 +- .../launch/global_planner.launch | 6 +- .../local_planner/launch/local_planner.launch | 6 +- code/planning/local_planner/src/ACC.py | 29 +---- .../local_planner/src/collision_check.py | 18 ++- .../src/dev_collision_publisher.py | 103 ++++++++++++++++++ 6 files changed, 132 insertions(+), 34 deletions(-) mode change 100644 => 100755 code/planning/local_planner/src/ACC.py create mode 100755 code/planning/local_planner/src/dev_collision_publisher.py diff --git a/build/docker-compose.yml b/build/docker-compose.yml index a68e97c4..47e13068 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -57,8 +57,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" + command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + # command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" logging: driver: "local" environment: diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index d26b4873..8d65c20c 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -1,12 +1,12 @@ - + diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index 8cf61195..9aa9f4de 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -6,5 +6,9 @@ - + + + + + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py old mode 100644 new mode 100755 index c8202ff8..679b5566 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -1,6 +1,6 @@ #!/usr/bin/env python import numpy as np -import roscomp +import ros_compatibility as roscomp # import tf.transformations from ros_compatibility.node import CompatibleNode from rospy import Subscriber, Publisher @@ -122,6 +122,7 @@ def calculate_safe_speed(self): safe_speed = self.obstacle[1] * (self.obstacle[0] / safety_distance) + self.logdebug("Safe Speed: " + str(safe_speed)) return safe_speed else: # If safety distance is reached, drive with same speed as @@ -129,6 +130,8 @@ def calculate_safe_speed(self): # TODO: # Incooperate overtaking -> # Communicate with decision tree about overtaking + self.logdebug("saftey distance gooood; Speed from obstacle: " + + str(self.obstacle[1])) return self.obstacle[1] def __get_current_velocity(self, data: CarlaSpeedometer): @@ -138,7 +141,6 @@ def __get_current_velocity(self, data: CarlaSpeedometer): data (CarlaSpeedometer): _description_ """ self.__current_velocity = float(data.speed) - self.velocity_pub.publish(self.__current_velocity) def __set_trajectory(self, data: Path): """Recieve trajectory from global planner @@ -186,29 +188,8 @@ def run(self): Control loop :return: """ - def loop(timer_event=None): - if self.__velocity is None: - self.logdebug("ACC hasn't received the velocity of the ego " - "vehicle yet and can therefore not publish a " - "velocity") - return - - # check if collision is ahead - if self.collision_ahead: - # collision is ahead - # check if object moves - if self.obstacle_speed > 0: - # Object is moving - # caluculate safe speed - speed = self.calculate_safe_speed() - self.velocity_pub.publish(speed) - else: - # If object doesnt move, behaviour tree will handle - # overtaking or emergency stop was done by collision check - pass - else: - # no collisoion ahead -> publish speed limit + if self.collision_ahead is False: self.velocity_pub.publish(self.speed_limit) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index f52b6b34..aaf53330 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -69,13 +69,14 @@ def calculate_obstacle_speed(self, new_position: Float32): new_position (Float32): new position received from the lidar """ # Check if this is the first time the callback is called - if self.__object_last_position is None: + if self.__object_last_position is None and \ + new_position.data is not np.inf: self.__object_last_position = (rospy.get_rostime(), new_position.data) return # If speed is np.inf no car is in front - if new_position.data == np.inf: + if new_position.data is np.inf: self.__object_last_position = None return # Check if too much time has passed since last position update @@ -91,8 +92,15 @@ def calculate_obstacle_speed(self, new_position: Float32): distance = new_position.data - self.__object_last_position[1] # Speed is distance/time (m/s) - speed = distance/time_difference + relative_speed = distance/time_difference + speed = self.__current_velocity + relative_speed + self.logdebug("Relative Speed: " + str(relative_speed)) + self.logdebug("Speed: " + str(speed)) + + # Check for crash self.check_crash((distance, speed)) + self.__object_last_position = (current_time, + self._current_position[1]) def __get_current_velocity(self, data: CarlaSpeedometer): """Saves current velocity of the ego vehicle @@ -101,7 +109,6 @@ def __get_current_velocity(self, data: CarlaSpeedometer): data (CarlaSpeedometer): Message from carla with current speed """ self.__current_velocity = float(data.speed) - self.velocity_pub.publish(self.__current_velocity) def __current_position_callback(self, data: PoseStamped): """Saves current position of the ego vehicle @@ -173,16 +180,19 @@ def check_crash(self, obstacle): if distance < emergency_distance2: # Initiate emergency brake self.emergency_pub.publish(True) + self.logdebug("Emergency Brake") return # When no emergency brake is needed publish collision distance for # ACC and Behaviour tree data = Float32MultiArray(data=[collision_meter, obstacle_speed]) self.collision_pub.publish(data) + self.logdebug("Collision Distance: " + str(collision_meter)) # print(f"Safe Distance Thumb: {safe_distance2:.2f}") else: # If no collision is ahead publish np.Inf data = Float32MultiArray(data=[np.Inf, -1]) self.collision_pub(data) + self.logdebug("No Collision ahead") def run(self): """ diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/local_planner/src/dev_collision_publisher.py new file mode 100755 index 00000000..c0a8a620 --- /dev/null +++ b/code/planning/local_planner/src/dev_collision_publisher.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python +# import rospy +# import tf.transformations +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode + +# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion +# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo +# from nav_msgs.msg import Path +# from std_msgs.msg import String +from std_msgs.msg import Float32 +from carla_msgs.msg import CarlaSpeedometer +import time + + +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=Float32, + topic='/paf/' + self.role_name + '/lidar_dist', + qos_profile=1) + + self.pub_throttle = self.new_publisher( + msg_type=Float32, + topic='/paf/' + self.role_name + '/throttle', + qos_profile=1) + + self.sub_ACC = self.new_subscription( + msg_type=Float32, + topic='/paf/' + self.role_name + '/ACC', + callback=self.callback_ACC, + qos_profile=1) + + self.logdebug("DevCollisionCheck started") + self.last_position_update = None + self.simulated_speed = 12 # m/s + self.distance_to_collision = 0 + self.current_speed = 0 + self.velocity_sub = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + + def callback_ACC(self, msg: Float32): + self.logdebug("ACC: " + str(msg.data)) + + def __get_current_velocity(self, msg: CarlaSpeedometer): + """ + Callback for current velocity + :param msg: + :return: + """ + self.current_speed = msg.speed + + def run(self): + """ + Control loop + :return: + """ + def loop(timer_event=None): + while self.current_speed < 15: + self.pub_throttle.publish(0.7) + self.pub_throttle.publish(0.4) + + self.pub_collision.publish(30) + time.sleep(0.3) + self.pub_collision.publish(28) + time.sleep(0.3) + self.pub_collision.publish(26) + time.sleep(0.3) + self.pub_collision.publish(24) + time.sleep(0.3) + self.pub_collision.publish(22) + time.sleep(0.3) + self.pub_collision.publish(20) + + 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() From 078c7dff2f8641232304a41ab33314477189e19a Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Wed, 13 Dec 2023 12:47:04 +0100 Subject: [PATCH 23/53] feat(144): min distance lidar --- code/agent/src/agent/agent.py | 2 +- code/perception/launch/perception.launch | 18 ++--- code/perception/msg/MinDistance.msg | 1 + code/perception/src/lidar_distance.py | 94 ++++++++++++++++++++++-- code/requirements.txt | 3 +- 5 files changed, 102 insertions(+), 16 deletions(-) create mode 100644 code/perception/msg/MinDistance.msg diff --git a/code/agent/src/agent/agent.py b/code/agent/src/agent/agent.py index e2732f00..d32f8907 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.0, '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/launch/perception.launch b/code/perception/launch/perception.launch index 3adc596e..eae08eb0 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -29,12 +29,12 @@ - + - + - + --> + @@ -68,13 +68,13 @@ - - + + - + 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/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 71e94d31..9c07c23d 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -3,8 +3,17 @@ 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 perception.msg import MinDistance + +from sklearn.cluster import DBSCAN +from sklearn.preprocessing import StandardScaler +import matplotlib.pyplot as plt +# 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 class LidarDistance(): """ See doc/06_perception/03_lidar_distance_utility.md on @@ -53,6 +62,13 @@ def callback(self, data): lidar_filter_utility.remove_field_name(coordinates, 'intensity') .tolist() ) + plot = self.plot_blob(coordinates_xyz) + img_msg = self.bridge.cv2_to_imgmsg(plot, + encoding="passthrough") + img_msg.header = data.header + self.publisher.publish(img_msg) + + """ distances = np.array( [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz]) @@ -62,7 +78,7 @@ def callback(self, data): range_msg.min_range = min(distances) range_msg.range = min(distances) - self.pub_range.publish(range_msg) + self.pub_range.publish(range_msg)""" def listener(self): """ Initializes the node and it's publishers @@ -77,20 +93,88 @@ def listener(self): ), PointCloud2 ) + self.pub_range = rospy.Publisher( rospy.get_param( '~range_topic', - '/carla/hero/' + rospy.get_namespace() + '_range' + '/carla/hero/' + rospy.get_namespace() + '_min_distance' + ), + MinDistance + ) + + self.publisher = rospy.Publisher( + rospy.get_param( + '~image_distance_topic', + '/paf/hero/Center/segmented_image' ), - Range + ImageMsg ) + self.bridge = CvBridge() + 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): + # Standardize the data + xyz_coords_standardized = StandardScaler().fit_transform(xyz_coords) + + # Compute pairwise distances in the x-axis + pairwise_distances_x = np.abs(np.subtract.outer(xyz_coords[:, 0], + xyz_coords[:, 0])) + + # Find the absolute minimum x distance + min_x_distance = np.min(pairwise_distances_x[pairwise_distances_x > 0]) + + # print(f"Absolute minimum x + # distance: {min_x_distance:.4f} meters") + self.pub_range.publish(min_x_distance) + + # Apply DBSCAN algorithm + # Maximum distance between two samples for one to be + # considered as in the neighborhood of the other + eps = 0.2 + + # The number of samples in a neighborhood for a point + # to be considered as a core point + min_samples = 2 + dbscan = DBSCAN(eps=eps, min_samples=min_samples) + labels = dbscan.fit_predict(xyz_coords_standardized) + + # Plot the clustered points + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + + for label in set(labels): + if label == -1: # Noise points + 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) + + # Set axis labels + ax.set_xlabel('X') + ax.set_ylabel('Y') + ax.set_zlabel('Z') + + fig.canvas.draw() + + # convert canvas to image + img = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8, sep='') + img = img.reshape(fig.canvas.get_width_height()[::-1] + (3,)) + + # img is rgb, convert to opencv's default bgr + img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + return img + if __name__ == '__main__': lidar_distance = LidarDistance() diff --git a/code/requirements.txt b/code/requirements.txt index 3b0948d0..8efeaa45 100644 --- a/code/requirements.txt +++ b/code/requirements.txt @@ -11,4 +11,5 @@ scipy==1.10.0 xmltodict==0.13.0 py-trees==2.1.6 numpy==1.23.5 -ultralytics==8.0.220 \ No newline at end of file +ultralytics==8.0.220 +scikit-learn>=0.18 \ No newline at end of file From 58249f6f7d486c9d46fd5b130922174f669e7399 Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Wed, 13 Dec 2023 12:47:31 +0100 Subject: [PATCH 24/53] feat(144): min distance lidar --- code/perception/src/lidar_distance.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 9c07c23d..7a00b2cd 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -15,6 +15,7 @@ from sensor_msgs.msg import Image as ImageMsg from cv_bridge import CvBridge + class LidarDistance(): """ See doc/06_perception/03_lidar_distance_utility.md on how to configute this node @@ -129,16 +130,16 @@ def plot_blob(self, xyz_coords): # Find the absolute minimum x distance min_x_distance = np.min(pairwise_distances_x[pairwise_distances_x > 0]) - # print(f"Absolute minimum x + # print(f"Absolute minimum x # distance: {min_x_distance:.4f} meters") self.pub_range.publish(min_x_distance) # Apply DBSCAN algorithm - # Maximum distance between two samples for one to be + # Maximum distance between two samples for one to be # considered as in the neighborhood of the other eps = 0.2 - - # The number of samples in a neighborhood for a point + + # The number of samples in a neighborhood for a point # to be considered as a core point min_samples = 2 dbscan = DBSCAN(eps=eps, min_samples=min_samples) @@ -150,14 +151,14 @@ def plot_blob(self, xyz_coords): for label in set(labels): if label == -1: # Noise points - ax.scatter(xyz_coords[labels == label][:, 0], - xyz_coords[labels == label][:, 1], - xyz_coords[labels == label][:, 2], + 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], + ax.scatter(xyz_coords[labels == label][:, 0], + xyz_coords[labels == label][:, 1], + xyz_coords[labels == label][:, 2], label=f'Cluster {label + 1}', s=50) # Set axis labels From 1c9342e2aa0a348aee59d782e4bd6638819018bf Mon Sep 17 00:00:00 2001 From: MaxJa4 <74194322+MaxJa4@users.noreply.github.com> Date: Wed, 13 Dec 2023 19:17:39 +0100 Subject: [PATCH 25/53] 109 - Evaluate PAF22 Traffic Light Detection + Bugfixes (#124) --- .../src/traffic_light_detection/dataset.dvc | 6 +++ .../src/traffic_light_detection/dvc.lock | 34 ++++++++++++++ .../src/traffic_light_detection/dvc.yaml | 21 +++++++++ .../src/data_generation/transforms.py | 2 +- .../classification_model.py | 6 +-- .../traffic_light_inference.py | 7 +-- .../README.md | 0 .../1619_PT_fasterrcnn_resnet50_fpn_v2.jpg | Bin .../asset-copies/1619_TF_faster-rcnn.jpg | Bin .../asset-copies/1619_yolo_nas_l.jpg | Bin .../asset-copies/1619_yolo_rtdetr_x.jpg | Bin .../asset-copies/1619_yolov8x.jpg | Bin .../asset-copies/1619_yolov8x_seg.jpg | Bin .../globals.py | 0 .../pt.py | 0 .../pylot.py | 0 .../requirements.txt | 0 .../yolo.py | 0 .../README.md | 43 ++++++++++++++++++ .../assets/back_1.png | Bin 0 -> 22467 bytes .../assets/back_14.jpg | Bin 0 -> 910 bytes .../assets/green_22.jpg | Bin 0 -> 1633 bytes .../assets/green_4.png | Bin 0 -> 22317 bytes .../assets/red_10.png | Bin 0 -> 16513 bytes .../assets/red_20.png | Bin 0 -> 708 bytes .../assets/yellow_1.png | Bin 0 -> 18050 bytes .../assets/yellow_18.jpg | Bin 0 -> 1682 bytes 27 files changed, 112 insertions(+), 7 deletions(-) create mode 100644 code/perception/src/traffic_light_detection/dataset.dvc create mode 100644 code/perception/src/traffic_light_detection/dvc.lock create mode 100644 code/perception/src/traffic_light_detection/dvc.yaml rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/README.md (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/asset-copies/1619_PT_fasterrcnn_resnet50_fpn_v2.jpg (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/asset-copies/1619_TF_faster-rcnn.jpg (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/asset-copies/1619_yolo_nas_l.jpg (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/asset-copies/1619_yolo_rtdetr_x.jpg (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/asset-copies/1619_yolov8x.jpg (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/asset-copies/1619_yolov8x_seg.jpg (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/globals.py (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/pt.py (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/pylot.py (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/requirements.txt (100%) rename doc/06_perception/experiments/{model_evaluation => object-detection-model_evaluation}/yolo.py (100%) create mode 100644 doc/06_perception/experiments/traffic-light-detection_evaluation/README.md create mode 100644 doc/06_perception/experiments/traffic-light-detection_evaluation/assets/back_1.png create mode 100644 doc/06_perception/experiments/traffic-light-detection_evaluation/assets/back_14.jpg create mode 100644 doc/06_perception/experiments/traffic-light-detection_evaluation/assets/green_22.jpg create mode 100644 doc/06_perception/experiments/traffic-light-detection_evaluation/assets/green_4.png create mode 100644 doc/06_perception/experiments/traffic-light-detection_evaluation/assets/red_10.png create mode 100644 doc/06_perception/experiments/traffic-light-detection_evaluation/assets/red_20.png create mode 100644 doc/06_perception/experiments/traffic-light-detection_evaluation/assets/yellow_1.png create mode 100644 doc/06_perception/experiments/traffic-light-detection_evaluation/assets/yellow_18.jpg 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..73aa6bd3 --- /dev/null +++ b/code/perception/src/traffic_light_detection/dataset.dvc @@ -0,0 +1,6 @@ +outs: +- md5: 3a559397ebc58c1ecf142dea18d03367.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..d9f625ce --- /dev/null +++ b/code/perception/src/traffic_light_detection/dvc.lock @@ -0,0 +1,34 @@ +schema: '2.0' +stages: + train: + cmd: python src/traffic_light_detection/traffic_light_training.py + deps: + - path: dataset + md5: 3a559397ebc58c1ecf142dea18d03367.dir + size: 13745063 + nfiles: 2723 + - path: src + hash: md5 + md5: b6c9cb867c89ad6e86403d9c33538136.dir + size: 23777 + nfiles: 10 + params: + params.yaml: + train: + epochs: 100 + batch_size: 32 + outs: + - path: dvclive/metrics.json + hash: md5 + md5: af33de699558fbfd3edee1607ba88f81 + size: 218 + - path: dvclive/plots + hash: md5 + md5: 774919de9e9d6820ac6821d0819829c1.dir + size: 8900 + nfiles: 4 + - path: models + hash: md5 + md5: ee67bac2f189d2cc5a199d91ba3295ac.dir + size: 10815 + 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/src/data_generation/transforms.py b/code/perception/src/traffic_light_detection/src/data_generation/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/data_generation/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_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..4631660b 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,5 +1,4 @@ import argparse -from pathlib import Path import torch.cuda import torchvision.transforms as t @@ -24,6 +23,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() @@ -66,8 +68,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/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 100% rename from doc/06_perception/experiments/model_evaluation/yolo.py rename to doc/06_perception/experiments/object-detection-model_evaluation/yolo.py 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 0000000000000000000000000000000000000000..b0a6a2f605ed2a508a81f340334f8d99f8fc0f24 GIT binary patch literal 22467 zcmZU)1yEc;voN}gJHcIo26uONw+-&@?kpj}A;H~(`{E88+}$mUL(qf}93J2O@4Nr4 z_ok{(_nEdca;9dwdSW!y6)@3A(EtDdrjnwp_8S&`gG>~pw`aRYY%BnPrtBaiqp2h# zL#^rQX6xW=0{|$-q@^S4;vW%?Tux5^GZ|;6rnWgz>tsboVa-Ftk?pLgQE}VV)^k?m zgu{`OLr9H|tTN|?!$mj$L5RS*+NAhS(+XlPbAXv_A1rj)bA6F3a(laVKb`j&@-vqw z4Di5bp{z{94F4@Pm*L;7`{C!$kshNml0P^|G=WY3!6Cw37c3Y6?th0i#06;97o8bh9^>ay$TsvV=8Adbr=>PK z$Nml|{5mu8Q>Gr8)%Mqo&#Jeo@2^)#5$7R~M*V4uLz%iu4Rq>@lk30%sZnO87kOXn ztJlIy20eY*>&-=FibeFoGyL!4_%5`PPZIvHNg%L29Nql~x$~6>QTz`$)hiriFK8~8 z(IiIGfPbdY@W=?^-6&;WHy0VWxuT}>8JJc7V2$fm*l^rZaa}>$};_gqV2vVaccowwE(Zb&&h~E>UV0%bze(Q*qz>Srq=2GNN zPPC>a`vzA+%p~)V#%J{FC|3!52ly{d+c%&!en4Wk4C)8?G#NcDlSTZ4BJ~OZQZT!; z)$GBsGB43qeEN62V_-AMxgpw z?>KYmAE+bWSw&|Jidv#JWc#Q3r~Bs#$_C5PB;hLt(m5qFGNs~c7|Jozoip$ytt<7E zNX-josp`{^C7qA;TE7=c$W_>u6PFa1yP(BR`l)%0?-PqSl!q&?RJO0F{NW}wB~^T! z)8-Si%u><1c1UH?BEcffuTV#_8#JG2VX2hTleseoV6*>vpdnLjl5e_i#=lSMDap({!>FyRn&c?Btnbjtq(S99q;Lj+s&LaS3Aau~;@W{RL!vA{XK- zNOEj>r;l@f8+w${m~km{nKdZ=?%WljPfX6y{xq*&N>~~+p4nLRC-^}1Ky2JDtza@W z$3Z}cmnZCSC#y|QlR$Vr0ap{Mu9va5sxuRXIyB2WI=Q8YEf}zwR*0WtEjS^q#~<$tt3Dru54%_c6q&S zsgkd(w(h(*uXd|2#3-t2U?FB{t#YCC+R#8l0OVKf8g~($|2<#Qt<*GP+a#o{xA?{L znfciXi4Hzr>eCO`2uhj7*>TQg$mUfnj^iL-EYG^jqDY+JgsuLcnyQb_!x&bKdl-92 zdq6zIlnw?V1~vu_wHCDj)wXQktWe%2ZWZ1QH{X$5n+uzKt8Lqt&HhGz{W_OFJf4clZVH<&>?!M+4Xy7P4BvaVbJY_us5PK< z3_7?2*AIQ&H@J?)iJFWpLm{o2XCTt6T7ME7R#>bk5u zq1@Ysi62@VG>jK-r5vA}cPurT6*9j={e@s^I^tJ*sx6&;Ja%{SR7x^XGRR}7IWEIu!eD^{>PvuYiR9~K@?9~6)AieZThBD^FqBY746AyC9%!ObPuz!Z~= zpXAM&m6rQ0*fwjuvBsu{NuDWyDfysv!jF{-FDR~BCuvcxvev=#4*SuwGxR~=1fB$o zHv1XE`Sg7@fmUJHTDEjH(~qL< zQ+7tzzSIAn&N=*8*F1`>eU%Q>*QfvHCC_)1)^;`fxvbsvvbLTDc6bHod{G`Mi(ToW z^}9Xaxpb(}t!1qUz75&D6}SyNbE%uJWnNQQ&uF;Zc`1B0k+=%2sMfDzuXxc+F{Z7& zsTb3+Gchoz*EU-O^Op$3Q=&-Bdd=RLI~9BEZ8Ll+`y}K3;d!>0iZ-;{WA)Mf2xmKK zxWGHKN^o2}`)53Fy*;H6$YfXIm5 zgM-hkwKMoHQMFm3uUA)2J;==Ayy3**G;blOai*buuj+KkYXxD2-K4;;=T;t;kn_Cr zoSc&+px{cs9q22v_ir%I$1le3`f_ZOrK8@n@0US@Sq`1?SPXC_d;*(Lw4LVzY4DTL zFSpAmPbzMzbc|~3l>|^cIh7C9c|h~k-0WWJ2%g5D(2VDv-O87Rf*07UM*yp}^KsVQ z+WvJvkury)>2&Ct(1k$gtKNKd%*wr6>h@yqLC<+0(UYq`&9&Hl$O&=;whL8{#CD!) zcCIQ;?3aQP0ML(Dm=qR(3E;tBK@)3#F%+!5ep!?l{h2x&26JG$8*2wozyW^x0{k|e zJeK$1M|Urxf&f&SDibp=<)!jQhN>&y+hFr0)}DXlI00(_n9S?`zU;*Qew1VP+6Lgf zzU<~8bqw~8{@FFFELfyg_QJ!d7lB>=xE;mNxAEF7E%a0}%BWej{CMye+8xU7TILg#E>6 z{!2pmjs8zH2MzUqiFiAS(SX!6sb$J158g*nU$L z{ZFm1rh~tY^G8_+m$yE9^C8Z~$1VC_^8Y{1|MmF4G(rDYlb`c{EB-IW|5s7Z%f?g2 z&E?IbxA^}Zt^Zd3-^~A36y^9&!~a)D{GVR_uiCe67Dp51_@5yYM|%{VdmCT?xr3~R z?i+raWB*lD-<}`d;6L~c7^v&yjo;=K{h5-il&(L}IX57_G{|tJz)#F+sj{@o&F^Ox z%QTfBT=qA`T`D>TdsS)=U<3jki3P4Sh6O5TmMW69YmKktP!h zt7Qw73&G-TRUeJ?i1Y=%Y;^}LxNdbGq#nc!Rent7<o|$1&qTN^{TbsTZ zZB>vty$MuzNM8?bfFSbjb=%q5`3465Ivw(NCfZXeUvswYtXsaRYPNLI$4`)Z8WU8Q z_UX#2!+fMA!)opLS(hMW%A2KHlW~*RSu@lLxe0WsUK;=UH9}N>|QTS2E9*AB8gpZdT%Y~ENy{HJY?Ll)UawCM_&Qd`^!8bVAIbn%^eXa3) zi%ng$9WM8%gFw&0=A%d54S{*M>km_7)Ai3&gdmp(M?Fx7YYnR-k1PKKF7nn`7yq$~ z)JA$KMV}dPnTrG7l+B>km^~z=gG+@b69IO9?h+?{e@*51vboZ?an=4v{b99%P9 zzV_iVGgg`)Ft6+DL4<`pYlN2EFPL4kcllTqMFCb$Uuvpdh;v8Lks&l65b zlw%}V+_o4;L9p9&$ywW42_s67U&Tu{$UZL^J?)nuaw(FE;JVt{rZ;R+Iku#%vXpSw z-4dRT_hD6|udf4j#n-QOt)tGTL+NDF+H*QFj=kov5b1(f%|%2cq^m&!5@Ijf>yAYQ z=&U?yl~8sLV{Dn@yuLmTrkKva?o7V{Zcn+eC#sg!;N?sjsP7G-8F3C18cP#7|64y) zk?DMQI&LgXXQPyC#S-bBL-w_+u6%x}@xxkt_(6=IgtwB%vwR@zef?mKQXX(_=K0Q# zuSXrt0N}Y7xnSQsZV%ij{8~PbrOc`E4=9QocVpb{6PRacfrDiVcLIO zO^=KqhR;~XYL*RUUrG`V$W6zRe}r?58J4QvF446EmfaGrze7tC8|H%Mip19!^I4*Lo+e1leU z5kSiykyuWtBey0kagy;T0G^MA271;L%>QmYriA-1n=qFZ+73&((3JuYjEZ zddmuJUG25~N|ey-{Eu5VdctfhY*kJs=k!pjm!Pnoju0oRo=u-7HZ%H&1y6(1ot+}F zu(v--l%*k+@23OnjOAZuk9Cq^tbfy{`{dJ&lTs7rB;uV?3GtVYk2y1F+s3r6hUHBi zal8Ja4SP*l6qFoiSF*ib?oc>3tPG!@<49G1SSUoM&y-SODZ3a#Lx zk7*^ID2QvtHhykM)f~vQ&Gul4gpeOUjhe!o%)0$86f|+`8)(p&S1-E%?rHVWic^Ko zvY}tNzDV!aJiTk3z7w=uGt!1gz=$>u?~s(R;ly2N7)kc^jc00%O5P~hri6Od2hQ|G=@q}anWaLPI?<~JCkxduIE=y@d)xf{1@=HU9 z7y9$51k^|D2*{cs^j%G0_HwR*k>2qyaju9ixfw~}gpviaLIMSOPLCjy7;_4J{s(^#$ZqiD@JTS4^d+Lc@xR;G z6Xf2utQm&8%0Fn3_~nz6AM!N?wCcE(k{`Q0J*MBCx-t&?mm-j4!S{9;tW9l=1KV~A z;gOu%;E%f-@vGTn(k+Kn28G^H zt{52^eLbbSIcKi6veZ7Zz?B-ltPMqa^C&|ZCw+o zbC89r$NpF+uBF^Oe>*~$$UY3ap2#YmH`KFwrYp@L5@Y$5i~#nY5vsU85CEkx<&J{~ zgC11HTPuNVajXprjk0kz))ndgB5D_U1b}1nIQy?$=5Y-p0gxzrFwHE^T`*?!H$;FH7uLJf&J92MD zy=xNnh32^e$ge}G<5g0M%i&N!pWwn1V4~^3+xdpFv$r*NuKy>Te+$DGlZ5C=c?5<^ zG5@PsO7URxuapuyJ?!%A;Pjo8>Ul0t6&$iSdQ_U{na~I1?36v3-I=%WTmKJUH9+uj zAqTrDf{ou1wz+K6PlDQ*9s=pZnhy9-mp+J5P{r`OuP0zUm4l$Q+;}xyC1;xsD-?}s zcDQ)H$N{X(J!`F;>{}=P&mU@bhgoDhK~^zwWV5{qJAaEuzMlOqR`|C+#`C=qe>~Wv z-sX1_8d$;q(;yICu;y_vM`A91%$yJ%1BWS9QK`QJpi)8s;aLs)E9^uDX#GhmBNgTK zSF6r}2=@_^eee!w3+v`}gljc#q)Mx>4ZX@c!X0DG_SMZxVk(!0SWb!XPHm zTui)m6=!2w)Ij|bdR8tiJG5C0*k+7e-*YvU7gQzK&`MBwf{;l7d|-ZCgrPs&RQl5T?di|C0XY= zqk?B2-Ot2?ZM;R+#jp@ni}SOmdxDIO8rf=iN_?!&C5!NFTDD+{q+WFUaJOejyiOiI zR=fJU20~UR1ejoC>S`AnLJyHSNltVjJP+1y_^Q#!z>5YKZ_hM)h6x|Fze54$O*c>@PGj4>HJ^EGFnijFf0~f=#pIHDGCl&NX5na&N zxIP;nQgivuebZvAPrK=R#aZ~^{qKGQ8&emW#!TG6L{uq86!HxAo(|>&rD8n2pi28TDW7YKv3m0n-`#G(lwZuhVtP%?c_q6tU$8j}aN6zWP z!#>b#T0g4vwXULN+b|8q`)VjQK}6_%go1XG*ImL!OOHoa$1X&&Yp!2*YO$bVrRg+1Ch`J8U$G(-_r+h*FJ-U zf0=u4673PfAQ6@Xv+2fa@B^@#&AApu^cp-cTj!LJ>-CM8on|!Ix6zcvM0LFKl$6;m z&GR6BlMT;pNj`oQXy4V>(2A_@18`@hYW4dXoUc5eu7pq@M2V>fb!e(^HS%lWaTF1t zhu~LyvR5Q;_#}P0CAdT;)S=YR&8}C%?ubU73uowrO7f`5dg(uy;qC9O+hE$*r3pyp zL6}_ILnyW1-{*QHZybM<%}` z*XQ_^7o}ovb17;F_ifXpvzD`i?ueaxyEs(dm@?!T`NdEt&FO1F53sZm)uZ}B+3P|1 z7aJlvvkEE-nx}f@;74=42=#Llz)NRWOUTx#AQl|kADoJ+d(#i>jtG(oM1#f zHALsR9I9Zdf~soG@96K_-@#COO2_amUEn=xxxhM523=}Ikdy-Y-RY(NKTL41%DeeU z-&Zpz++a1*F$*{iJMPIVT&@T)`&7AuEdHb383ObRU_famLOy`eQ63IS4wgU7+)o&;-`FPHg1il>BYwdlv~i)3tDuVOC5} zA{P3YfMAqOfOkDWLE0<6T2Qs`G%W)V*9G^?9OZL`RI|(i)m5r80khPQ(=@{AexE4q zxi+^6`viJLJ#{7&ruXWMb~>p`#v05PfgFduUNGjc8%sYJkD25GeS;r9eK=hXbb{I zvE~Q{d-@{4taeXmV*`o`f&v306|47^Es(HOxUEO`Y2Ef>UohBDdfFQ12j`3CzI=ca zq@sQ~_F$*@*Y^M%BGXPXox?xc`Ah~+kL3nvE95dH#IXoSrH2*`Xu{o%z2~bVN~%{- zC^v=a6o(2I=XMol_wGO86Mp9i{%3&W5RblLXpW%!*A!^n^ONf(VDeX~W|~aV`lwpM z86$JMz?W~z09@S2#qAKDhzhL*-``JJTVDa2r&_O{I!*Wih2dkw{GG&HS*iMMix1%y zV^a6i_5`bhf-USS|L)kALY3Pa`^Q7%?Dl#W?n^X`j-$Ury@J%6UXmf6!b|>0D`{9^ zFFXZMRCh~t=HDpeiNKVWVUtQ+n!2t~tHotRUCW1wFGlCE9G>;sX3g8c*1qcLKISB; z8Y_%leLqAkruWk5U_?XQM(Xa|25s08CBlLk4*5|AVSDceieO8fAiO);94Q+)1j9fs zv0G!E`1qvh=->WttDvWSu3zl{Z>81Bsh#e=+sd#tUVzKu>p_Wd$8y!Dja<#s3H`tZ znj6F2>W@QT(g%8^W?GM>v6j%w?rSq@T$Gc-QaPN-*2mLBLy@6Ajn>k5WU-9FBDzd} zv!+=fKQg1He&42v9Cd+*rOS+5i_(fSbonZxW!iFe_X1g=vnMCJ{tpf8lu8{c^Xa8hL574ExaJEIX5FROWS8s_0d!YYgN3)(zhio2MIPb zgsVaOHci5?pak@4e3fFt9u0)<=l&pq-WEOVnEOx1DquY{OcB+o4h5I&lGPaX{foNT zhh}rWON)%H->qCd13v!EYP4Ro#a^{C;jaO+MxC&3`50vOGl%IoO_ijBS>sb6P&wwg_3yBFR9I_UzqCMRW~A4+l&-~>eyb>Y2>P1gaH zS@1`v({?RlJiGc8@4el<065kc3uu7*>i4n{mb~=C9$}B#!3zk- zDn-?2<^_7F3RCbTL%Vo(GrpA>IX6jM8pNwSbjeBtbj*iW%Ro0CF!ojU?Bi0I(4BMIvLjMHKMT? zV_v+w=yzFKQa%*j(t)79kle;h$fZ?aD*}crwgkfVvYGN@ao?T(j-wT^7L{pYx@MZ& zgDcZQ8Rh9De?9dJJ%0tV+6oZ~Y^FXsFu4l^Ul1fs6{uzk=?slPkCRHNA<0DcFhE$5HYOZZ8cG_{z?pDT`kBmlMbQVOwf z)LL;NSfp4^s;hhO{7h20Lp#Ltn^Bv|zfM|)nHFPoPYlGW&F+sizLm!0?Cf`f0rtnL zu2Rd~U^JMqwNaAS)~@`41Tg0tRnYh-skdItm%${j>QA)A!!|AN4db$d^>o?D&AS3# z5P{1?Mb)<*(KTL(4xg2j5`*5sVUyGlM-Zg7vtE?zN!5QvW!0US4f!h*7F9$TuD9Q` zru3o}_zB!sJ8FN|rZL-ql!WOzmNT7GC>x_LU&&|(EsbEA_fKAq@6|awG2`^j-zlum z@84t=W1qR`rS$`l!FTzdV$t5sDWgS<77uw1C0(A zKazV9`!4d&S2h44$kK?NSP`2pV8FEj5`~KC4Nptp+486+I1exj#{CgnJc)*U$ z9I8zkdo{pq&E8t$DFSEIScSA~YW78xY3(f>wd^q(@Z(z?nwMhJ(RpkcT|XbZ(+&4G zZxg{>tzX8@f;GFQe|y~?n-UmYQYYJp7BfuziVFu^rkS`r!aBH^tGp}94)py7wv2xu z_q9dJ|MY;6K`mqkFbVi>lhi%`DRcmQwnNcH(1fE3 zE|$cONl}PrP4x9A{N&J!CBvS*fxChz_{|lR7q;sn=`_r(x~|RtrAev;Rs62l08HWC zUIh2yH`XiZj58Xy)Zh}kJVPra#NTEs#%$m?@?@_iSgCZmif=p9js%eJaeGLtOV(0knBzs zmQPHfalIs?35a_;DTQV-8BJp9h*G_Jl<=l{_v0}aDU;|l#nOC9mX#dze9uEumclq< zBBzN>+tNbcaFKj#yV!`kVF3;fa?ca}%Vi#t{8{|D!t_&;_Tu4?fS|epSA@20pYEWQ zA3U(}{aSf*IQ`Hu3g;!01BlZ_AWG8+I*%POkdc8Gr0T}>ZCL7RMp+PGT+OA>qNBSv z{{8h&<=2!#&L7HMx_DgQt+K}m_^M;&v-QtYX|fCP{sh2H%H~&*xcdU2c@~aAfNd+m z;HcXNj4W*bDx28}rgKh&6mD58_l|G;kH2P(Pv^aUX-~yN-n%T}Mq%CRLH6XI{ai`+ z82xz7ypN+Ly^xdw%A~x3u*VrvBtuzm)Fnq8b44)UrOmVj;Pv+BGrckS0d}l!tb|^& zn1~2{*yl348oSVxPA#p^ha-xzlT}Gkp%T~Mx9kDt^3k_Iu|H}6h$Os!Cifv7ln(7Y zz#4(idb|AdMzLug^~1E1>`FLy7Lg=+x2*9F-6jD@(#g0TlWL~Y9i;+*em4PTG0rsP zB_;POz8RW%MdDZ9G%{&X;mG>Byb4*EfaMFpEV#oA~DKV6Z!rz+I| zYu@RQrz_%_+n0Gp{?@jFVax_{?!WLf1tCZCR>$kJ%oBtw%hh*$U)lxtJxNG>!8%2c zKiZx7OPo)-7ln*{esxcK+@LWSJ%Bdq1@?t(Q`GpEfo)`rOaMGQJsLQs!lLG|Zh{j^ za8R@tBd#VNWQW(O;PM}kb(|l}=aMtqjk^kB8D@6RY>FcFgSjz+p33fjFgq${d z%c$)qEmMVF{N#_#S<$80NRLHYP@mi4`3pL2DM~7>J*HmdU-ZI=ZdGUg{@Yw&c7zOL z{mdL1DSAggo85d4vy;n$Uk0T*4tAH1gizNibK@s6gXS)o-dDA=_Z&)N8Il`T7|4b= z4tqb1jbVt$>|I*B-m2q%Xn8C@W_@o@mY5zmXpU$uPZ`TsbYLg`d_5IQ+Qb0B&*%j4 zD2v)A`%dDqS=pAAKGiz?nped;?)?-!#g2AWh=^eiq z+#`lnh6AFt$nb~4c?Y%Ws9F5ic*?zArnOlN%@ZGE!C5@Y8=*+bTvTUA?2k*>MCLup z9>fqV!?pRo`#}kN+w9-qPxM$fn}`?oiAIK3Pesz|H@T@LekeFW?kWINU_6lr7CeaE zQLgCMI;0R7?zJ&loq-Vq%y5aTj6-Wrp8`h^S5b4HaY?@(^s;}Q; z;D^&FIe^{G3ZjHp(IfDb`Bu4r&EDW)nawW~PQaHVw-HXLWR$nZ{GkF>z|~=os*#v9$a*>tmC`DwVaQP1#ZJ( z@zn!Bg?R&T7$-vgnjmD(8GecaPA(LOkl?Jf6pxGQ{D4^%p&t`8=Q3;(w>m#p>=R`$ z8^38zd~yX`hjwHKvMK+rsUo6~R$EaT)B($QCMv@FWA}9`yyK$Dzep0hxlyd$bp-nY z*|7RB1c5agcaYee^8pi!M(~*$o@7aF5n99?4Picy8(%S6pLNk)%siB}N7 zk1Bu1%WT(OiJ1va7$ZvJYT)OEd^MDfDd}G!6sWPo4t_u5f+JMhIB(+G9S5pTt=`lr zL~VB_n%mT2J=f?&agMT&?-$)=oy`*LjwvS4ajIgZU!SjPKotO)LUcOE8DN;YAe0&G zrk=lkS(Jmd7Nx+@(7Fy+Nf|StvGcsWNdq5ZQIhZpK6w=|i-6(hL5aIww<1Vkjy@a? zpjyC~bBGp#n0uLg^VyM><)w`=QEELo;q>UJ zmIA0s4XL!0GE@G1-TJwy=$-o0w2P{2Jb+aaJVAhMy@S0$x?}*Kod@bj;7OC1-_RLV41b=YMk@jp9uZXf_qU*W z&En;=_q7|ZX5=6JD~*`!eR{Hijus}1w6o5Z#xFniUUTr1eEfoHw#B;0Y=Kb{6&0|3t+uKbN|4tNC0D(^b*YFLVFKoi_%GuExINF z6HJ+SWRiE#U2EYM9TC`xTaM~fikdtR*=2hQ&{gwf?mjg(SiS+O#RM&2)v+ z2N?i#V;E;HWA^oa%lVZ_fsx_pfdq^U z*$0un^BI9E{;D!^j93ixk4R1Ad>s(|f{Fp@34cW*3QihkXdTO)B5-?n-4(w5$j4!+ z*axGRFfJcxNWP3d`mpmrCaL0m|AGX%AeYA+wE5d{0Lc%oq@ny0rHkR76H$~W?Ixjq zR;y51?>GR3-y(0p+}9*$0C*(5kcHj%;Q!X0*X?7tQ$i$GzFH$>0*93)$LIFFMaBf# zI8D)P&f${#kxPCW8PK4sq%&f{cOsG_z^kxg-NI#*R=WhWz}t0s6ZY`#gCp7%0Cjtm zgS3I(IZNLR{_!!ghdpXpyNShz5WxN|2}Yp`pidcpIyLIM7h7yz#cAzc;#kVL8FYw8 zsj4GjHFbaab-5GADERS%N{6KJr+K=Di{1`IxYiGpZ@pN;>JJf$@_N;!G|`eh8b z;A*J8(%ZyR6Va0enz%<`pv4hL$ zqBJ98lOO^=Vr3#&!GmZxpae}6e6L@=$Qu2VFH8#w*?AA9;sq~d@FCz`+RFM!Dg08B9p&iiSt6fm=%n1`i7@FN z(u&kB!%A#bD1}KiC0h-FP5Lm;OD<8t@17400ktl16*kw(1C6X+BpTdZ9S*)$Jhxmq zY`HS5aUZ7+KTZ)Yj2?PjoP2%B$3&hECxpmexeL||#|bcL61g%bI%2UN2O4|8c&A_c zp!78w`QUJZJ2@EAiuzth3p+poCk5-OKRmqdMIDx-F%@3EM&2%My9CIQYjuaYZGt_p zj)gc^u_ml}PCN9_F%4!_0ff4M#J$m7L0=dH#9{I=Sc9XWLVIAT&?Y?n?bJfvY}{42 z!(d1NDVgT85|6X&^#l5|!zy`2TYg*HwjsoA$}_;_=fTf}r|qLNoQs6M-q1r-3l}|0 z8)1=LN6fevW(ZKl0XTW;xV^z}GW&Man&#q}`yId}U-I~3GH#}gCLAN2Z&UvLpASW3 zaO%qDp?2DPesV_)CQfTL+HEJ+jwxe<@_UtAYjvx6Y@fL%2#I*(g=VZ&MIvi$)3BeD z+WvgujqfG0Un?S-_n7V6Mn5%Ll)8r7y?s=pQ?kSbqPJ?nOG6vcUMk^?>(M~m=nzD2 z6R6KJ6HQ9(@7}^vy^qQ~JArqRbTK%=a+{EVv>PKewx&+CVJkA)h`GTFiRyJR^GhyA z%!!f^%o(l&ISTvSIN;OHwod}2778C{m{-`uO=fy058gpMc;mVxzujGRm%qUv21bKeq`12p? zOOzLj#>(aNk+}tU?)d_6R|?{P)jB5io!)LZ@aHtJ+wy?v!DgS2F6rIl*Ot)Dr6wGf z=FE%7XS(6yV~V#54<`9oLE$O`^L6rm$%fVTo$q17hVmsAeW7AMO1P|?42P#QI2TOa z%%5MN(}A=>of)f#;{BOvr)>bjGu*pks+V+Q+mf5A8|p0ea{jlU_&7^Lk{I57){{c- z*p@T_X{UJiRChM1{uS~kLP-NHWm@zv3co!FDn&zW^+kvcvql3_+gW7Mj}I~Ahs!Ty z8;jQT3S4hX!5f>+FW@~}^H}ixHm`EpTe<_-_{`NfJ|h07HbDCbd$V9t#!31+)%0Q3 z*B3j>{9)4dIP@k$h&g?14{8Jd`$KtxCy9DkX+-1zX})lU`Kud@SToC;6%&5FAI@kV z0&gXy>Rg(>>Yu(EeIuBwe|812Q&6z;_2-3T^bpRs_JmG^`_$tCtvJ}Ro?^a+8f-f+ zXRR565u9T&kTT<}1tD3;{tN)Gubg37P0PRKQV88mwuxcc;Dd?)y*b~L=3oAfeyX18 ziam)s(!5CKQW>O&{A5C9YVisKSn-h~nSW4@cbb1eqoM#*Smb|gBwECvJpJ>ys|dNf z;yS!w%h$t^7ujvca(gAJ&l#b2SI4VZiC=>;poSat?xSV+%(wyqZ)>C26F|*%;LAP{ zpcTNn3Lr0ZB|72U z2lM;)9-zby*fH}m8pCs&#P6N}zfzZ4{c(;sWBj;+%L^I~=c6GN_IGgIaAIQwJJC1# zpXuo-D6>;NJ0r^pvs9I!u{-u!kYN3Krfs-kKfZDAkC@_}3-fl-;2DXyJCggD&XJut z@?%{8K4exXG5|XpD`*zmseO^t08SUemI{vi`I1C6YOtd3Y*T7AWxF7!elWJcwy~M) z+=a(-y&T6-%U7<1+kCXj@4-nJHNYI72d^>^+XhrNpEpM>Lxrt$rb1bbLdzlno2sI$ z2_B%mw=L7dv7!@bzKy6Hb3=|-d7!kZ2gSH@Q2+ky=R;AjmDSSn@``im24e6KpYP=l znXcQbA~F9vdsNm-HW;juXebcly*$(c(`r7z7fZF>%gfdU9aKo)Y_4M8!!g2SpVg)h zLd`AXidAG)zMB1<>98$3fwO&&ak9ph|FCB9Y4KdhD$5_G6QNFjLu#PlQ6!A3FF0U} z-K^I-gWaTYLXk)9e7N>4!}Om^m(OEjPO~}vpZVCPWJf+aqCdl2iOLLY_71B}76Q*rKAhuemb3lBfh=L+O@5^zCIBp1YW&rc3+7X6I^ z6%!FU6*^lGlTwfxfU>jOuLe^zMZS({Jn`{j9GCi>T26C24K3Nt_?_KP*NJVcHDqv@ zt;Z2@1mi<~D&l(n z#gP3^r@~tTS#a?9gJ1*WSE>!G{jn6r8Z$>d*Y8Twg^g1SvX)g~GCtr}B~di<00#$9 zitebKc@`@ZRD0kPTX;(%(;%k{aIYO)r$woL$bFQm%7TQSg7km@vx{nQf$yp1GZw5B zvT)F%qm=y%c$$&8Rmxq?uHC8JIB$*n^fcE1)*${$p%2B)@~Zwwi>K{C%+25+ROD_C z#~Smox;~O#N@c*)`%8xHRA&_Xa@BSp%2}l?a8tG`ArhSC4TCh6VKR6c+7p9>YV-C6 zeG}gebN|+~My<3-sgc}?;d}DR{QJvsCH$bsz`#I^x5u4lntQcDs(5b?sQi9GxOziHc5jdjew<=or6OX^HPP;g0EoB#*adas0e^Dp%gs?sG5O=8TK#gn%4zX zzTs;w)i$3lwjFGlqrrbZcBr!IXdKqI)0 z_Xw&gi+Z-+U10%B4N1Ec;T)2k^*r_+1f+ls-B^MCn(qflk$`vpgWL!Y_KSrFGAMci z*21Wyz90t@Si!4;MD~RBeDld+cW0!MKz8fB_d*bz3a6I?aY~}D3!+y#{{*Odj&G9gj+C)LU%ghFYd2z6pxEm@ zObZ}x0*9^05mmP~@P3*Q^DEa0x0^dA`YIyK?4`fltHlVfP$VzWXPrWzp!S^2tZgRW z#fm$MT_GOToJpYK&y)zhUi0Mb(v0H@<1!BCR9p6TlrpuNqz*FsE+-`TY)1`(CY2vT zkfH^z&(}%n$Hz`>UkZs6wZEI>GamZ&@ZpmvsTvg)uE_C7CSQ>Xbo`a%C{7a_-M>K+ z_4UH+*}9In;v;KMGduf~tD8#e)$#~CPI_Df*#}|{+c^PnNV?F@#0!Ufg-kE8OkO8{v!Q|T!l=|QI9M#v zqySG4Gc~zCkd>(t{&jpNjNf3mB)jSFeCz-=mAK@^MzI36jLYWb;|FYGgGYSu+g2d5 zWJld|0S$m9l4YT zSN#W~fMp_6#eNhM)d~6K06ZmSAYfNBJTN#ZO>@}l4Z-WM@w?bGZ4D94&__k_Rz->J z4m}ud&%z&7w|YQ>7-)l}(4*4EBj7K}#&Kso74H&b!xHFgmU}b^No&*kZ?;N+l*~wY z+4$*L*kgE!>HzVUDI$WPMkn#|lV(9!p z;gWQ@@|07=RpL9%Vkg``Xn9x~qT0CFLp!u0Wm-kHL(k`%l#2OUf|@*^3l-15=chYm z>ent`^}X(D98?J`0YGL(x7=E!zFz4ct>(oJs^x67rxftf=poCdnITejyydO26+j|* zmSY4#3jg1_~g2OxPXHpk6b!9YobGTZ}8~^!0bJ`8M+J z2SG-0rL)a{3GuSnzVYsO7)t?OTEecIws%jOjV?;N@UaaCL_}ts{f>n8ypj`5HrSHC zjt8S~p3v^Hk@N{^t2H;LtD6}9{cbr=CnSyh&z(<5zyW#K`yj6+)WguQD-BJ6&fK`& z2{dbLHk*y$;E(2xbcJFrq$Nd_bp10xxJ@Jng?7ufnHeOrv91%#L>VSc@l2o^Uh z8d`Y&HJwRi@rR(ecr#w0A*mj3YDe=F@?sAhB>@+oU^+kaw6;=30ctFxU0#2Nib^S} zwm7&znx+Jg2_oZTDH}I5P|KwEp{;A#C zWPj-i!bsU3U;&(K_s~T<5CF?*bvtc5bk`G)wY4>JY1$=dc6WCNdHzaySOGW!mNfo- zfj8Qjbx2!%w}5g-xo|6C4FlXn(~1b8b6Efj8YzL78WwC-&?F0P+PHBvJVV0*o>DET z9_~>Ks~*1jm@U&M2;*eB;Ae7#Ef-*YZ^^e@ST_IoGavU{dUbWhu3o+Bq6Fm0<45-R z$wSi1$8`96OQ|k~WTZojSj#oqJ-Q)DyRop1R_4;B{8YsyX^;!I>SF|x1~{amGTQ)5 zdJHgSWU+=zU};&vGG#YuoLm5AtA+9~16Xb30pYUOAKHyaQ@imPc{CxloTFv!q|n;} zI4{?+O!`wl{HJoQUupIOA9xe_fd4s!rJHt_uvE57G|Fk!@`2R^{yxB$+A|Zd9G?qy zb9)Iq@pT$P5bKpqv!z@B)>;n0=j*qMmK_+_IU3H?kZ0o{>@AJ#JwV@kh%gREGeRkv zW*c?E8a8EH?zqtKr#}9t0Z#GGtM=xbZ`zx0ylKj(Z{JcJzUgt;30H?_arv#y7E zuf6W;`SzWVbV$Qq%o7we)?CAVG+Y34LnV>vFFH8#w} zDQ%cQ5)T1?`fvPT-sa}ChOb}0Zr83~vu>yBn!S7Xw%xmT*FpIvBXL)nMWP&NG+gG) zZ8o+qquoo$8o+HFp_WRELmz@|#FCLiiB1Y$;Hjm6Iivor|LC6pTR<0DR+?GCX$dxW zkpojgG)J46{g2xN`^{T@m(f_cbXdpabKGasg}7#wAOHCGl6WM<;I_T?+G{R~#rN;u zMefnPyw9DSziK%S%LZ8DfLRMUvu>~>4Ywu^yxyr=x7)!|I@Sib9uFuD31`vJFC7BRyI_1J@w`@mC2u#{s`DFxT}mvjpb1x`wHKa)8UeSw;x@&09VD+6@8s+-Xg< zhLIX)L1GTVB%J)gKCSYx@Bd@@e4f!zOWZ@tORGn%MB~YmN481lBY4eVYRa&Z>X76k zgr%iP1H6#}E9ur~6|LDij&P;hqzQ1wB%RT|?fJw7Tr>-)potvs3tZOJzJ`GhCe2!u zNweirT8fGVX;;7+ha)urg2 zQ zmcUo8Ua^ifPa1!;& ztublVv0qHy56M_c8Nh7j6Rrdoa_JJ*Lmuz(fqzcJrvRqi;u;Q?p<+R>UwIc`WH1ad zctQda)yM~uwu?et<@>(#8}rG8by)xlS{c{T(IGkNk;|MZ)V75ZkIJHF)%h8tOq8-t zPK6uN8omr{0Im?r)$;A)@i`q(ACPL|wD8oP&=S)%P63^w*(uNgSPm*^HsYE=!?LUZ zOsRoM$fN_XOhuN5&$6$+dum^K=alh`3=Hk!smBvq22>`^q{Wg=xgL4>_k8Q`&L`uM z*QcI%kbI~v?hpDdItk#d5@0n}=Bz^j&rk89YS{!0cN()-fR&p|x?QE1gd`*(9y4PW2mY;c8(ha#ua~kJzYvj=D zor;Gf0n1AZw%$M-xTxEt;R05pS43QE2@Hs!9auLfSpri};-#VWe+B%`{mlONy&$J& zBXa+tOFrlR@HkZxkj0$e^PS)PnwzxNsCrtGHpN4zXq4r!cffipW1g>J(;CLKWzyV; z%&H^c_GoF+io+5I2DVWjp=oLO{eZcKJEkys((Ef5egzm<*D@IkzgI6gty`N_&RJG{ zJUZ6CvEv_bHEZm3Ib>+MMz#HsrViN#)ABuIP(P2yA zB99ih*TTRifCmjLAyK9^vny!WfeCpkF;!;^xY0h;2S8I)RQ~uM{$1Xt z<=RWL6__fW5?X)PKl(SWhD=Mzhcc~jlLF4{N@ol-8g^iEX@FatE=vIp&|_d_(#p%J zR{<4lM$1P)S{f2*Sg;PP8{@P8CouOUlfJW^*lpwv(ER}w6UP#V!=Y<<+{a@N$hmQ3 zl|T7y-;n!BRjylZK*<|BEPGBG1zf&j_J2Lo0=z69WOm&q8Wu3J&T$ICstQx_kfxM| zuH^vi69{-lK_FO-T8||#z7z|3A7GVM((Xl*=H3K+dn>V9TQShIa!28KhIq!9^r(mQ zNj0gWDj)go-|M+FR9;^y>r&Ug&Q5imZl=UoMr!^#U(TRXF`dp`+4w#N86~}xfxDTuR)gSowe8#ErDO-z6U^J}l z$%y<~V;Pb(CN<7t6~HuX3SCg z;$Z1`f|hlf7EOndKl|S~jl;G{Q$qJbJ?BX6SZTOb02dlATfTnei1nK`>3Se?eC2D- z`Yk1BFnogh0-7X*QUAEZ61bZ*z^>sU)8Z>K>31Jj5VQB5=+LQv5!$%Zk$^)SuIjAi zFMr`PULIx{<~#}~X@@k&&id>&;Mz5od#x(Dn%Wqvb6N^T}0z zo(mXhU|X`x@E;!7fPt{2(SZwX5{wF5knz}rg0PO=Y0(3AV6Mp z0=V33)#q-~>=BhFEiWynwaOa4x&m;i;R|cH)f{kQpwMus7RGa6?JWhz6h?Ph{6qm< zIxW{QJzzm=9qYxMsR8gm?~IqCT)I!My2nZRsmz&UiZjl2VNO>0)h~T69}IegBoz>t z5~5@OFZMXWv;Wdw?(c=!50@uY;GLCTl5NRLmto1B7Qok#jV|RPTQ{wiVliHt)Y6j= ziU@z5bwSG{FYf$k@qGQ(z`k~KK*xh{L>vx% zX%#*y8rUCZf_(Sz7k=)?@{<#C&XLkTj`Fixpc;4YU=P{1y~Bg0CzeO9P%YW))TJyB z1-%4brE>|e8i7hLm0G4~xakE1z=Flwdn{f8%d~M%T*q@~H)UxTkyXz}D{-4vpLuNDH{h7Bv zMhH~9RY&|d84s}j{G5$6mlr$!<=`Kb=Z2lbNVzm;uymv9L3I^}S7xl^%r37GrdC$H zb1C370T@wl+Rd=kFu#}L8emLXKn3uejq4d(Bs1K%Dk0pOCqJQM-{pQfm}t|6G$%$p zqM)Z+-gz*^VNG0A9~Y~Jk%phnS^})=R(|4BKg9lU6Hl6Y!Js2cD$|~Bb7bim@_d_h zp!QB^^CX0&Mj>82aHsOH((u(4rI)mNR%2iV9J-dxxW_h2&wJAsH7r<6W}aM%=tn7 zj??0aDrM0bRoWeu40hX68V{wyx;FP9&3n_tRb85s3R?wGLV&9M} zfz#-WWCdXQ)0O2K*1Cm8B4{^1U5TiJCZ_aoCF)lRR+H|)(HaU_V-iw7N~I%J`v*rH zc`&<+Xq;v`FCApTo|4utcg+a!XMgO^<|&7+bjG9H;*{gq!IXE{`QLfDyG%K?d6fd( z!9v)7b1$v@S}y@#=}<9sXAUgbwN=`*GHC(VP7xg5($@IW1z_dtMVZXa>4-Ux&f>IW z(r#g{VQrtT^X$Vq?>9NY6AsJ?_}*6T!1p-kF{QJy+#mK`LmuH%^Io~z-~aKC`g2P< z%P3$ScaVnNLl>S}$%xmMj4gaXm)*gYw)#)H|41AZSgVSQGWfjNORamN>Sql-ctVh?^n3uq4S_zMCQX=4Lf-gxhL*j7xSfEbgRih2U zG#bWC_sA6lEnuDEk#;ru34r2SnI|0_DONnz8vd96;79U|`bmXC`4U8b6duwVFqS6( z`atX0VW&lh1JOxY&AxipO&)GuPbJzejXVZ-iNhT_9vvF-9X8ZDY^H@`BOG`MCXI*T zXt8z*wCy>jE3wedf5Q1@pmQdsbeKArmd6|9B$xbAfW18(dL>`jJG95o7xw6R&PbgN z>|944aI~`$*dM25vVZaOe=qM3*q+c6Xr5Z~1e%-_`%P0OVM--cK1`vJhfDlmX`92P zdPJv8M|E&?g}&A*(oH$n;aL_nahljF+oY?sXjeGv;SEBZW227Aa{khEwi@6Oo7(;U z*k!IV4&W+3Qv&z%rD@-`2JgfbtNj~C{rr>7nQ{H&!OndMAI6VWrrnjDFq4enA{NZS6KLO2oB zWM0V9YIn;0 zYg}XlvK*=&&TGvmDa(+h|FJ=v_}r$jD1o&F>%f8qdQD{|O@WYS^R(Df!kMxJgnHW|9Nqc*H^P(8hgo@BSecF>Hq*jBB6ti z8Jf>HF3+(z?q}+9csMnlndbauj_)Zwk#ZO&3uiTFmqMP}HT>n@_@Xxe)SKpW{~;YO zSRt%**%0fpA*RQ8N;y8*wUfggJ4QzAKZS!OW0EFfH0_@zkMYo`H1)Iu-C+%{`5GRx zjyc1nlL0C$5Set&!_)$nc8@vEuOt2YCo|hSF-AcEOY~A3hru|H=0b1X*zd#g7>|{# zoQpt9(UC&|t6K8J<14@ZMM9}E8Erm=PM^$lolSAc8v9JUI(o>%moIi-SpV!0?H=0@ zEsu%IWiPC{;9xkg4FH-vCZLmj>Rifcb?AY0qcNU3^VqETtjC5HEQ2|xbLlB^qON#S z^nfG5CsW%$TOe~Zrbr;u1PI6pN*N<`Zm^0VJ;M78)6Wp4qSBoI!>H^`rv-FG!cF?C z|K&^GKU0gx137U`Vdx55rk5q0u6@2k*7f|kkJE9Vbv&X^6kyAgvjT7(06mV^O2Q|IiLjM|MCj`Jl&h%rxN$9xghR zL&w$Vxbip;g7%qnswI7{HV#=Cqh6(kb!x_sF8RnbeDy#7>s;p^^?yv*+|+XpPi+rq z9dNmR{dG?DT;~+Pv*K}pXV{m{O=!!7ndeT_{L}HF3KqgMVJ%b$| z?u!@uoZQ{FBeptENJ37Lp{I#K!~E6Q`ZW~q8V=zH4y|u`R{p=B(Q$%8!#=8prU>E4 d5IiF;N$`UAd82aiwDF383NJD#LCRf%Eivc4pu@E@&5pWAP0jIg8(z55(ASU zBeNjm|04|YKzFi&odL?6mQqXwbzED#l4gO`Kd};u4Zls%q*Qnp!5NX66=_R?aT2 zZtfnQUcn)uVc`*xQOPN(Y3Ui6S;Zx#W#tu>Rn0A}ZS5VMU6UqHnL2IyjG40*Enc#8 z+42=DS8dw7W$U)>J9h3mboj{8W5-XNJay^vm8;jT-?(|};iJb-o<4j2;^nK4pFV&2 z`tAFVpT9u3cne5k^_L*fUreAlU=!0ld(M2vX6_bamA3bYzViR4wRiisgb(b8_{A%h{dWA($=WjeVMm1Pir4&kv3suaZ|D@*%yD`S z^KA=@_2NB~3MW)P+f-*D{o}2!UQU?*)H{|vGW`=ap0iH${G&ZvmeZeGRXly+i>F!q zvZA(I|MXfsNjP9GX`#%v$1iwm#pG5GehWoc?x#>}3MfF1@meibkK=ma9G!wgs}^w1U|^7I;6JqX{_O*G qoaYM?oK>QZ&6e@sk}CN2o%agcX3-$`qQ$nXB4M?@s+;Wp-vj^=+&u6A literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..5df96d68f3f46eebed0a107b40165ffd146749c4 GIT binary patch literal 1633 zcmbV}do1PB07;vu3JdCHfbtMjKR1yZ6aD0E4A)P z%DA(J9l4YhDQQs5NKLV3Bt~R*M(6DQvw!UF_dU<|yzhCR_dVawdBh{)2|&@o*4`EX zfdBv`S%7#HI0%42Qb`>w6%-i8}#xNlw^$JW1GZ0)Zh=7#tyq zAt!05Q)f=o;fqOwG*q9ke-QYj@b*!PV`!yN9Qjcfjd0fkD9`p%*VjL`G3A zM<*sFr=(tCT+Ph7`D1p@Pq}%&6cv}0mX%lBtzp*I)i*RYHUGx$eALz5)BE_@(DN6= zFGpT+Cb_(+x6|+5^FIjZKP`M-ToNsRk#d0m$TuvB|05Sl!UdMj03qc9fuke^K|x^} zhHxb-XM{gld8bi=jEeP*I}baMnm8AM>ZuEZvS_UFq?S;Mwodl%!07*r>@Tptxj2A4 z1SA|YQ}tJJw5XCT25L7Z85fwgbDR<^F`N)gG`=suwROX3 zl7ru17B1U-%!e!nPAe9@J%)J3s(&}DE*M%oFVZOwWudC)468a!a?99f!lwyOu}Z2v zSb(L|yNhK+bzbpa)p>?DizwLW^n9jvn!M}QOLXcxz4%UaKqCSgecmqMFGn zJoQIT*Eoe~_{D=Vf_@Hd6;4r~dI^GUYII`vk9? zm-xMb%h4x&{DLNzdEzl*aQao28ZpdwL~nAfk~=z@u1qO(6%~I8%n2{!_tf;mY8moK z6_ytAxMb+ny{(hk$I?@H0uTvsxdyC>0d0D~a^osg;E}R02Ns!5^waasrJb!G zj>353nd4*Sn{Z=C-lk-GbWLkRuoJSPH4SbbMt;JjkmYi8zH70qxqfD*&2i4AA#Aes zAa9?VzN(&%d)|_NOk_)z{j0rsgkLf4EOQz_wH|6ypt05ER>O+MkKnK83bSMA{mUU5 zD5g;+Zd5Zt6j|Ly)ewevyv9FBJRvA8&Z*BaVc=T_%m^=-2_0sdjTVW+tlg~&cSWr3 zLWNcR6%GEr`@!tC{c)RD$r5TS8eRiLnU(=yP{lJ}x-4hAE zTjp-)9O@vv>49_j=r$DYLnvJs36H13Cc=>vo^2zu`MM9ERBABW7-;Uf_|T7{K&LD` zt&6ei&ZS}mdhA?KmQZ=)Y;V+!gz$D1i`&cr02EmZad9OX zadEJcqn)XRwFv-7hbN~Xs^J|F3|@?m-|LL9fWan5@~zCMNX*&r*b=R!m2!65D(cqK zY%ti8l5k03A?5m{C{Hk};p2=-iMwS?7Lgs+K@ww3`u-}T zF8^3@?mpa~woPvU+JlAC&kp?jA~-RO)xYUtu6j3?K(mtoI>gH&m#FKJ&JuFSNlyA{ z9dYH67dtVyAzss+-gIlnWz<>Tb?fAl&$iE@SaXtSQKDc|**z9)W!t+)@+~dRiL9&f z)oJD>m5#3D_3peZ(I9N*8TRThstu(mK*W7>6a;GaN45V&W_>9@81)P0^CdQ-Q+F2o zw^6kEUiUP=>~$7o4F7fuc>|2&hnZu7(}VfC zXWcm0dLlD(TOoMw^c_Tmu@}F#du-A}RtTOb5tnTuYb;PJ{%QUHW4} zj4=&q983`rqxe0w%TVkPdl764^p?6Q4rGky5z{V?{0=r*TwPgb7H=2USXVc3H$== zsg!r%5GakFc2p zFp1CQFRS>J#n0l;QXl(TlHQ@Xgz<|e79UNWT7Z7KFMDb+R>ixfxTf9*D15h?wmG~x za=3N5Re7X(^mqiGKLqx(88@@_5FbU2MUhAe>=5k0Cl1e=T&Ek1a_*a&lHYLN2;W5G zeH)RG1>-X|d@0v1|5~niT*j%c^=Vngv+h>*2~P_P5fT)RpkX@%@t3nV!^GDxxaN%REam{C1n!22oURbP{s$$PC`%YY%Ubs}`p%_`xKNB&(Ts>FD zRZ>-bR+wG2mgn;=w7hpFd~T_1ruarnLy*=ANClJ~dim+Q*%ug?Q$MhrV>I|w@< zT=>KmdOrFO^osHg^1Yv%GF;PrIqN^lajw|84rZC0o8%a+o4%~}Oo|yR+d7OTSRC25 z%>S6XUa~G+Qm%g(?X>7`ysld|pR>60w;IEDB5f(0U!KRF$De-|IG2gMS zpWkmD`}VEcCgaRtMdUQ<4CnOtOltpdEIgy$Z#=J~q+`;zrlZe)2W#uHBd|}oSNTwL z?+>)5KXzB+CNvu@Ydaf*SIC9ii9mqT#rMqcsBh*?=Re^LwYZi|Qka zo%K!OeS^Kak;1ja!=tm7xq7`kCM@J*I3wM8-HMKJcaC;K_f!v&G%H>W!KN|9yQ}M^ zeJ8h^@%CMh51pIuFW2_HCKM;^S~ezbWNx1KQz!fm?ap)0HX5KE`ojT_@HH@5un{ol zaL0%xh^`1Sh{1?+@EsVg=qwm1L1`hC7%h0^WF&;kBvKftC>0o_dWVhngFX576_@Ro zq>;A-CLdaBH9Z`*w{hi)Qjt>0!ogB-pJqR`6F#C;1ntPd%e_yD3D=eokc_4I8^1#H zsqnNgqA;ZJtT3|hli{gRV}H~D|3FHgaJW3t~OuSN>l-`Sb=K*?H?2!xQl0 zote{)0cl#nshKqF?uj2>^1&&W2VIQimXtLpOq}DvT*!^`Q+H@`^a+yON>Fh zM{M^o@MEm;uqmCHj;=wo;Y`F<^7(r|&EP7Ws^ii5A>CY|+GUHob*`a|Vk`ay%c?OQ zfuG|>I%l5Ck3n}`yJ>rP2kom^?fD-xxZHHkH+d*FICo|C!fzgVmTfBk5j=69ubj)L zWV+fsrctK*)pb|q6>a+DmtJnW?QbOe^0e@II=NvtnNJW*r|f>%9`jGNL_p4y@ znAx`**YW#*XRL0_OAh(Qt}=TH%j37%3DYgbRc$}rY?kginHx_%Tbw*pgJt_mA{N?c z+#b%hE-Wh5s+dc?AAD9Hcpf}YZK|iMn3g^*r`BC;z2rUXh+O)XR(z>uDSc5&)TSxB zs}WK))6vkVQPG=)au@MLQ6Py-I!)f`TNOI&tkVaV1c=+ed!8($r15QcSbVZSz+R6Z z$aVHD=N%Ew*ofk+F{iK!w~7B5(s)mC!#}@dy5M?imY8c5V@1X)LgiO*NPA{{K0K`Y zUNO4q`%jUhup^>V?P-q#=bbb!yA3(bTgzt2?qd$)550F;Q2uQ+_h%v;eojqLb^^@gTJso%y`= zoRAsM^U0QO-P2WI=e{r7#Vy?J=3;pDeM^mF*Re*RUM8*fa5!ke|2HOqU^B-%67PTC zj_oc&9VtIjrl3_|E<|fakx{x(o_YMdoSNK88pKt+_Dy~6*e-jS$$i;;b?{&|wmwY% zv$T8DLnzB?sXOkw#COi)`>H-&5x(%(E@^$XbFbsflklIdJN1pwU!NnyKujCT4w3cj z&ly>tu_J9=X|;m>e0^fwH!;R?8| zS~<+`zz%Jnhk5~&N^-v^UcMJgebf59P|&nFU1aQdEy)Hf0h{8lySoy=;m7R}|kCp8s>&K5D-Yh;idDuD|xPP#9qWM2g{$D>5CQe3< z7WU2-cDCUE_%$%Jb8!};rv6W$|0)0Hdz!dg{NG5nPXFDkw+6EQr-qfCg^l%pe7`{j z|D)wsvT!%C{wiT%^VVl?F@)LqJ_`Pq{r|7#|3>^@NX`F?_FqN+?fUMG{=?sh)<-k7m{5KT`mSv_HMzY!pgnvh7><24<=C)xZCpFI$)2?_L7xx#T4ufHm?v1HnKH*b zVO2>tzgm`+$@QVmRQCt}$yTzarA$u7p4SV#7wt}iO3u}aXDlgRYUl67zK=UESCV=y zb}CHo>Vh^rif}eww>B(b^--bZQ9L#}mP;WGYGqmtBtw#3jy!Ze8GJ8i$!sb5RSZTn_vF(OoRif1R|Kpx&Kx%Jx1=31qzT-icThEa=A5)Ys0HGn=NBuk|s) zg)h;qU%#*YaQ$vXUYejbs$%<7(Yi|x1WsOAG>u+k9_5xZ@2|L>E$J5E4xo? zjdz|x0{r|xELB20gh`fZ%El1(^YY&g=1B}yvkLrW*^R0K$1YhO{-vqPAJ7`2@LY-h zIkq@wOf+%l7;{q`aA%r8ahipuf88WqY0+7K^82lT97Hq(0+$(w$P$c7-9@wyE(CAJ zT>-nWVV{tZplD~6nTW@`uHJZnG|Zjs7?Za-aoxrIH&-u=UJh43lU@VMUS*W9j;b&2 zAItF7%IES!YPPA*bq9p{+96~gNxEXe6ipkVw9-`4VA~)$uJX9F1s1`hbfGi4``aO4 z(gm4}lJZ-j)?j=}cH>F3JU#CBDRYD8U-Jmh z4Hc^9R~Df220TeJ#||U&vU!ocN7oS^mV5Wni-mt=6`^&l+Qmz!BB{=@J#Np$)y=B@ zC}}OXKv<3x9saA!ZR2* z{!Bj@1M;$Qmn)+1vK#x&W5&mKP4|}Dzm?RAhYyv6%yg2wV$u8CkH6K{sJ8+{S0e;B z(b1n;qO$MKaMg z+yHHEytbKnR1}L>7^G$=>$v8RPiwiz6eD0$ZoP*7CL6I1i5Lp5-f|5epC~b{m%$+F zeewpwqrl`fXcHg@`C{NK)~y(5z(YQ7XWO?i0s;PrR+FOdhTR1bkaVV~XnBKnMP5R? zl%(?v4yuB)o2ceZt8R%<$CSd`hUd=*NHe#eTyWZcYZuQPw_E);r zs|^H##2V?>HmH=daj&IYiM-?np z1x%|9?dgJNG&g&VZE7w|?-6i_x)}YkL>Vm;&-L98Y^6-;sEq!+69Q@t$)tIQc11t3 zAGM(YX#_U~=$8C?sn(6Zl&zITh$h0dC_#W&e*2(iM9Ux2PR;9^*ZV~6&tLL9J#3?K z+9t|(=ylz88ampuS1MM5qY3P_(+G5@M%a}1HRMVI#e#7>f{3c}#7y?^DyQx&MW_)9#l|-Cn+*Z{-0Z6aqgKg5J5*|tT3l*17|?{~ z_tlxn6$PuKq^UBjn~n2J=p7x6@L?V#?GcAbJ>!|>DqpYqoGPC zw(M`GT$m?~ezbn+ye!7Pslc{Six%S2a~0A6hmvfmu0$n#i3Pz2VEn<9HCEeY zxv!RHevcI&64w3DAOaUSNTk?nrE1*68!x0H{*H!Od!~kwV~Y#;zPg>hV<$X`9UbT2 z@9&^a&^~xCH_LfB*X2swfE_?H`5z~#ykZDH+d6_p<_s@P++?z+)Z3cut|)jXBBCeu z{^_6P4H9I1D3Q^cVbFCoW)Z6je|nv^>#>Q~;UAb&FV?1!4 zm3-nl5*8k%?_V41U1!GmZ-LMjYKQZ`TAJ$Ugn3@fn1c;J;b$Tr> zzy9g(d3j~~)db>sL;|zbXqFBP5azkS1Yi5-SW6SBu{Wojj6p-|{eVH8P9i!u}b zhXfkXA62!(gVM=VmHfPa7?36XfTo+doxHpRQJ9*Sx)tcDKFp4h`H!4sZ+{5_(pWbN zz@CGRVeQ~Fd?}G*ex_WA?|6w=W&#;vh%as>FA3_o^u%{K3hFJSb8igNWTGOIu}4^G z*)B^TGjpP?trPKzr0Qb#n){?}+x|-$hgh&lNoDTj`*Kck2zLs#eT-<}Q^DA* z=?xdpdM(aH?8i2A1nii<0Z-s-;%!O)F(gri#Ea$OptZ+78TGFLJX!a ziln*LRfb>qd&}6;TmM6duOI65>C|_^^Gt!);SNNkS+)LysR%#!l(a~G1Qe&pu5vCw zIfmRns9>j@|LUaS;>m*y^c5}6E#+G)S=~akT$JcHV7kv^LEAX?fOG&arIUU6w#WE* zxl+GE45{lZy_3~LjVf*M`!|sv|o=XY}$FAN{ zC#PJME2mnBrkSVCm8dee4W@{0%XeG`2SPI4<}*tp(^n(+lg?Pc?Gp#$Wq+Qj9$JMf z2PV*ii~;n-lPKFX@QxK`9M39Vr!GQ;0d}i{!G!KFQfSbguqI7gSr@7g23b}7EguCi z@0MLo5{6yulhJSW7(7FS7~#qqVunLweeNWvj0GBV;&?D08o{9y*dl_0QF-iJBLnL( z754~tN30@?35Gv0{7p!wwc@iK2Gqdtc``4@lmO7X-3t#vVAirnU`}Pq#}cmi$bfH< zdV<&P*+Ec^M{RXND{zu@kLO`PmYRmGVV0!MbjEQQUeo#1|COVk3e5qb*Vhp9^xyMZ z_xZ(>tBBUpb32+k?^(Y-(wpfZJ!_hU-qi zb#GYa-(!k(j>)I7+O$!taxl($ct@%DvGn$u)W=b(1 z?I>B`;LCB&lPq0#`uF{S_bf|KPbfjv8)Ikp_v;pLPboC5|i_z~)3)jx(~p%wTr^FI42`LDx;XON^qRjBM15JZ#l@*u%hP7)O}-Ed=I$8X*Av%x7LwFQgut-Jl4+$szEX ztymQZ=L~b)<=9EXO!3u5arb{jf~-CxY_%OX%Di9zOuPE9j~kGpWkzJH8U3n<-DP#lfss#gY!_2CXfde!>WZN9C>}_r$*7@`96Vy3eoh*_ATew z_vE+=pXr@qJ)K6bsZA^?fh3&=J^gM%TUd86+Sgec7yz;rZ0~2-uE;9{^rdBM{Bjjz z0Z%yH5336JT+M2A+#5w`!b3>c>}(jIVEq2q_x7r2U2Bu&%Z=XIii`-Vfw}S}_rBWo z3xzR0Le?wxIYtB$9g`HBbmMU!pMyUlZWR(HpqoFCcc(1yEuthae7=5w2W-IuPjX0D zG_HEUzz!Y_y>_Qy!%U>A)%L4VL&3m{>(FLvTML(}BTCGRJzZm3iVa`YAO7reG`@#y zUy(8=O_wt+PThhV@9K9u%;l$U{zI^E5HtVPL-dAH7MoEpSq=iT7--v~-nw$p9^ z4MYN9l>DZkZup$|8&{x(R}`jdFsxod)+MQ`GPNe^3)L3YS7BOmio^hg^Y~3z*C5~b zwCUKwdWxj?uFG{iGu856kTWW5F+mi-2qMteE{4!D6}g;JuWxq11^B|{!9y#05a6I| zewr9Q>OnEO?-`vwFm2b6b1g#L=o{E z0AX^AK2zqS!=vY@64|nM!+bKoB{7k`fyumWdkN)IjbTBBtF~)oruXZ<6M}N?sx2d%@w3BDDPeSWply1VM%z)5U$?7*ZF`OqjCdVj?;?oxJSIo1N zXYcZepaWq9lw=S9JBk-LbS`)GUHW;0+H*i*qp8;C^^=KoAAJ#DxjYU#v7RZ=`}uBE zw=3hZJWY1Y89ylXgh?`xzeRGbF{_m+ugsnPp-id(7Z7{*!BpKz7Da_ltR~o;(>;GB z_if(;f*NcEJElTvBHy=Ft}rMyRUB(Je?)0m`6+oiU9!G}gv_g?LiFd6t9Yn}YMvJv z3oYfwOT5=O5VLR@3$>xVemfb|(}BE^?9ks=97FXfVxX#F=IDoi7c$`bjL1r&kR8)U zA%$N01J`>>WN}{FbBOSdeY|1s!@>z<)_t}COk$Q`u~Ujl z>^Ho80b+p5BTOxk*FPkYB3M{JZL*tZ#aMa05Ht}!v6Ft#1@vQ7VU4TM&RGZKy#43JLUzYZcp5vIzRHZ5hp zFuteA%>p)Ga&M5|vG}@+35w9`664~;zVko}L7f)a_Bw`9$8nA)Ar$Dg;tC<*_M75N$O zxyG2lf|E3qd|uaZ0Oo6i*C9QgEklvZPRrO=L|9s?W7YTlR+Tq7RrNHvewV$7e%ZgQ zL(Xw!;-t#C^HjN#c2#eECoaManHjF_WYeZ@ItNEjy)6Qpr1de6>N&N;pQKNkK>Ic0L-Nkb%HK)H#*_jPS`O(WUHs z$abnb&i8UI*}9VU4c6`Ip%AYOPN`ZEL8IuqD~2oxOrFJiEm`=oFHQ%+3;FPXdJzD`ZUvSjLAFPG zM|nWfLY*K;!XNuM84|>|#Toua-LlRSFGBgkdpgiu6eD0^$Lp?DWkf(MoSH!gC+kCL zD2pZyv`s+GT-GxDB~~K=vS=aWZzBOi2ASoka$lp%>$sCi0;rGwA|k_qF(BZ-8-bie zsd$#R=`@M*2-SVwnTuUAmn8w!friOzv`cFuch{Z@*0>`_T5j-ADi=9gFe%hy%5x%= z(DNJz{9*Q-K^kGtLoGkZeUJ`_vjZtHcO|^28DTm;%gpW1F_rQ z&;1M3oz+-(W2NO8;mgA?TQxcIAT@HqK>{(Ei*2SCJ@~-^8Mek$B=fjc`A<)E9DYuL zC}SMU3Z-!Y{GhM_6yU5~TD7Bf+bb40tQCL;C2n`Y=m zB;4ND9>UW@GAnoSgh&0M5)KE78HECjMxp>Vk1(Eop9hjo;h~&N!Q5z$0({VL4nH3u zeW|cr{Z?+#V-OQz!ut^ zIO-!qn%umBsL2pRhaX+f7C9ez=mA{=+T1mm-ItgF(l$RA$+H}Dr%t;?L3G?GWth(2 zeNuFXSWws`!UQT4#10VJYC|1M>=lzg!Og=SPH?! z<@dCwMUPs!3?B-a|NaXP6&Zb?JuDz!H*TU^Hbw_%{X3&kkVjFFgD`Jv*l^MoY(Mdk zxX&qW^HshCzQEysR#`6eBC!s=-@-}sRGIC^*k%!dc~@kQ1hKIE(eYddyEvu3WJl~( zWLu9I1PnoG0GUl10RFXvnAeAQSAeyo#X%}n?A2lpe*<@iPL`>d{DaD~P5W-Pu^#QS z=jJvxAOvO6p*rfC$7~c$*F#)-b0A7C=J~VW=?U4^S$|4Hn7s;g`^SeHq0vZgS_a`hl(^7P;$N#n zGTC@#6L?(HWDP8@`Wn}U5598v(>^^NO5b6D7c|UY8T8$7U?qdTU^wI5;q(0q7^njb zlanWfDSEw?t2Qi=3JtkAho!LGt!qmKyAm|!`YT*YR@Q9M*9x^FNr!HAw7o;$5X?D} zP>5PLSB~U+Wh8|>q3;h;4Rq#m?|zJxYKjvp%3EMi@Ai+HID;THt%J+?JpXtZ^RRQ* z#x_Rg^l_p1z!$XaYH2D&yq^>l^jJoSob-)E4=Jh|4DaYjF-Z6KOH17(1O;cymFo|p zKiZX^rbhlX9F=XzPa;VmA->fURn(#EiJsy4rrKFl{QKdia_?t}agA5ltdfZ3FYl9} zUt(W+A}F$_a}-Ru{<=1Gp#lT?y!|O?oFCGp7Wj*p7A_RCx#2vB&BRgaj%Zi?(;kct z6_YNhqyy=HifxgYKtz7YqNH#dRWVUWvc5YYZfKv+kClzJPBAni;YlN+;>Zum!a9BESOw?#ECUX_kApd{|_|YgL@6y7viyZJl;o(=PVpB^id5K2d zZ9~iq4+|>LzigGbgybBb@{?0|!LXXy@W1&%*{#U#@i;Jxs!RMQVi4{4I4f?kGQm1wiq56d}W#zMl3Ps=}{!22Fl=(DY&fvDbtqDkvzJ!1|FRzeR;8y2~2bp@boK zs$%{P!--IpCnB6f=Tf}f{w2VpDilWvR09@=n7olAm zu%P=^VLu70?MKFf=|&ef4*$CN(%h?0JPvlVfpLAth zn7~Tw_rMAnu}|=uuqjT2haYp|DYShE!$9{ur_rO`;^Js=O%Knb))^_t&-fPT!7>MFwMlX zOxKWvc)fjo|5yVDK#87kWuW?V&BbmDWw_x-2j7LhV@#~LqbBu2{&6#X3oA?h7*93& z1j-i=xapCZVjzlOJ$+1x&rDV4SyuQqUos?DkD-BZ=1dr*up|>>ml-|*nu~nd~N>QEm`4BFi6P@oJ zqOq@0fdSpAr>V`Sz}nFb9IC(K`0T>dkJFm7|O0AAY_G3BF*0WvqqQV zq$H3@Wmb0GFUyxy#$txryOzDwa!&VZ!yAmo zMcGL=f86WFyRkHqiI$Zpr}pQKiq8@Lg#f27M}`MNL!5=i^<-V?Cx3<8zTF-2{c}|8 zb03K6C!vgz|NF!W`K$7eeJ{(%ac}pDbOwz}bt#?6vAaIqF7MifE>C+PQgXy>^RH5- zLj-E^gYBW0wDzv5kjd>A}*;07{8k_+%J76ygxD{ zd`YUPKRhB>m%MZ7Z9sx{4KyuQ;t-BKksg`qvgI26(Gb+TK92n6^>>fFFtwr}4&sJX z!oVgnoKaMhg@5WB2PEua-Qi#;*HBcK8YOBes%7iyJ^%Gayn$gmh0Tj)`9XphBTI%a zOWu(fBV@9;Gt7q!-&?)Sb}5^^RMBckK{}}p9by|I^Q%^8&q6qce>hvI^SLwG==@g@ zJGc^5PX8&0Uu2{$*^L=g<(tX>L?L1!E~tl;sI9)5Ye?%+XzcTR5CA7_E`_#>P7xNU zNg9QFQcBQ}9&ek=_8nQBbfi5?H4R^h!el~!{oCo43wKn0Oj3mF$gkaOId+NXyAvMO zr+>fWeO~&9=np#s#P@JKc@?J4QLgDDa$%1fK;Lo?a)&Kq@p^YzdX)t2<16ia)9eg% zIol8H{eMzWavVPVYf;@n+E^b*TMkQ%Cp1%i2jrQ-I4d3LG1(ix@Q1QEi9Yf5wHBCT znVZzCkDn3h$-j}MFezrBf!7paO@pW*sT6^0*8JOC;?jKWyQG8jFxV8Ig2FDwVIl%8 zckr7m0Ple)$lo6LkY<<91cd#qB4YXFE%Pet5A8!`&SVt8w8)-en;J@|DDZWwudCUh z33m%(A*wm2o8BA@!45(K&+B!eLcJi`^*wHChxOiXNdoMFH!opF_Lz&;K1<%3EyO^=|*TlPf3dU3|8J`2es z5)kn37C5Zjlw`EF?q$37AWt zRHB7F707eK+}!uSt-z$&!`mwsU7-1=oK(z-j-YRJy18=8C3h=w)Uk5;`#<2mU3z-|jD-8)}^%S-nptHUV4bH)GMZQlfkdmDC0uZkXQN zt6C$Sl?&@AUOdk1rB?|=u}*So&|ztm^~y$H;e=>YyW*6U;LS`XW*)-=0Y3V8=lm^2`&3` zWPp3FaM9iAX$!~eFhed|v6$|+j5n;XCVz3Zj0n1Zj`c(!_j2tjZRaPcJ0x&t$pcqN+-JR<154}QbLf11 z2!~aNr>E)R9x@Q1ad;7_Sp)%Mjgv-*B&hCC=6!v=*Q4tTUsp{T`r?j|%fR{QUiwmG zDiFo>g|ZyZTlQN^qR8dV32jfCuS>%^A!<1!wO_I@Ted53xI;KoosQ!cpICu<*luJ)v}bu(Q4}y-KCL|#?I+QX2!1>cwh5RwW?H}c6Ugh zc4@*~8f5>-A3OV!FUjRxT?;m5y|B=RKg2*IjW|SxpR53?1haQeM zG<4WZ?S1-oGgd|a{`IGuHVdCueZHfo@vZD>} zTfUP0^2$cj;kKV$$-)9IZrDjhz#2r$YfXM-X{ljtP3OJFQoK1_^am#91s+Yh_-Lu{ z=z;g77^$v4LJcRRhn}Y2&78~)%nne2S{v+Fu|VZ17DUN5k|b@IWZ8ZRx=DQ8^A0Vo ztV(Tt!<-sT&=vuz68rVU@y(v&62@fQ;VB~N+mG!>4M`}RWs_&=REmv_-4&gypy>gs zQ)IlI%y!5Z!Gmjry@2tWlKboaZrx6@#R?4?QhCrpUn441uPpk&gjAt|J4=nZ?)p(z zQdmvX;gt>!i;Ca|Mu(~Y3yY+Zb+nX`{KNSgZ1;l~Fj*~wLU$qnP2kETKXGlFUs4SgK{rQ(V*dguU=^z!?>&oyDP*9NK9^~F%>^BUKhCA0uHvL^LPr#Kq zdh)ORXNoC^cgoQ6%Fy)6(&6bw%z93C%5tutrs~UYN0hhYOI$*~S1CYtQ9PWLvue)v zTPAA~gmTcSh;6X>OQUgM%I{hFdN^d}e0UXTTmbhDLO3swmMhw79M`&EQ7%u2p2>2O zQbc!7Dj>Ohw-ki=xw1RgX>NMiTCGwU(`vkZ{w$k3Qo*k#b3L!A0LVLiON{!Y=2Ee6 zatLlc91s{ZCp>)a8Q3XC|0L(k*Kp!%=wofbud`&Gf*m!%0T5phLrFyJfXXA_R@@Cq z&e|j*TNDPYZo!?Y)XQJh+OXns^7Aiol$9!c-onXO=a)brZv(N?0Wpq&Y>VUld8Yhhv`bK{3Ai zp968n#_DQqbhLb_87Vmki8i%OSYUD0{PIYO-}GrVdC{A z@5sp~MwWV!G}BH7+^pHt+n*6E4AmFYfARLC zdSy9byY<;H;hL+qTCW9=FwK^TV5zF@(Thc9jt;&f|*F{UmbRq z);`jP*0ZxSzB9v8L+`@5S_y8;_D)Ke!YfB0CnEx#_m|=_Zy6Z(yXF>vSr1J%UKI;Ih&XS<@; zy@ScfrUrC+YW?lIa=k)_<(X9S1zn@q8cq+cjVGtTxqg_c&imX2H%!t^jz3*P(rk6~JU-uehSRhA_Fiy~ zI+XZ(?fjMdDW_paOXvQjSL^R*T-&VWR6U7%zFT-sGJh2yt_KE#&^?oG9l`D(G!3I3 zzU^WWZ&tJ#ts3Xii&%+T1~T4_od>fBEI3RYERrtz7_tc7W>t?KkClha4NTXKC!FT;wz+~22f6AJu!6%>zu+)ZLBU^jUuXR0#fetXe^l70#XE-9~bEgMp@+cSYBSNz9bAWCYN;kS`m zSm8G?g^2EwS!^m)=?fk3=Nn!Q7Ca-KHB1;8l!6&Q?hz?S?eHE3S}{=tcfB!e-`}oa zppkz{2s3$hk2J}>cvtnx0QklsL#o?im2wp+t4>vm@`hAG`%!=!3+owdU_erla$)rC z@Tzk}BCPXt~#Tn04MkANn zx|EEW_H_&&VK(N9o4eM|;{xr|sDc526o|?SCuO%PE47aMM-2WE_|Ah2Fn=%XVSPVE zQy=^Fd$SySjm|gbp-R&~w_o8K63*9kkvqSxPYz=nv%vu$l|vu`^bN@1K6iaxH`T>; z>xS@~m3h%*$G41Djtdpd%5=gwrg)1a!DVDcV}?F=01F-vR0dLhWtm85 zN!S;M5AfdyyX|E2Bw{b7}LMA(po-7j?7j$tDG z7>*RrW-K9_HlrdFp*>x;-M)@yw$A9?F0H>jlm_Dj@MhnYD{HELWuQ9La-gWvbi$u5 z?`6&KX+D`MEk~!^2^Q`{^|>YL?OzFkjbW%+f$ zX>%L7qF2pJ$=1&-wH=uNSW&*7rkh@G_FeBl45z6U5l))rEp{AA!-za1?w|(?gt0OmwL@*-?NGXE5K@XQ*a=DezQg##bs`EvBVGy8)vV!i3kzltf=1rxO1U1wcai(*b~) zYiKKozHLcVZOfNeVRU^$uh%wcuUs0dw0&+FiR@D@J$EENmVYKKCx7P4+g8$~FI~yw zu2@UOJFb+RZQac957FmQesx<%Qbz{79i#Aam`@WKnwc@E+4YzRtHe!=>l=TJ9gkQZ zLyR?OG1a;m2>|7wMUS~4l-+7c%Zg$qmpQE&H|E!Ieck#Ki?!+!bkf9cm5KD8C_GH6 zVyJ3mD`)O53RlV;P5zFsV`y_?>gex_%O!cbo|BysZk!aV6>}8~eD`SyHA6h66a;=C zl9qMU8O)bx)OIfZEe*?MOi+qI`Ypt%;T|J8AZdvIrA2+BEu8AHzFa5jL_wU1EU}WF z3*u58dW(KYIhv9+Qkt7O-SBq0gBqr>f6T@DYc@{W`R<$FEM4BmezQT`BDQ<=?92Xz7oU~=Aoms279mkfw zk3OV}OS=Wx^Hqzwkg}Ew@?p9K5@uu+mIxzBLwgu{9=<%@bS0b_VKulNG$_}g9eNb|hHf(bjvB1p*NWVVW zPUOT2X9yUIbu6y}YKv+sT*g7XG#P67r`Aff8n=|XVj6)#ID0TyNvARy1B|Zzk^MTg znEADpwR~o}5H*#FGWAa|xv#NLR)AtID>K$8FMzKSNdTk;E?U130psmzQLE`cS^;Jd zTY9N9w^^~b2kug2#bS~mL$P!PZFk}0dR znYwssdRC(*+rk-V(QtKzuOooyR;XuVa2pB~csu{Jnzw4l0Dn;a*N>nQ#n*?L07d!6 zUz!nwbISBUVN_Xo)u*dld=F19JN1_ZAA8OsalRjBgk|Kp{R(CpPC1dFhr!)7lpAMrg1?Q^Q&`)dcoRM2IlzV5 zuX9~1>v{dRdapJN9nD!wd+=ji4_jHK*3e4rHEbV)`fvYF0Fo7L>Hx$FN~j*+1%P)c za}bpQAup6)?Ao%p=d>#AIph6d-)^AK&k}od65FF!>KCiEbp!QniF(xtz3VVs!nx() zg3iraOfwwmX!RahJG7V%c4)Ky=YQ2sPft;wvzay9WwN0X#7a{^7VlP%-glcb>p<1= zEu$$$yad1s0IRzI*uwt%rNGS^v{NZ>6tJ=63RGX1jYW+uNr$e?TJ*0Pbt~V7qu^RvvN0ZF z00I}RozFq!%!{}C`}ZySkH6kZU%Z&pVqiuWZ?QJ`Hdj|jwFSx2pSuRYd3}2v~ zm+UQN>g@Vq0OUQCHcp}~8$*+56XTVy*MWr%;C+tIOZMcvjKh%aNri5xd{3;p!1iFi zNamFXm=54aNZ|g#1B?FsuQk&XN#JyDM-R8{?Y95{b$c8X5*1V`jK~8{p%apI+LGCo zV8qMH!fjw#Qhiz%001$tUZALl2by|Ke+-k)uM76{s%*zJ63?y`yrEkC8%t=kHK+jA z*`G2|>D)``1a?S3`kzy?g$k*9!Ri41&9Bty)GwHESlGS&4nKrgw{|wIM~$*aPFgOi z2Kf>IFR?Ca?AetN#LWR9THdt{ROr%LP=^_pmW{JUiQ=B;%t<`GsX#Onb%+6AI2c$A z58aERbuV$>oE0HL#n@#ccQ+rM*@%gZ3 zPlp6`oWNsjV>}pI>0)Ku&uX^)ylPEs|z3l%4uqQPaC#(OlSU;$~d7vAqj~2JU7WDAY)eE z41gw$?m1p!ZoA!*?L)ggK%hEXyF0c?oxV|@ag%eWLYx^fdM*h}kieu$Fo-MlY6NbN z;L~0a61)GXQy>AaA6+BnRQ=fNWE4 zG)SP6Ug;pir5$dT>~Jg421eW=aO!a4HCszO(f}ZR5*)91dAJY&aDrFprInOP!927C z#0daSaDQ5-*=~xdSJR4pK{6u%SA@{xaRz|%;Q;3MkL%WbMdgC%Sf3XF1#}JqAh{b| zOW;?2c_TePr%Qi6vBT}6J=iYUeaHtZK-q$tZ9<6l)LNY>!%uh{c3)xh5($i}xX8-X zCZ(CDhD+Sw91)z6$ea{tnzPOr1`o}6KvrIcR5j@2USmt zcKEn!_nvgE`$CffAxDunDpO7usjRNZKs;$x*(LhkFKwraM8`~P?6zXt>p-lMKwIT* z;*LgBg;atnNYh&dYDKCU&2gO5qRu6B8(Hy`Y0KWU( zopnmNU5{)BCU*##JGd(UFi7jP_-l3Q@%17ALSsSoqIqRyuUc1j0-aa;M6qG!VgiUO zK6gcDT`S%k;;cv+PoVEGDp#3wQXptCICo)4~YT*M!sqo6XRcUX`%q&(q1+N;D)mVU(e17z;&$*mFyZ2 z3&tkJ{Eq`rd0Dmn59y74v}xThntp)EfE8BP>Ke~feo|hilLDdKzyaik#%49KjY>lK zgn^?!0H9hE03-lI4gl3yRF*Ig03>i$xdwnn6O>mF=kw4Tq;}1^1i*@_Fc3JEwfpJ+ zC4tg&T=(~h0HDt2)ctW3BH3H3MQ$Q$GV}rCIms5HU;M@Gb#1iAVH9ZfLjV-4A^@mF z-eg$dkzNQu^TJLmH&oVOUa2Dh`V9ay8DC=!0I0yBN+U`s4uIoi;87#n#VQYq)?e)*Skz>F>=#kVoS^`nbE2~LGf21rHf$-#9bRGcB#j8 z$-cT&;Oa7_FmSXeCw7ABh|Ws`LIqA`PTeUd`-F5mMCvAux*-0C9AqS?e+G5@4LLCO4AQiSWSERL4MFQXa)^57Q zfENxRK>`Ah;6jN>DC^0HCkG_@Z@}n-q0LAuLvTgOgc{Zbh4PDtaZ9THbK;g6huE9~ za9lt?3zQXE#eRT5phUOdm<%??kLS$XO$oIph^2G}fa9)hLK{~>&-Lss8@LJM8%!;3 zcF0YzJy$JUv;+A12RmM!mIT@`-l4|XMgnb+d(_8SFg{$`>6=44dkBCH0I>TNXhkjb zQe)waWZuxS&_&X2|r;R2HHagKppZO)(^;zn`1&`n0hpH!)H!f!5i7e@g+aM7y__((x3ZJ3 zOzq?y&~K;MfX)K$Mr=ZYWz2w3fnaCR4tV#gh`TDST8z{K0CBH~hcs;%@sdT6vPyy4 z&}yhLS#fYd9!K;LMO8>FMJWUN8k7cUI^xwxIe~S zA#zZT{VEK~&x&sD6Z5<~vzK4RW=2|x z`I2)+&ncq+C0u6t6&37(UYJn2Iky%?-J|%v9YyzTzqG^V1y@-^6RH^i4gTr59tp6j z`VqbV&4aZU660ld%2$}nUDh=KUc5``1pp%eC_2mmERgGx_-M)JJ#2lh5}P~okSwZBIbcvb+B0f6~3zw+Ud)K#J>2-!oGYvNT5)d zH!`3=> zH7YQo6EhqV)3}g>a0#$cl^Xys;$NdO7ZKRyw$UxM?d{HyeFZvf2*9neDiafkGMS(N zNGe*jVbL$Vp8>#?;w}PV$oabiN*!lyMlO30Kvqq51AUixV@}6uo1l!w-=dBf)g#0zWAq$E)u(t*4N0F}8|qs$yeY65mEZ9CdHvbQ!K+J1XK+x;x}5aVp-+Aevv z!WnlO-+lkcQ%dbnil0{3T$8f;rFLJmtWoNN@mUjwU^*9L#I8>OH|a0~K^mGTAzF1D zr<7PY#T-EHO3Nbv3^*WZ^c9%JQ%E46-l=u$XzR!xZapL~4>sWLGFK%Pw_K$%Q2g8_ z@OS_6TWbIbj5`5BJ1OP`WLYC}gh2t|R9OSipT+zX5VJeIGhnWnPuX2Dp983jX`2oS zQJ}(t(@z&Ty9{`U`J?S42av0ZtBY6WP^qj!mjiPn{-qDzb^!7OYXAVzF0KJonV}N^ z=yd>$V&0K}Gm|kS#e91%^`1T41^`mn@9wWR$$@C+ zv*MQ9Mh3(IeD7QD(4`E%IN<@x%n}S)H+6$V0t*1(6^#i2Mr?7BnITD?asZN0IOSnl zr@b?edax$0=Q@oERVK8FCIA>S8$<^)e@?H_^WB#O1R#F_$<;OA`8z5kFC|Gw|M+kJ zWSWPx+93fDPhuTdC92Y{HURN3CbJ6#Ez!UJ zdw(|7O9hk3*t0T^SsZ|jIJiZM8@x_JDcvgzs78pVXooW6gF8*=CF3Iad>D|@iUI~O z>NrYIvLLKL8dqc{NE8bo%!56+K4( z;a7h;RSQ(}3Vxu${34S;@It5%^-h)FXC&(j#Wnn1Mk$^mnhtS{OH&OW*A`Asxj4l} z@)UrPRGe4-^CK<|yz13LmCQ9Kr(mwmP=|(FHM3OY-Xe|K@ctub zfk4df0B{w;Am@5R4*)9O<{_=3{(=BNnt97lZmofFF)s>?=d1E#{d4!y!0SFe3B3X= z0P4+&l)UH6OA@h0;4%;n;5Yx-&p7~rn8UnsgB(C^#44HVJz>2stXN* z4(L%i01h)rK_r>yj5+{1h*BTzdhUlyOcNHVJ{Oq_Dl%25YrV@%yM*_|wBGKvGtb{W zA-N+3F}p*pGrTjfMKVJwscejSA?bcDg$U+F0-@cEe&e5f0|4}Jhl5ZacK~=H3{=6A z>;RzXJ)RMtE+fiCD?Ke!`zbTWQv{5f(&2?7Nhwn!wf}Wx7}fyvqZ|kxO5^vMTG#8p zsz@|o+_N#Bp|NBKfeJYQsgVFk0{`wG`~{c5Kq{0;V1_DrEQFY(&oV+EV#Vq!j6UWi z57j2CjFeS1UdC(R`GF<3slQ7?TJK&GhOjvCxqRqPo-3$KP#d4jLjN_hq1|^7Y@Qg; zC7=z6{^j5KsZ?!FsgZ0z-4w0b`CL_`>&%q7PHVzk<1QlYSiRAL+73+iquAe1+-wfI z%9`1*e8#e4vO5zDp1o5%tD+F$)MjN61R z`V0x=qz@<-*Uf_dq3i6Z-mu7L`}M9%gQ>&@KD@$Ip~4ZL_2)ifyowrBQGwhpf+XaT zh?S70B&Hxz;kFWgeGbd>&C3JE5EI>kkX_~yKxZxb<)8iIoW!9qF@*tjS5%G+;R>gD z4j^d3>i}E=0FgaKzwqadJfYEz1VE*6HKrwYq19=u7Y$nN9(B?#o?_1)&^vwu+QnzL zc<-gj@3h-`D|RLVM3KDJy_Ayg*RERtIuMl;tHz^0%RZz^vbkJ+bM?vB+%rm*bVYyj zryhDvEODwqF{VzV)dBD!NkFG$k5@r@8+yCwzyS!vu6oFTk{VS|l{w#=(~Z}>e~iJ& zV`)HJYJYnL_0bPyFY4f{HY8g>i-KPBa0kHmef7-}$DIAu;U$ zTmk|RLJUbS_d@PQT^a#|_{>R0AAS3$|75BM>cOr)FfUco&WA2N@3W&q^7;C|$>fKa zKxp7%#g4h|!kX@(2P*v4&2zfL{aXJ8Tq?8%fajA6Ia4kH)n&u3=esXE2u=0?!aw^r z{^FlVODa?FU>TD@Xw@4+YB4En(4_1J3=2ru{U8DL4>VT1!~rOL$ahUZf=3}-kxIOF z1qtK;@Wikro^P0000Rh|Iva=_1f zC>VJ4>+F960>MEVDoRiNDS}!}%5oe7mX-)--`n3GewWsG!*s8xkT-n;$4=8Qqbs)z z>gDfuPDC^DQbauDh6r|>-+9+3;wt>}wkCqlYr6dgVH-mV9i3%ftfX9MSSNenrwIfh zYD(JrOfOX?^dYaFDfXnaM7pRm2>(p_c646W7{jgM9JCmer<<;K#+Fdy{vtrCx!EaA zhbcv@B^g2m4X02JYpLV4%YZ3LgdoIt`^!BxgF=G*AJM-`vk$ZWaA(1EX94?!(2f5l zy|##Ms;X7Q4!j~d_n@m9i=-@JwFDm$7M?xi4Ycwn-dt+qNRveMCg$OeccTV^aj#H zWlA`Cu}tc1bRB$@+c&Rw@m&SeT?JEa4^A8sLh75FjnaTEU$;_*F6=C5?r1si*qK1< zGq^esIthl5a#ZmdPiN~!MFXkg)XWEM7(H6nK9~RYDkE+lV*?Smz|o0 zqtooW(!zO@wM6u+#tp@2m0#gtY*s8_Hg&X~ps!AiNsf7OvtpSvmhZI?m@<4@l&0VR zM-o(lV}yKQu@Lwna}$1J(HNa>3$%CD^1;54$P4sIGweJ>XT zeQN^g(w5r2(yke!(vkbYV^7x>kAwExRcjw(LE*gZNBm3*ZbfyZC3952M1H}0j0PKh!3(N2wr^V)(h1uwrv$D~HOva@4H^*!8=uuj z)*-g9+V-KWPJ4h86MunyBX+ku&@$A^zuc1k6^q2(78DcQo2Fi<_dYIb+`zrbt;naz zWW%!YzP!JPXg3QO?xz<_kTR$->mTzQoO+L>p$vy~rSX)qr9-;-C@bv4j^4B!y#_TN z?x#mblkH{8h413Dq2XredM!bm0m1zw-=(lKAY}XRHbN-c45 zUEiH$X?LH2)m)WiqlM&a0iN@P1JtXx&PLtsdOC#d*8d+vb#Q&)-uU56e#t-croyoz zYKFmFVcQ=M)uO2tLpNULG_`*hxMK$u#KZ=V{TVqq4LWZBULxR46W)^C($dfC)eE}I z3!wr|;k0L5B6(b*ofN#gI5em9_$`3MRU56IRAd)^WlCqPcx<YK4$pKN%Rn=8sCNINkg<0doZuo#aChfDtv}~ z(%0U8_P-+oQEC87*LVN%Gq0L07Li(Wsdrwg`NlS7wKw`RC#R{YlH*w#PdM@mTO-b* z%xG;Gv-^RuZQ^oRa@2Ju2X%>d0GA7KFkb(YLu?Ei}r^`1r3A^rH^DZigC zA=}H|^($3ungGnx8KljPckPd%*V7lAg}O?=jTSY;>1>)nv!t+PQIwYE)oi zN(&v-O@r-K_uDkC=)+3k2Wi&uemb%L zEe|*y5Z~8dSp!dgpHI*ovkCe3_4g2>okwV>7>7NEQTe&UsS5fFGkN?T_7e!m zL;Eg1OC%z-V-^~BS6=3CW}TcOp<3{ZOA}L}QC}rT#~(+Nhdq6BPPj(t8#?*J8i3c@ z5kko$=jRrW-}3Z{|CgB#-+o>&t=OEreK6zHU_|$dXTgEuE{+$*mc9UFrwZ5m`(dA% zi2?QgXx!<)RvWRLcf&bV+$8tOjLDw?vWkJHTR|0afqS2y-Py7E_0&g}v!3i7%H0-M zhWGW}m>e4FJ#QWIeDqKCIU(e1aeFSrgkIRrfeOmgYmUZvAO7(piHpAC8GO+5peMOQ zuj5+pjJ_LtuRh^fkIi~xK=8rB`l*c%{%h*qh93jh{eUMWEF&%$FJGLDeAeI+YS;(0 z-tb*GtF*5#r80=rl~8-Wmp)d;NeI|jYNX~Lp6JH|34$1Th8C+%wq~Q6R8HYH+IhP` zv-NiRsyZ5QPq$21O2K%o&)8FQ)3Gc$J03nWmnr-j(V?n7^kQgJ@nmnuvri2)Y3v-##~7k1{nCZLWSroiuimK_W+wK!AvswwG^0;PCQ^s> zLN8Xjn=u!s+r)7n_c6X@9;FDVj(OF+8x&)S2eT)OLc?D5RgHJqe%>0hoTxu@8T)UzlEERXxf+BZzycif4tx44t;J_Szcr=yue*RyMu3P%n3%VD)0 zt5%$lKX;LP?(&L(-b(|A+#x{++=|CDlDIV=YI7ZiJ3iFTlHm6ZRHe<{#O29WCmHNd zs&0O@_HFMVZ&%;bHqy2a`Cx#xJDR*`WyK>+Nykfwt?+0Jm@ou#cV4OcRKA5sTHj)W z^9@)AFC_#@BcMi{7$6M{ocqh&>9Zk_Cju<6A%lmOFU0i<&Q>nG51N}#%L9H(7L{gh z2R1xciQ;-C!ldZphzro%Qe{woG1QexL!j659 zVg>g%93~9=7LVJ<{X}}3+~1F&?_2g~6?#?JQ-SReqB|3S@zyy{33j?J2aeANRxf_6 zQlH!ttTfnnlfz0r^DVmAKkeJx#Q(%x$vyp)EjniH=z&Ii?k=+4B0hfXAbwzvP3Ugl zc6!S|bv#?LJ*PunCK8(ciB2*%qJ`7DnaM6bvm-?T_ctde^_i%i?BqL-7LC|c6t6xc zS*gP#Tdm4NJ?ldKC2!~{r)5q&pW=GX^=PJm5SAB48y?4N^V!8%>f2B5(B@;k-R{uc zIZoe#Xwg|@y_W>rzeuGy!9`WBh%3<#`Ywy+x2GDdH!&y%5fApi3p^nh^zrXITyMf0+zhSNKd)oiwEbHJrntK+R-Qhc9 zu=}T0P*tN&9v%g2STLU|O<&tRRyD7!13g&$fRt;%g$7A-#4AH>^~gnWt({}l1q*Ob zYh5&MdO|adryVpDJE+%u{D6EYx-oF*C_($r1lt%0HHMn7*!;Hgl~KUEbAP{R1kke- z@?lDOp$ey=P*!(&A5dfVb1AODrS5a3aAH!~1~>N=>O7bmGda)2qIh#SM_G@FN&_;N z7#>%WQ62_oi+~vLCXkE{KGqSw3~DqaH8O*v z)aj{GlE1r7jo4tsEhT+~oMqTJxHw7NT=Z&0D`r7biKs&ncg)gfmdF8`aAumYt6+Mx znG77_z#H>0*H7o*S34_~%VhaeI&sKbj>bfagFY6`mOu7;>zBgmMwzC|12ZOgxHB!C zxkUecAM;v^5>XM6)xtY{^IOYXf42Y+x+KCWl{K^fV70p(AtyL=S#R%k2^}wPnkG3a z^12e_!F9o8?bN&%LW+smeqLfZ_9li4m^2fCg0p2_df$z9!g=9mFdc7ZCT}uqK4lz@ z8N(HIy5KXqMYuS*H@Ug!)bjelPArYYlk5W?uh-8ep`YUX+b-!+U50q_qnOfox?7Y` zaEFdhn^oO;Gi6=2wGs_WJjR9+3R`pG41WJ-fazV3kse*cfI5-H(ru6y+w&*RnA#t)qhL(jjaRclRBQHIkpVFkc~jPbk@H^bq#g#Xf0P}_Uo_cC4zBzj`;g%vF><4@cU9J9{5E($_NT1(XNYn#)4$Iu&UMg- z>CkY6ig$1}Fe^R%4-AvEm%m5o=Frf`oi_}=_wHBB_3wbo8A8ZK6xX0)$sp>~D42vc z^c#Ro&|~IKZz*|fVv>f**A?lg?D1juuZDkImsjL$cwd+DUzLfZqrTddO5G6E!o$WB zR%L(b$HZp<``h_`w^H%hlLHB2vQNc9H0%F!9Es$Pv~UP`_*x6<$r!F;vmkY@n1h>& zyDPSwM;7ZO|NV=_Z^>W7wY+n>v377rHg(tsJ;=?=@w^e?Vv(ThX-Xf2itLZjzFSBC z*e3=Of@J<(58SK`y=z0s2IcY6uxbF>43{i9-!t!DGaPnR)5WZr#5<<$s+PkduQ}74 zI4Sjb`L%c-D@R!Ey#~?Gr(BwTY}F^BtReBQf<}#y{eAoBBu)w&{+fFty7|mNtU8T%-jf7)5d=D~C_(=DcSWW}}Y3<1DBmUCd9&{F|9 z`k{#*v~6Lx)(Sj=Uz|>+64&T6G{A0ak%V&{4SOgJ-QB;5V;rY2DLyZ()i#E99ng%=yRPX0LCbc0Fa`z~HbpDbzFLgJk+{ulk1=v+I z2XDW|>+m;llxV~o6)NxxrafIIlh(BKJ$}dN(4FI<7r5FaiZ1+p2jy+1qSnUYr@8!6DS|GgiVylX@e6fGyUnw+P!R*iJvb$>YC6nlVb*WPp}ugGmA@je=9e9t zz}47{HwfO<%Vj+(-Z(RXluHW>zv4hcy_CH?$p}+0Mfjr+Ep3HA_w}CR4;KEFoaNbO zZmX>~g&ys<6n&t-Iv?A@OOZlHSsyqixwaTklfB7%Ve&EWEW5ayNe2CA$k3niwGWWER{-Avz1EtK#Wmo}&HB^(7FE9A zwzX@SyGJO)^9sh>LqbV_CMe^a6>$*YCl6aum?HgxebJ#p1 zZG@La%28^tsau5sfx+}-3fFgRQ*Vs(sm)rkKdmeC1vHDtlvmKV_=ac_KP0%R3V?fo z^An^bErc)hXwdXQWfkni3-N!OHdil}UgktLAH}g+k|)l4U-RCjyb7*53_bMycH5s) z`Ba5Txkn5RG%L@Ux>+-KP65{$N|B#CwKoYdMV^~_;aiGmk6x@kdhlRjix5PhP~ zqE1hv)MKSCuSt02M<}`=>ee>+?wnYjFYcD%D?)>8y7=y*Qi8V{YiS8~E)-s>U|yb% zwEqGID3t>PqLU32+H&;M$(Ea|O>VjsAKiGK_(7I)IOvq&s7<@S$<&{2s?ktfy1nvn zqkGQpaIN{uj5JApkl?@Dd|pj*GBQLlc zw-F1ekvZ*a{GzApKO@(q3&?5?ui$LzZL=5MwOVBn(r_^tn74wD0=&yiF3q*X*p z?#JNQrzABqL1@MVUYyXTHa(!FprnJ1Q#S`T{<^hm2z10|Oc^2+VBQ*?CX3R+!6lzz zIO-L}EuBb<6s*2@h)?0Mx0 zH$SttvMiP8azA2c4v{_ejF@a6CWgvrx=7l5i>IGOSi?)zZM2x!oQg_TM_hu%bB%l= z#U6;<)&7ytFFY8RSrBPZ^Fo~4*m#p`!V|XY0J~Y_BgOSHz&X|0(b+tatJQf-l&R@b zL9}7X!0G#4c8`!o@@>aQ@lt*)DRo|A-CcAsH>iOz zv7srWD0Z|3X>2ZM#G}Oob`YZIFK=!3j01OYMg_m(V5mwfjW)V|K9n(AJiVnqap6En z5F^W0ZckyyeNL3m^pNlvPZ5LZ_3~j;8vWcGp9v3&{_`0I$`ic4h0dMlQ5%>PcHLAN z3J@IB-%|W$7OrTjv~()pnfw-64`1l?#NC~cpS&MJT9~Y1wFtg)H>yR!|4>@>nd2-p zJRAlv@A;I#-wstK{j71vl^cNz$eXNIP)|RiT1s;Nc*^+Y))d#KQr$rj7S=x8TA@#{ zZvg&TahCdO1B4MVGcYhjnmbQmwG5j3l5QD%wm&60D^k$tFiyRBp0B<3Hvtf?;zcNw zHL;_8KPQflgDR9odRzGJK2TTg6;@9KZEHgf(;|_B56X2$kO4;vq^54dOue61NE$+C z>`Q5$uo8^JhGF)PDXFyQ^~hJzbb&l2sLzE z`?BCZEWEV4+ga%@k4@2nizTCCv>bY(+lTxmuG3TT^x3YDt?lQ?6VB&~JQOar;&ev7 zl*Yk$M(^b@!onLIBm)*lXScUoTj$R0BxYx>5BesRHl4<`XNhiba7?Ie;&Tdowsg(j z_&-!;g7Oai@u`{3&are`eb|OVW0~Nk+FissCuEKeb>w5LBU_fv{@-Y_A0B@=u|zPa~ivIdL}~ z>T((V<5YYD;;~DqTOh4A_yVbmrAWs>D~t{T-9m^yBo4VrvFqjM4atbo-t1sAitB6& zB+MNDYaGOXH!5+FC>$v)l!}WkxboD;Jn`WlV4quE8+k?+f#+TOsYnJ@i(7iB+rN|* zGC2EnEsy~u4Ee0l!_mUT#b0y&I*kTAxv?SNM3{wj*I>AMQW*+!i2O7@Gcz?qU{C0_ zmu(Tw03_>!g*^sK0(qy;gLLlsKD*$QNqj%+`1a8Ae@hj_pbG!}FT^o||Muke(A=B_ zmvYZ8v#kYlt1x$>Uukn?D0(>Lf)v{OyX!kq5Gah$-ullaIA^22{fP<8u zrJE$}I3#PVxV=dltLkHVszB+3+`!(Di`gj?v&=&`4J&x*^5yhu1EZfVfHIn%)u5D8 zagU3xplYIZXviNB$fnB`4vH4%ThF2`gN}b1`QhcW%EHDQQ$c!Mm4EHbE~B7$#q05} ztUaVS7~&!$n^yeUD*N>{ORdrtO$_NOTkQ_($JSH*LOFj^ZDIgC>%}y}rblXj2H`UX z(+y^zVhqWEdd(;2tq6V z4Eo9yndT6gnHkQ(t~@0ZI}FzTmRGNv7zaasU~1&rL2u)Q?B^?76Dj@=!)Q|>PDSoO zZx|dBVsS6Q+TMf<5VA=dS03hl5@-Rm&wke2;PYcg%OLVm(96^<{BBA9=ETkdtOv=r zq()OI%+6x%m_5^g^Q%+5o}|{^3WD-vOaShtdu#Y?Epi1PyhAE`@QnCI@z!fCv^1

#lk%6y3x_{i!a6w9ERswXGxyMiLWSWtz~X7Q02{$G^Fn1^k&a| z)ZbzFQEhZI0GnntMH>-(cH=tHHJXAO{SMJ;YI(f^QT_c-G>eW;&m+m3_=}aCMuz(V zJCNBL!GPuoZ^2ICKK)p``U18}j%EEQBqNV#l^c=+zjxaD)0NC09T*r`HQ^?~l@M{A zC#to0x`~nMD*0%}GWuzz->J%cY$+QQi6Qu{jhw8X28pq8adK%p41Z#?>JY)X?=R0q zCMllHGgyZD`Br%GL#FkI4=VKB)0zBZlV1qF5fj)#36iSR)4v@BbA<^C?li=Kk`ZIe1^?irvx9u9^ov zggiMk>o3v28zjT_$|iZR^yQqc!em7ffh@}NykEWr6$Vjv(|8UStHQ6P^#NiWHlDv~ zX2M^Ba?<5~vy7$3XqJzUuR=5Vti5bgAs!1(Zb=()_D=iaBg zMu~|@2OjtRkOFbrcHLCNt+V?<)mhiR1fPTW_D3-7zW}aggL7 zajq|6W@|SBn#_bn-Z#i9T27Pa0CuD&ZP0hud_61QmHfw>v8OVdxflIJI~ieu<%d5+ z|8Nsyqvt2ivx*Yxyeb3H-eOq^aYF+~TQAWKMv3|0|2x1-n~)f-TODZ&F?Yf<_sxym zZ>KXv=gm2>kwdC1ME|k6j|r><;lCfY=arFymh4D0{21=QvdPkwlmhj%RD_9cY%;%1 ztl5tjrFJHUc8i`!nF;Q*`FCsGWI(_jqpE7}oqLg*6Zu`>XL%AKkiX*{rdyxM!HL$h z<63)6g+~^yCl37x%(_%iv{>oRZe6j)t)LJX6tI1q!|h28Aqh4}8yNZY>e~tRg@fn? zJDD|-7g1OWz?6x&u&-J>tH(yOC%KcDC7L}QfRUKuOi=4V9c%}BL_dA`N8O4|eLi-9 z$&@PMtqs9fkPn3|bg7yso@q+@<~~Jc(A%61_QV&Tl(MGVXzL7D2NyQ<7{=555676# z>WHo$L+sMR@c`+0PbF(9w}KyW%Q5r>prMCt=R?4di_MhkpcgPz%~Rt>L&saHo(M}9 zHYOYx806&SNb9`IR030;l*-?i58YchU28lXGaw3Laye|c$;?wyzT(PQoA~72JgB(u zqlYo(>FM_L?JMOR+KGC)=RcdtwPnrQU0R~eb`TsLWq9!Exx9&)sSD;I0A3%XXA!O$k&s9nmqabbaMR z6uqXEOuye@lE?J-W24YBBSrG@ARwGn3Eyq0v-v;VqMyb&@}l5=z%N+H_{paRH6~^0 zMT;k=r*8FTky^36OK~F;$ESNii!^$S>VJQ^y%(vxhPkDz8*d&3^?sD14{o3HL}s2I zTb_4{&IarzRh; zVE#p`F3`M|HEutW+R>aK^BJO|xA&&uHDe5Cx+eCAvkhx_;&u0vI7YU;(7 z;`yOy)9Fs{wuZ?9#afLG!~>w!x=H%&66htU*FJ6bzqqoCNBQD&5d-&}R6 zGP|R{K69y9I%}j5b9AI(pvs>Q5tQ2#AH4!tQta(xd+l8jnUc-aV68ClA1ynLQ^f54 zRK%>6NQZiDEZ9kKP|=!mj~S#U2k)HA^BAVNY^(87pkLpiR9bHwaXDMqds;eP<36Sk zviG%=Jw2@9+u#7ePDX<8bDH32e1dtr`*6j(^bUGWxbkP>6Pg8C43^>5u(gT8#XN23 z+2%#oRINwz{)tfgT+avhMryr(I@}wn3?Wxf+DcO4|KDAJd*8%igX&g0ydu}FM*sL2 z7&tmE3G!hk-A3b%=WP_IH*H z`l;JjhPJnRdxx6IYt13Yi>#r?yJHWDf{%`hF-J|CRn|Y3c$@(W79?mrWhQ z_RVWEME}L98jSQ56*kByS_-rFbr!l$RmEY<@9Bx838vUk+kcMZ)G=!RnwS5IaWMV^ z$pz=amhh18?eLE0rX8+ocU4IAVj}lqB9!cLzW-vdS&ZY#Pi1p=uIW&3{P=jmX0z=# zobhWdsuU1Ub*G*Up~P@aElIBtr^Q7s4cuASJ3k{^p3M~S0ab@rCmZwx!)Bb3d;>84eS@Zd1^Ldtl z&DtI2w+tcwQaIj{{%C6IQilK{j+z%+X=Ofq=ie?+#>@;-vUr?lld$Bun7p27sLu!jSQO58FVNQmya)2$OsYx^fQA=?uG z$J+0l5%JXlHpr>C;)0VCQ2P(eBF4M8huVEJD+2?qb8}WQ9BIb`cBfUjO*_B)M4PwT z`kGG{vlcy_9D=8&&;Dg?a#@TQaDR8NK={6*h3UPH0;bTXs6Xyl`UbafT43Ze4!Adb-zJ5PHF4%QS-4;lxQ5C*jmH$dgK13 zxJ8$nueFr_(}a8~jDyKPX8^B0Bql1L=Cr!1JBolhJb7NO@|C9IaA8;e8!E)F_PA$wcBdAkY0Cv);F>}x{FEr8n5 z0EsC$DYYD6)f*UGZMX)fPKBt0F`DkI|0U;%iQ`Vy*lz+<8nb>)jr_4N;}XNbM&gPS zdgDYR%^rhwzlO0`FoE*)Nj#Fx);>tI3^;zbl-n%Jp(ExhmF3YKSnV7@UT^n{xfbjW zM}^gW7DCz^kBzSB>@N=y8=NlzRdqbY46bKBb!e{r1nY>wM#E6(lt9g)RNJ~h7KwP_ z1rru!G3*;V9=ZZ*`M~JVlLC)-k>MR@JdbRKn#Jj|w zGA_Zp{Z@>U?gdS93-qImjNAl)Kx<;*z8ZMQ9enXuBR3`Ze8^*2dhgz#x|x^x{I|FB z0W}uFX$6i;VPd;J{?YF!weC6YZK2!U_DbtI+?>^l$5u@c9jH?t# zN<7$s6&7Or@z2~&TAl1HPtR+swZ72H!o4~v%;))a%mL=ut*4uXyObKmlb)%O2&fMt zcdsfV4VW=dp8P+O?!(ntg_&iedR)1=7X7xlHUtdfONKqB2g<{q)HmUcyffd~=5yW}wy{FGXUE7Q z1TVZZGn3iskL*`AWPZ8W?pAH8ec&!uE~*fqHuL_^O=<|#6P51&BR>CmVPU}Dvc`-g zlVS=baF-}ijhYPDtmB~D{SMFY`u40WJ+5<8@pwM2tT{9YSD3XdQx~{{zw&uSfSrLl zb;!!m5y_bphoJirt{!O<_ILrqo%6kVZq{?NMd4z3b2+UeO^E3)TrAuyjN;*#Z``p> zh1nfLS6&gCo(EEyI(oXg4~e~2GV`%YV}4m9t3!F+xj`XIvxiCeKz|mp0`p7$9;jVd zK?o8`UDY}?ySHTpbW%3)4x`;NT=Wm1s*gW2&r~L-AA7r41|3ONJRwAYDA2GBAS&Q7 z=8Cz40K=QiPatI^FPe8(8ETxqyZN3`alE*QU1>U zI?r6yV=tLImfwBK<=NkHjI@z@0Nn zu+GR+gZ=-q=JWcLm*rJeRiU0@QxiYq5K&=or^)!-+}zNe30A_{))9l{#jg(h8mFfo zFH2xv})yS|GuF=3>2QGi|MK(`IBpu^Wp* zi+qXN%}cgd6?hft=^pBj7L%RK)#bN^L)xLOu*f+Xp}y9E{&UjB38Aaq?GY6UO$W?s z)ZH?97|9{xxw+V!TK8Pmi2-k4e>9RU@dvsm?TMyW(=#AR*Ajvb|5k}^`Uc07zU|&V za35~u?|GtWlE(MsVb_-yL%7|<*S}xZ8Uq{imP$FVPS8xkVLEpaOX$>XJ(E>;SeY%nh~+C8om*qJFgYeI_FGm!yxje!e! z)O{Fov2n=2+IKtb!C2r}LE*}iSXAsA#guab{_kgv(lu>{E%M9@(p!D|D_?Iv{U}BW zp?UJ>7xTAP9Aa|r;5XyJ0PO|n;ITMS0tqvJ|pI@4kG>gA4cb?T8m~C_g z#1OWW*-$ZHkJ$h@mi)dER>cz5&PPk@R6_Cfkzb^hUlDoh{Csxc;9W^b(a7jC!43H% z6CDnhfa4=A@p{R7N^B9=T4Pc0Fcb)ka?;MR?+n6Ln{JZ^%&?}W53wU`K$P{_D`{P=_ynn4; zH1pg?ZlFT24{3QTuD-@s2Ajyg%P*Rk;GDsUH10F7U;SY)FduPA5=33}fm2xcLk$3I z#H&%wZ_k;7Jpa>x{D_m!%ce*k7LS$Up8h2(jfin7oN2B`tZ~4?|ALhuVKJyJENDPV z=rQxL(t=>w?YK_w_x4)0+O{)pI2k!EK$Ei{0ST{XJpN-~UO#CZeJCvA)B*;;+~WL_ z2Y-aUOplWrI~*|fm8{?N+G#9In6+Uy!LgM2qL``akWQHsl+AmO*8UC%(H z&q``)pf@=o*2~lzhDUz~S`*fcLXVoLUC$K0d1e~)O)nTIm0`~kZ1?`@7oWY=6(h4Hvn3D&9EL)G zQN|F07fvI^PQmoEi3KypJT3p*%LzHtv7a`X*ehN38?lH(knbpkjypT8?PHTXia13_ z3RgN9yhZbKq(E%Q*2wm)vD}C&RKTyPN8VB{= zpoD0dU42t^#K zfEX#DppW7@*d+r45r-<_KHUK-dlTqgh^~x@gGNf?!@Jbw?Sb@8#dmJo-ofWl(7MKV zYKGBL2`RyNmiNiEn;fSL-HJ!^Roq-5G12v zU?)iO2CKcnJ9}(_$@?_>&Ni?}l}kg0oA<_pBH9?J2u!4h4+tn2 zd^DpIk;(tsRT%D^Mo$&_Ng&&nabWj$U*$G$Z_#D?M^@2Y&ukwXb56g0cI}KJ>HQ^8@YXlP` z2po1jEG)*VP_D0X&i^-?>4<>=Gq5)umKt##bJ1NFoUw!Rh%r%^`7g*(=h9QE8|8IY z+Sef!;`Xo(lP5NOiIi~Yhsc3<8$ZuaMq6hG6pxP`YdvPa2W~!xMYT!k0fQx3Bq#sg zNvDyBUWZg07%K7_*dVA4U}3-w5i1_(VFE!BCrOIU^}4>9G^`XMx<)vi_bI2YUFU z?E%t=SS-ar`mS11ZW8MyAg0-k9DYlJXre%jJO~pCIGB-w!iGInq-xIJ#>LhI2oBr2I)r!}UeHP%Of=;&Y|lT4~*#4EvYHdf8I>bBmIGC*}O!bzK!lpX|c zV5X+l6faJr4;Aa0=czx&-1*%}^q|*MqYXGa)k+ubl(p{S{ts1@F_20gy)$^@*uGsY zEY!-!xKt+`H#`iCi?aCjW?XyA zZnfW+!(+?S<*uKmex%vg&&;i}`XBQ1M;3StEbmu|npZ4zfg==ogg`2AFbt~Wo}yZ* zo+2p(L4Y7(GHDWnNrO}c-D*kP+|S^Bnvt{=Rv`U+J{Tn*EYB2i36{s+v0cMHCPhAb z?vX%IftyOiNjC<{n;QB0a)q4s9*{=LM{f1lNCcgn?%TZ?S}SN?Z1D8C0zHp114U6l zp|B~*6$#FS0v~gEhgQDUSX&XGrFl;`IB1DKZMWeQqR3#PV%7Xy#|#)U#`tf}p3l;M zgFmmi{(WeTDtxv26MRd4L)WpSIn;A^peZl}7F#Ew1>A&I$Pif>8Mb_367wRNwvHFr zT=F$2O6BX1LV=wjrzdImK;ZzqVGAkWblm&BaT$aE-hD&}xVdVV7QqAw{>Yp%&ge32;|9 zpcq?z?C3L$CgU3>P+J5owjgkA?NHIbuBnl|i!(;YA+=zT5bp@onH^WDYwVHN7{fpm za8?jhBss=))!_~A@JfHiD-*VeL&)Z9s7UW6n6)YdjsiXh+z1g924aDQUjoI2{aq1r zs+~GiJOyMe0t3R@$S%$UkCDX|?5*~e)!*(Ak5cv2Fa!#+3JBUV;Ls;&B0{}1TzH#( z4Gu?uRE!xJK`K#NU@_Xew04(1TuVYCV2mO_d_YmK)>tSq$(s$J(-lpD`v)^*B{ted zH*wBvEGJja(3)ejkNxemmzf}nag(KDcj2;_@)tV8e+=&3(9;`?9$pEO)6%wmsl@g6 zw`L$0>NCZy!FE3D7%`|2gdq$hOowWL=rKYcaT&hd0f~KoH~Xw$*n^v6x-NKkbf4Yt zlP?3JZ(?C&t(L$2I;X<1fj-Nvz8v@Bn)N#8_!^ve&5QmvoOAXIdjnS0V7csUU?Mrx zAw_?AlJ>)2jDleXMfZS`;B&eUAhEx#6_muJw>C>i(B*Vtj%qOS3QpQ%)5Fd8pmd|O z%(C8(+&)uO+g3*lw;bTUA0VrP4me&n>O?>3uWI;u|H;7nLXo1%>Vy(@TXx5sOED(5 z*F@tAeSb*ennw?jdamyA&e>4Z0u2Pr(yhCtRy{9hZS+>vyddA=pfMc5s#-WT>QxMI zwa-C%u8O}t-r@$&P6Yg2+nQ|6fv>{8Axysqzt~%yiJ{_8su#CmH(qkSG*>A6kGRV^ zXeqh=N1n5hA_}ec*gkd0bs|3e`7!)Kt2H zw>2e!YiU8(K(QSq`@U^;*E%aUyLFs3_TJ5`J#G|jK}#tp$OdlvqKI=sI&$%on$8F& zUxNza-nU{^PM@LBRLHC=lxz=9zbXcyALzkKa`>sh%9v2r(#}*Nx0P(tJ~3_p+p1 zW(@STsMuJ7%`H$+Kn0A z&1#CRgD>Cza?Rb%eVuUecUX+1{JAGZEy*%FAze~7bYjKr_Luu#C}Kb`-%%5yw!5}- zVVhLfVY{Kn1LrHru+|d8IsFOiiAzI_4KIh?OkjmP&LHln7%^I-`~Yp3ZE7} zQTk~9M<$Q&z&p?UjF@{iWqG!$DE_`*TRGIUh*=7HmJ@QZdPe5P1!T2}WX9|2QhOvu zODY8Z+1POAj6fp5DgbwpxArZyp2}$W+V@8)q1;vbozZJz!|>{%^kMtgqO+LHoDv=L z0k!rye_uJIXpuetKf|8&_g&)1OP%qrI`o;?^j)uAQU~VKqBr8IcIWzr33WFt%DEAa zw{}kaJc8>&J*!4c_y@IEhxrNLGlbFaujF=Z=W}0{F8@c$gq@^? za8+?{UZOYIol?D-f5B$;5|M@U-$WE&F_j98|sWw+rzsZ~pxEmt|q%uCy^}g`-@XFSVom|EM zL8i3ii}B%a#LeeHmfeqOvCrclKWofK)D1QtrH7t1T1ttSu-Lvlvncwib+yK$_^?dj zXgRg6sjI)&dyh1;(pURHE|l!&nk8LB-!lw{!NI7g>urf8GB_Orqhqwfea`HA&do5W7rTzUOqA5=(HB;6LoQ;l=trfc#cB)aWQ6BN zPo{=R`!IWW`l{I8$iRMNqr6dBXU;uFja%*fg9Kd`*#x~10*Q2_6o*uWPynq&+j@3b zj{EpzrSYw*-j%=8b#l-9o;3&ikd+D=OP-=MDPX?+qc`+FS0Qt9Iht-)hL2+J4d_2* zr$l8Ceb;GdoE>F_245q)r{SepRlLok36_G1Crze!i?stO(sUq9nKe~a zH+8ssgM>S{`Q%1A6vh%}6vbIpl`rL^aGky@74zgyUXS~8pVAElH@{LR#ajpvVm{j| zLGX7cTH(De)$mZC;V;~P(qk;E$(uEl&t#4OiP_%d1&I<@T9W&~9i7R*TGFD{C+}Jl z;N|~o?fGo|#p7|8NPF??_+Y<-`Q>aX4?M|Z=6486lM0Oq^et&=Kx{fa1*{AmOgQ_MYo(}=D_C+L?*iW(P>#9Wp-^{Vl zec*n8n;d%{GHNk38Uo#cvu58NkW!T!Ob41}v0M%VdkUNO+io?Ii@H5eHUjbA-cj3o zA#R->i=NM19F<*I4@~J0pM~2EHxtHJ4vAT7u8j;_Avdb!U>ZQ+=B1`@-ts;tt<~(C zKBHc6MnF-3^k8h9Q37r3lGM4!4QQ2EIWN~4f!N7LX@#wJ+yP|^rAkz0iC|yDz*1i7 z@llWjveCsL)$BJjp8}n7w#S#4`J1)SZIV60+!|>u99Jr@iOPH=_JG3o!(s# zIBKGzD*s2E_nU$4ZV!ek?}gX9)%1D4-Z;8_dw^J^c_f{v^}wa~vt+x?nKxtFmx*=E zou^r@Yq1eOH;S^R0=;V0K1gQX4>(0j4mzVx$cBzn3{wk&W(X8Of9}Qr+ye!)Rbie= zaFwWQjHvO7Dj)sy-aTZd&Va5(mJJj2ot>Jp*DpJ78r~X*T1R$@ePbW}s%@n%IO0vv ziOBb2cR$Px-6x!%HJ^+%ksh*j1WG@T_A-hZKHlTz;-+`Fp2HPVQ}IHL%p<5{%oC2< zx_A)wHw6WOIk4bH{~V*=7xZrJeeGefao8Are>JSb%hozOsjik~*(t4m*!kb3>no4O zmD-5AX;rwfpX}NsHzf4z`3GX8X)OSxb!JlZrY`j$A#r(ct1lsk90ChxhC-mARzD~# zD9%&KQwai#DVJfy<>T@*=n`3~RPouEuX$nW$WcyTjraESU7PB4;fvMZihlj^`Fej_ zxWagdYgIb3re^%xk#V}yOX@KO(uT$Tn8r0wW#@X9hlOrRc31bsFA@o2&5i~CFKff# z{(6v5T@aHsBa82Pv3sFgh{AQO9>4w|QTH>l^eug6M4@=E%@6FZd}*NV&jks23lN1H z%gec@8COC!_Rr#PS@TBiT9nkt!KC5EM=7GvMhK7 z1_}o>im+lURfvkeo@Y3`4d$?vk#i4YZrAOB&1LBJV)mV~{;Flr%ooWr!{tmXRaSlDAy(%V-aTNr6e?FcdaC^)Mj4a_hoCQjXVu+V;a5(ooe^DOI+L F_p5rV_MNZa4^^>53_?_;TFT7LKkp8nO2l`ulHN91 zSWVM}D!&!_ST!@|nguhP?_G?^q8LOJl!N#;(IPSiYF}TPl>;zzpa`l2l5N8sK>Fhi z2uV;XKq*iH)NGJuq+gx_(Lk<(t^iN~28aXc>Ik42q#{rXK!8$c4kBH-3zUE^gP{Q> zKn-x7L;7|Ys0t<)C;?Ij!%>slkbgbqQgUDe00fGn$gVOhuFX8@CdAE%24z%g`GvMR zA-FU%Ce(FCE~=`oVT03UHvn*DcH$sg+1ZH+fxrYKUQ z!+pB99oOEy2yOn|4JQOqR5b!%>Irype)3*8nu_ejB}u8*b(58CaW~@l{oxdsizm5y zU6y4aI`s9O#L?SBk#IPfE{lSo1NgeU7yA2f{Nc_0{bV+mt%ZxD5n%g1Zdv5Zv88_~7mVLU0X1gFn7=&VTOx z->bED?cH0us#?mrBUO}T(NRcH0002GyquKU8|He07$k(ZT;%nk2>?J*u#uEhk(ZPN zsW>}Y+SpqF0CJHjsfZeQ2LvO(C#G%<$Jjt1izB5D7E~mb9C&Q0j;8<1~<*-!D>P#f}&=akF1%7v5oo5T(-EG`YLjR^Qv+zp4o;yOsyI`8PDU&W*rmX4AF`Xd=G96(ZR4_u9I+0P8RIhce=r&e zP8R3w!~O02=?e(x$3hw61T<-Vm>$I%`srq^elwX!vy%us#LFg^s_&P}7Iw-_Np7-_ z`s4L2etKj}vi5sM>$M|~Sx-gpwToXq$Nu|IwI@k7CCcEc?~|c+4g-56`swK|WW6n~ zF7q#GbaW-JH|OO^pCjgC^}Wr+*T2o?Cg z;K9AoADxxgA%@3b?7XT5;PoVkd`Y~r0+O|(fP|muO1IFy^THfLBIbsO=LPJKW#CBU zg2@;HNvs23N;lh05Pf}>jDDg5u~4GL;p!o9rqB?39ZWA3fW8T0vN+voupU2Bcs_ib zBw}{(#~Ik=Z45aRB#Hr|t{`MMaVQe*XBveF!Gd5SDgq=-C$S%p_6Sj&C@B!999LqT zISnZUrih49@|M~S8V}_xf^84Drf!7*&GEeAx+IYqU{fSDRSg&L_VSg>_(%fSB+O>^ z4i&fwH)2u?G!KDI#Ajmc`OB)v4@Bhe0D;OXAZx4`2z z2{ST>)CP2NNS}}#IdO)E`O`W5^>TGU^!=%3c6Aa}Xq7{m_3u|S?E$0%A~+dR_yyRr zsSKbnEVGD=!4Ia$^_iZ@o~fQW{8GNs)CqWU-n4d!?--NuKIus_(Vo%sB&^AI7m3gD zXDDh>lO~*v_Lx%%#AeHGN{fhzNT1VSCTyu3;<-h^59Q#<$d~M@C@|b4CnbxFaaaT~ zNiOEEsri*BEZ{FtAN$*q-k`We2#O~aAI+ZH0GmA5y!9BX6WmkX({6*5OYP>shgU~V z*KXHp4^$6c4}fRZ;2{q4HjaMcqnODU5*eW#f*tsz(FKdkjL#F?`<9mESKL=3SFw2d zV^Rtre8y(23WEyWiciPo+?sl_Yx3Up*9u2CM>KQVMR`ya3s(yR^AIjO^D&FK(d+S; z@dk@Y3qOk_b5x5H3wCH+9hAi|m6SO#WqfT!aFWCvcSX;huqC29ushE$RhU*baXtwb zIy8rZRN8w>G|4{MdlXMH4k0~S0vgDpR z%=)9}R6=dcslaJmFaM|Gmk?cCVwPIaoK`VGvCmj~L;j`jp5mVHm{m&NL~@o5pE~#Z zulw5>t(q$MlK5X!G?^YX*eYWxE6TmgO3KdFgsUHg_Zp} z9~f_B$-Lrl-zBJz2@DX=tIS?3$Sto_&Q^2gmzNTkWfZO!d3}m58Jdq;TB}(s=P9YK zIV;Sm-uULHA6_vqAGx?%K3{yLr~Qdf$D`08`aB}HAXm(>*eH0@(66MY@WuI=>Ddl} z7B*KrsLvsoLULhdjANk*UATE4T zJG}rsEBz;>W~Bke)=c*df9^)EkKF5y?jzY2=N7qUo0cy>`e!7}RUMor6K#$h+Lsy@ zFIVjgS5+JDCVFg!S}yC?tQT!=0_`U8T}aytm)4fBm++UKKM3Sf)o7g8<+Om zCiV4ez?o;C*F{fb&TvkT&t&!wCnGZ(zfOJYF6o}}uk9WT+`-zu=nfuK9Z)^g+4~z% zI~2dGeHEUAmi;RSgJ0N<*M&fc(#`+O^rT^}cq8TK@pk6#wXul=EQ67lBGTaa@hZqj2-iqtFi=q{F-VOMTfU6+=YOfZ=(-?{ zz9z6>?Woi7a{BcP_hV5SQW{w#NCr-JLAHzV0i`lz=Og?_=G3@I11TZtc$)i!bsE{i z)5562u)?##=t5c3Q?r(#m|?-;)IpI*mq_MlAA;Y+#>B6JeSG=!pSd{s>lq^x@e*8F zGE%Z3zLptl4OJH1j53T~jEQ?K;~p%OxIWR9>InRJ+-&kdGmGs6mu;&j4TYMf z%nZ-G*B(M{dUwd*y7%m-;2C@;9CP%JrO+ep0A%PrDnN< zAJQo^zSe)Q`d0MQFTd>Km&g8Aia%ewfVYbWcB}O?!CdMtD_@JCLp5tEtC8-9wn;1f zEBC3}r!#gBmQ|;Gb9ebY<+Z8noW!~I;_A*O5AfD z_HUaijcS%M-#fn_cYJr=r{J2oYNl1$wY2)*+b`dq4Mi{f%PO^M*vej1k_>3dZ)%0r ztqirbYt@Vw0(gt~VknSAXIy4(OzaAsb~folOM)c7Fg(u`QquT$IjuZ?Il$gb7|wI` zuizgO$=r(JuC=DHiv%Y$g|*yLTnR3%S}wa^TP5Y$#o3Xui&A|pIHWx@KOY@cXZ{r1 zTG}Lf6mdj!YB1+@;0ismv)a!)5;^g^eG<5HDecVeP|j%;{rVJi7HUT5MX1l_#Li>f z(&2keSZN&R?$Vi6t7B|)R)1u3k~8nqFkN4}Q*pBBvJAJ(W|-&EeJArXHtTu&IWa4N zPu78M)7xEW=XNm1%_Gv|>i6gm=Js0W-ec`x<1AW((MaHO;5a71hqm_&B)(7j$Bw_l zohi8}Q_(6hmt%Ee$SBg2^@Zhx5ZcXjuw zpHP9_)@aIqRp6Y@|5bCYGIIIeF?n;LXRrIroAAlOlln^d-tP!87!yp{ExMVbn3=7J z9Tl2a1OR$)3zGcwLI=FZTSgIXd(q>szIs^@g>EI!eEn&|`ggP~U>pXp3iv7cy1OeizPlT4+qJq5IIAtW z*-IY%`B0x`zmJdS>Os3pN#daA^8${2rPy{sG#>g2gnK_lv>#3Q=dE)YWuYT)siXv8 zdZUp5a6l3O{2K-Qr`ri6{Vy#8WB|baiw^?;gxdh%{-dM(hW|;BH~7cq-xxMA41oA{ zg!2ZT-(dc$&GilTzjVx-3?Qy9DKGzq)ytA*uwQQ$P;Yu;3DWLO#Lqn z!8iIJHaj)wUn;J4!qhrSDj-QmXA2N78wVQ)wFn9b1p44?ZYiiHCG#Kiw7daw>o=^A;QVc@!?dIGGDR&XbaQLTK#c;`SUs;`mibrs@iMN zB_qVqE!P+e&AW#%5Q+>H^uDg8HF#yNp+&d~*3Vj7Kl^VX1hwa`dG<)Q@EV`@(_gXc zJ4|Y2LA9#36MhF6bDe3}>G-;MAMcOfwCa=k-}XPD-xtI-+!?^(-0wRskqrj++&8mt z1}Fj(cx!5E48p9*pl@p>Wj&l_IAIkbwEZU6gZ>LzBS{Y3FRupBH?Vr|Y#+)2;IUVD0Zi4!})0&g-LmX>t^_Aad1H=T5e zSX#&IMydR>{Nps9@>)ckU+yJLf;sSEwe*L5-13R)lmDaDbGhf{^4F*NE!SAZKnNOw zpO9#8Q*`vlyM%lCt)?t2k?$q6T;u%0ZGc(IsAiQjd5H(2%a*${hyY9&vsm>!eS6;}BD^9TU z-Qk5!P{Gk)`E~ZDef^*(O;;l_8=ZqnT@`@Bg~(Rpn~z;R)o<11r~bkxz$bb!i=i2S zm#5x!aK;QW9&WPaB{de3n}McerS-F`622ml{9&$SpXEz6&DPr@SXTzE@i%Oo+wYik zk|_stAa!Xt&Ohopie5jSO|$X-p58O*2w3yYANu8ixbXNoBwm8Sl@-HL4a9Z z%3@{U_)OK}h&0@sl(lBr4WyN8S=T!Sp zFUz~7JXwqIvsTrfTiJ=GJvzEkETa=B905+GjQqIjv?(#GKIQZ0u^aH7;H%1% zfF74*+Ajaf_7@AYIG)utGzT}lOu-_(v?^Dc`jE@_Uj_U{&3X~2Fq8+wlJsC;xEzazy_L9ZQl&}&^NZnmL@K!o%)AT%Di63vYT)DxtPD+ znwY$L_d06tTxSTJ@@g@jRpEf!)li}XzBI!*>lv+*@4{gR7-&|4FuXN|PJuX#WQeh+ z1r5@BVbUq|o^)I3yzi3aNqn@x*8xZT5hk=SD)oX^O7j9)GG!8Q-^lnfA+K}AtmPBE z?;?gp2UJGX(GV|Ob>J!`*Cc& z40Pa$%E*??`>PI)ike_vxCR}T`ownQ0Gp1wQF|LRbLB++B+?!}B;H&Abvb|NwW!lF1c7{OSl zoxM#So72y5f10(~i$vJFh!~3*q(S2M34#Q%6_8xXmaU8mUdZpz{&?1IYkODWG67Jc zLT(t)a1Z%G?tbDw9-xs#IKgm3kRPDS7_)PD z*-qzWv%N;GimNL8@)j@`M(=`fm@K9?=KvTFuK0Uc> zLdP!VeT(QA6_p=K*E5TeY_$Fn;yDeaMo0#1wmZ?n^6 z+>~N_P_OBr&Z&9fwGJs!22sWD(m^gE?-UG~R4-n)d~dE+Nv&WXr^B_`Boj?QTROxL z0?-*;I+Fm*dq~qsWcLrFmGSD(?gb+~ny)>x)+?(fmhr0#JJFFNuUy;82@XoIyi^^E z1RgVh{NuDcZFMyK!MFf0DDjXfXMzV040 zK3dT{F75RAov3cX6`V3+%L$EE8N!zk=r^u5X;oTmSe_y5T8N<}&970ipV>FnU2SI% zhSqv6thLMRo}xZ|4v$}$k*Z$$IhJ~QG_sb}j1@w*%l`VaF~DHmutzWa75u&jh+&4^3Cb%gbiTHe*^*w#{n zZ-J-hq&8h5eNetV^*5=ne|$u9ggeMqPt5O@zj)IT<9j+U*mgCtfeg9lz{$X)cBI{6 z<%@X2z|qU`vb$Jq3({&PUOg>)iKQH6n?h{-+#2-CdrbwCP7z#PgM#JtLP=YfbDnNAsxb zXFJ~Zf)_m+eM(bvDaPjMD6KwG4Sd2y$wWx9JaAEO)g>*_>3VVNS$MP-j6SlyVmFBtsXj#rNJdzj@FO#kfjMW2qd4HqxWK3HbGC zT{JTXvnFzf#z%p&V6Lt14$$C~{%je!vPEZ3?W5#_0sdh(ea2=db1$a!nb_jiWW9bI zjJdjVd~&2x^GVMAne!{9Bhgd!MGjrVw*?*juvn`VZ<%Z4YAzC>RD3^=sYJ3wdz##C zql~Z|e7IB6;{dzg*keMY2uFnN^RGW>=4OwRD{*nR5I-S7>vbFEW})`eg;tdXi>ksZ zyWqp`2<|5Dv-xsLjZR}PLMu;rBAqRQv31GHI8`b#s1vIPBcwVaE|W|)VgAgm0X48u zi;QY@)#s3&ql471AA4W^oZ50Xqh?k%_Jyc~4~2DP&%wRQ6(uy2H<=0v2uNU@{jIZm zS6xodXXU<-zMA@CQdy_;mpJYAw8PhY`48u)SKJv+H z6gfyW+2Chvs|ms}%`yd08}pDAI8<8>2r1hLKaADBn3R-1(iv_qT1%X!%1DG@L5&)Q zW#QCX=I3cTT@GfHDJT3c=2-)V#(KRVWgdK zrV+e{@dkBr%Lra-%|%wW7It9B_~_SdRXUr6Kd6gtb5f-nMWo=4TQ#QrLq7lQq}34l zC!Kx+yGC@7ZK7?vOi@ZOuP}52w$sPk#6!8G#dyH+Sk7%A#JN8pK7rZq#e5x5Vv$5H zCJ=E61swErOsB(2 zJj&;>1Cb0RD>B9jqKuMdsn}RMDNv{lJS8@?XCONlE6k3HoB2a0C7{5qNfAPFUo>?v z3vfE3TQ+J*M=v3k*WL%ft$nA9)IxGg(qbZ?Ce+(jWtX3{s)N(}ZyK-@LOS-)?yx7vwP z$~Y~TPMIk4MmAux)!dD$__9beOOt>$N?FAuN(O`CmSHhQTOFl=)7O;Qt^%&4CyN~> zc>eXtk2|Y4#-Ct?zoGaO;b5!`nYkaVvo}fvw*nO}G0QH-_w_yxC8uAZBay<(@piDKK7#EMPr4ssk zBEY4K=*u=sjyh^}dKbIZ;1|IG(paeP(%(KXL|91RT&ETX)KTT!LX7KIeYf>v#Usz* zOz0CpaS_RD@0Yfp__gj~1ht0=E0d&Uh{+h2gs5e5a9M|Dv>~19+=!RDc9K@t)5sAI z8EsTG)JNM-%>Q;oB$JhZTl|Dr*E4JsK30zT7~{?^02I#O+2rK?E}$vH&Mn=p8K8@| z_h9OPxoNO^@=5!44VRi=4~c4XQf`e1mPecvhN%KEg_3x!033KsfP|$10`_;Rqcg*) z6%H4u)1g(G8N$rHOOO@4I*PX#--E*i)lsa5V{1#KS;iXZyHT_%ZM{;`LLIv@_Fq7A9Y#7oeGqX)8_mBB)D{J4ZHP%^rK zQo#icad=mT)$!A6J{PW*cX>0829CmmOcNM~s#8mJLWtHeS}3s$Om4({@`Agp0^Yzm z&^#ZM9Va8f4hW_d(;zDv9>Rq}3#NUK3s)_Li%aG1zx4b`wOQoMk!+$$2L$QnoUU(+ zBO=kWh%G4Ov$C!2go|~(SRZrca)?2BxC(rGSCY?gGEVyglsPP#X7(C8{tT~GWwKZ9 zBq0^aPtTUwTwsgAsZ$9ITZGgf!_1Rl0Pe5fiJ&0`Zb0LRb&pv|fJK2{0nR^SPl+Ou zc7(;!Wq}~1NZukD#n?qy?mrudxP*ZW6AHrsgR=^`$r4)h?sow*T|>-i?p+9Cd)}PQ z=E^@||NLbI2^w4?iwM_-tqRIV`2RJdEPAKF6FZa*A(Pr)6r+)ybc0}k#Gp+Wx9dRa zHIrp3{=*xt5x$w>O2?S9_1aH|JiL6WcA0d%P?Duqgfy0Rcxo^UM1W-WFV8i(9*?LU zPDAFV)b`KpQR~wp&dM+XCVHL~cL?PJlsUVv$XwMA3IqDU$^L~TCNM5DJ=LUxZh6=V zLjtaqR&S>)K!wQbJbzB8GjlzuvHfu@W}=6B%C3#KPc$s60-ZNMRJVkTXU>R|C&M2}UmGBh4Q9ZN{{>hKO#WRMOsTTtpvNF?+9t7y6W!LBXB&{B-? z^Z^im!n9?*hod~$Ppp^`ODG)S5uToi@fkjWYf>&F>Bv-xKKT!&*Om|GMKaY5j;?Ce z9%zi_`XuPfkvz>KhI2q;ng=Sg)pweXpogGzcD0hFz8Jx-rwe$DaO!HK<#B(_hj})2>KORGzcvazQT{)Lw_efUst0{_ zoZPZtY$~TDXaexae~I|{2%v&E#iZG*HAxpTI>Qi-ak(M6^K{BLLxYKrN@xQFzK&rk2i~Gtl#UO}Yvl$HHocon>b4B@?3-_`nfx zIKX6(;ZDN59+gg#Hzewo71J)H!;%__7QoFQ+kIy54k7<}3L{Q~ zKL&^X9d54JvAr=~<`zb-bY6xO?8x(cGfWk4rwX#hCQ>5z@Xg@WL94UCInMm@IZYA2 z&d%|C{|jz7vzc9H7TEZ8{TM!3wDUe*vfV~{0*iI-hp8G~t4FJi%AVTCOGeXGn!!Ok`l(diwHA${; zNZNsY*Zj`XZO6wRS9c842CO)p+3HJmD6|tr9^CfKqAx@+-1N4SNJq1szlGCMkn(HL z?L~k^!-~x)6BG%@HDYlLnimNL8yfPg0U-_4f8_2aWQ(xH4_A~)0{bW}B&^XPBzlaq z@ge#`OX+B-lP*Fs3}ohQTOnC!ytm1z0!a7BtO#;kf*0?Z!jdoe3Q=Bu_+ zZJ#DB5FwXSF@bqYh|kb-W9ce>p;~6_4F+oSjPLVTjci15h>-btvXDbbI5lmp1C`P+ zq}}$U9AS^_cAHRn%IyR(P=u{6h^i5FpI9zn7C%{{Cz|7g-cK_z z{F%2qHgVKM*YNq|{CsK4hp&;x26Z!)$*vSZ7x^MxUm$nc6tL7tW4M<=VkFt23kiM= z_b%2uxxr=jNb`>5lBoUC_?0Nai~7DvrR?0)%Qp|nzvt_!`?o5RNqLS-)7J$)6fhQ< z>K%1V>{95a_zH^dv@ET<+S~!#FegZ>E_umMJ7J|G@(}Ahg-#mE&Nw8L(rE7xoev#{0_#fR&SoE29q5VyC85 z140j1?KsY-Zg#>24&J$htz!Z`0TFQSiXst|&G!synHEfv;v{|-O&+KrFEGCdTsAG$ zN%~d;+fGQReD^^e;@_eQmz}Hm1Hu(e+Fqsl(Srg)moyQ%@fG{VF*lj9zV3pw3v)h! z9%Ix96*Xv&Zz%JuZm}X{t6;V-Rz5FC2kp;dozRM1+-WNd>PFtC{tVZ(Pgk2U;$_C5 znHLluJH@*a1XLU%;@Dqh*d%NSM}|T*OT3qC>ZT5hV++8GcCM-ULbPOqNkwDCjVE4; zbowP_yHkiL{Sp+pU;$}V=XKdA3^hWPD1621mz|a=oqOj30ks37j-hIWYOP-)&fyn6 z2atO$%q!?PB4727Rd~Fys3(vNsQ5zHS^3i!-+ZpGXdy=WiFc@Qs^{5`B)nTo#0#2{ zS{3PZA7wt;?VF3IPINWdbSveR!>Z)aDXa@!s$KUKrUuhqZ{qXL%;X4HGu>!NP~;;5 zcSBBlmKkk1+-&EdT-Na4Pz0H#1OHc2<;&YzE^+LAdOBsspuXh~=>4(pV&59)u4(tSO_&I+B zXDKgdG`U(WdZL9|@&cnK?wzk?Bi?ei_U}aruQQg?h&@qePe| z@o-ko^iGV@#Iy6ZTUQNAh3WV6yv(*oItV4F!tarvG5g@hT@an9m}&AR=`&M{#j%umTSXc|4Or=L&C9B-NJmD z{6qSZ2whjv{4Zreo5AOhSfEOTB($oCm_8mu4I$+47hO*HnVjR0@9=g zAr?uc`^SMsKtUyw(hmexx!cE<8Cz*fI#3|L)Jz1(LF^(P05edt*RZ~LrN^8N7hm$j zlS5o*?}-mY)=(pt%iJ+ZE<&JKiEB~eSw7*%y=qGzu+Vl;G*ymeb@BsiW=FAm0}a%t z7(tYsk<5Hjx1Yr-w)+kSxpj#NNiG;DF?B@J4&SiuoJaG{P>4Eh*@W95D7#3rzagZP zj-&FI)^~Vh$T@LdzKeOTFRa=Q1}Lyejvw6b(7EviTQ1(^nxfUFDRY-t<8NR;;_ZduYqAr=En?VEZMq75iiRp5s{c*`+B0w>UBqUPGMRJsMXPe?oX&oto0(d z)LFnomJ}97fujX1p1j?Ylq$a@>8dhGzNeJ>CP`z-M`u(}c`)4D(553=A_+HgxZeAT z@NMbv^))`<&;S4+NBn2q&@D?o#(Y&XY})6K(S(b+mX_wHymXDX$m;{ z1{*pW8xKSyPZY99W^}T*B3gF!izkeUI@Kh)i3F+iq0*`jSxOG;XPfSwP$uc|Uj;S^ zJodfwE<$N;OLaFSpAA^w>$9b?Mel9yM?(od?!2d-r+N506D4y4ph~y2t#+T(tIl*2 z47he@J9B@bz^VjK0yu%vv&}>x)h@o+X78cQ*uhL+%W48(eXDI|7QW9oVAWgzH=Z?i zT`N{ERYqxDaBxYS_oTxjz`mh_l`w;q0M*=~QTiOz=w(hS_SZXFMBuIa`3>%b8#cl& z6eu9n2{TI+jOv4gd^N z1+2on&#SfP%~^Hjg@wUV{0QP=HI@tH=49MIn>*pH+WP&AQ23`2HaQ!g84IE1GpK*NhIRH7$=A4zWlUd`k+)NU79Zh)tL)joF)X&U%G zixte3(J`?vq{!@^ex{5XA?*u;CE!&{UbJPmW-=p-fT317>9mkhe5Ale*UR+*7x45g zjF>YuX;G)#G)U6~!X4{$u^>LL&`0RND!vIz9s4a-uQ)Vv8&k|6I!gR~eD1X<-2NU`2uLhZrx)CdN5=jTRe+1{ zQ(uglKRX?^+#c4^7k|V^AftOt>p~=Ot=(J z|Ljaqe9)o#s1NLKZLHG~cv0|fJd7>;mY|A>Bo-Gh=)N<|=^PS|WIpk>HfWD}s>)pN z0V&AT7Y}+bWAjUIMlOH~hmxBb!k=m*j7}XJQl=?M`YE_ds}gns_pRp_yV2Y0tXr)* zv`vNegCl^!lpGmOAZ&kkH|%3;QX6kk3UnWX3JbW$n22OasaV039+j0kQ6WR?iq*q7 z9JU1;6mK8!J;;Ct1BZu2nkI;WE-9Xj`|@yh_5fK@F2R{TmJwr`OPGLD!=91?9O!s?nZ=zMeN8Y-?KrGeE~RY7}wIGI#-2h+X0}*mCi+U7&5xRV=X}vPlGN@6TNlzk_n9O(j zkt5zsMj%MbHe|zA0~N%C5va?$+yXFdFMs@dIJ-pg0}<|OO|`UMgpH?Z8${{H%OrEw z?*Mex*8}dtMI<1v@xLfscRDnacnI@+*%xbfSPCg0;Oa-4QcL5iLu{aGsi9b%erN*AE zXyWg;CovN-y>wJ6T!BI2s`(HOTct>D&hHOt3}~n!fFlr-Of8SI;)>rnL(2hnAQ?IN zsx>zX2m#vHBq1_me+uYJbKpzf2$>C*@)c(x@&gJKuj0k#WIb|N_~XJ7h(%Mcolb1st=#D$B~g z{g`|SQ@)G)(K(Ri?QNm2C~iVQD#5rExyhC9FjWK558EdKO9c$7juE*hKYjnb zc8pUGQOJfcNY0j(-J0Og;CIFw7=RgemcLi6?);8@&a0L#a3 zCjh2riwlhCPq=L}bd z@ZxV^(UWtmkXC5AWU4kFZF(wMu2;Lw=pgsxAPVM@Q&M3etkSd)Oo4J*Wv=2B(&s8xTE#fy4br6s9_`NaG@1lJ{Trejtc!6;i z9_Kd$$yNJTH-Y+WwCd}U`WPLO3__noeh$O}18j`00a^ziWDp+Y%U-vAP z0#gdod(=*x8_C>?rdg)#kRkP*B6lu?F?~r;js^ob`5#8_be#@lOlyIE)~+;#h%0>N z@>_>)lemOoXxCzifnK(j0;92$#2`HX@DByC%0T(4UFU+N{mP39w0H#3P=^d3wSSCi_BUjQ5z#c!~03h7W#P8kZZ)Kc+78TQx0Jb%Uj z-^0)aBUy(y5?3WXHccox(z+4=S>Nz9Sk~NrCSayag@mhfIHo8STX`AjrkF58#AdT@ z%WT6-;$`NFNle;^gHwUnTg@$M{6_FpdY3t#zFinjY=+`H^O6g@BC38$$SMo86m0-ObzmsvRYG2ak>Ms&A`zj3=es#z2P>)Sxmqnvr zbi^=8*{Rk3vGJKld2~ngW@~>tijf_b8!eMS4Jl?wU*i$Q>OFtPUQ>{>oxGfZ7 z-e{<^MsnGLLcrjOtHg*ZS$t)8_KV(d0G!wZ;;EGe~1|j-cSFtlg~0xQfRwWR2Y zZSmrmn)|(c*H&dN-wmV3c~C`u!AYtyTbx{=VqW7fF5}g!Z2EScs~^AdJhMMLL(t@d zr%VX_>+BH!>A*c~qJ*Vhv}_>4AFNn0Inol5n8`49p{h5{VHlaK{$82$l_aG7_G$MU zc-HaxvT>-fu>U-!g^hg9zs$KgF`0x68#<`*cPL%IN=ZQMN(qP7tZ1=T`5tDpg3n&R z7fiaF$a`?KqfTO1>h8n~OW(-3XcyKSB9SN<>0JdYhWORKQD;sKX*~vo32KysFyx2H zTklqcjk7%qloDWnXk_o|Z5twnR-cbKcxdrcV4OQ5`FydVlkRGk#KKY(XXLcFI!W|r`+&mVC1$us<&v?Pt8y0#2mQ|rtFG-g0fA&Bg6bz7vbKOFC^~;qnMYP zXd)OR^rQ^Z&(>cQpQ63}B#tBll07w&HNO!*09@`Fe~o=cA~~ueFX?jmJ<*j zYCsQ>>@pVxMbn7AmTS7}$UZzBLp&Fikwd1t421wq{L_W3+2iKj`MSR2M(dHM!&;`o z@zcCY7LrZU6ygy9FyuGeKtnuUI_dHIX<0<946S&P@0{T(BgyYzv-K9euhV+(QA(Oy zlN>DhgosOJlW{}h$Y>UKb-w;kToX{?Kr#n@|FI?#)VKA}>mF~o(V4WcDSj@JJzv=k z+BI6St*0kqMu7>MZ;ohHDjE>j0mP!+Z?XkSuYHNNrjl_cg?Ogm|DXjr4Y=;XxF9()9dv!D6t7d8j|!McKpJGds)(Tc)c$Z za~87ThE{;4Y8a`~G|w~ztd%wt5vrE3gzt!gc|sD3w{@S# zA1^Ao+TAZ|seCr5P-pGGS-o9ff;)gL`@P{erSK<(#w0%Z|T!`~i2$kN)v4I*^ z)EnksG}yH#_g;(aUdRQ5UfuILqC(1?B?I)9h-6D{NW}sUsr|UQObR4l-sUyGcJPpa z;MAymy$k4_QOw1t$xJd0#aul}{jQ>0qY7)p+}V&n5`bnRmL)5k{xXnF$1g)q@wp!H zHBbM7l14GER;h&^fsf&>kl$4A)m&EprpC&VE|VN_QJr?Zn>Q`U<^vW?P?&HdmK$%# zKWkI8*`8w2B3KuGLVRxx2kCtcwu(v+)6>@<3HgGLkAGphQ&kMZK>CY+IP$jiSk! z@$74~LhLLuLU@>53Cqx)RzJPZf5tWfk4jKrfR~ZFMFS{_vT=*xPo$u`$8y0(GGs)Y z^?}m}vc?Ngj->;0&y^D5ard*LTikyAJ8&?Aq*~Zs|Jlw3kf5t-2@CnGCFuGCR?P}l zy>ykNFrhOXeAvy&;R>K5gqq!wTb&|Ir(a}_3L)V}7hy+<2>9SzIqsBouoYy3knZJ- zNwyuwlQ=!(Kp*nzeO#lzVEG2$-QwTyCpuJGKP^!TV?r??Xu7j3NxTxfGsAMB+lh-C z4a-aS;pU$fyiZsfe&-F{ZXJY}@`?y@ z($AV+ZBWI>@b{6-r5shh-w~&eiAo&AN1PRi&ZBUdSRQWbxQw?1*vQko{(a|RZEB`v zfJVfVR+FGY{|&vR^eTreYyUD=(cKeE-sy0#=aHlN)iW7TV^4 z>W7UrQPESUYUU46-7bmqu}=bz9ha;K?ZHel?o2IPzWVFA-JhF9$xZXM(SCN9W$Z1? z>M8d6MypX%5w~SfxC;cYH3_G1b7ss^*g@Z)oVn?W?Zt6*owt#R%_!y!mOm((eatkW zsu4HWdwn`!cbTDqw4-A!Ex)UhXTOLY*Bq*W{E7Ne4}(F~i=AS7Yn+tX9eR3iO6acd zLtkgbg|o^lpg5b^_y7Qbk)+mbpjZ1R7LeSjCA>7R&{6t$(V6 z`50~BVijm$?r+nK-5@}19)ufWkQx}N{fb98WvZn6ESnS*=lMB4QcatCY{hwHkGmn< zi+mg6JWcf)b1u-Zv`=#N>oM4+FfxQUK8~nsu2}w?r2)B8@@RF`Su+q@3RYJIoh6N% zf#@R={)Q48H8&7DCb$mss31YP(*jI4=;(K|GZay`Ua1UWB*BjOB?=ndMkl%`np{2hRBpeQZ!{sVv# z8fRsyduDoaNZasc9+{ND`vd+a?eD?@ILK_(s8S z^C%>$IO-T*VzZr;1G8hBO^*qf=3xgTZv8ZhSfwos^~#jk`RIbv!iA%hg;RdY!LpYe zQhiAB`72&L0h4F&_8$QoGv2~HnH9Hc&%gL`0Y?ppM@|AYq7dF(7*!kU*TdyaR4zk% zPG&I(YLrCJvb1msYQZvm<^?%RuU#IxkZ!B|FqjHV-{<@g{1K>_!_00!Bp2~yKoBkz zG?2(}^vt}+01ASOMUu%=^N2l!X6?hKSs_TWjZhUP)R^Zd^ggd0MCj?q>pia}<5=vh z%@8U9yuM6H6YNIqjZhoFU0+DxtTLK0&}b+e8kvU+mi@-rk^J0`$CJ#AE_agcdSr!P zBDknYEa`z1NoEa{Yl~=i2V|s))yai_pX67vdmQ37j1v-^OQ|x8emVb`$10;_o3oa9 z(Fr1e#{z%Z{~^KOt+&d^I>neERUAa1Yw5&O`z>~5IA)s|0mp=uu1o9x1pfp8`&)!q z)}Pv{+&1d~Q#WN(g=Yr|ekc&v2rwEKY>5}xtwsUp(_JG65NV_bh%^yEj)F$M@y9<` z95vaY)iN7Z*;mt9lz&JT)JqrG>gALhp8Tx6JO4PylCoumEGc1pDiK6j^W+XMV zA#>AbsaefSfT;r32k3ceOS{koj^YpwA_%T^8CNd!Aa-PR$rC_!ln$a}2O4Wkt^^dwUgw8^_|MWhqMJf8Ex{}$ zUY<}TFj5q7eFVQknkp7)|Fl}{DtqJ)uJh^`U85xW>gQi6I5xCn*9ct{rdS?-?G>H~ zRfUAmRGOSJ|0d@zIS<@WrUbJJ+7gI1VG>O^Xi#QEk4l;VaL2^U&(dNo4Z@>D+{Z4R#?SxaYef@ufUSvl>2Pv$xIirPa??2YMff>5wN`tt z$syOLO#XLjVTI?*z~Fa<1$BVY?|M&Zk_y|rL@WV_>|ToQn)tjJwfE?FSCI=1fOIjh z9wkbTiw(Nm5dG3;K3t^i(h_O`iT2Fl&MkWkw4$x2Z;qpxQbg`ShDF;0Ayks!{ocd_<9JF91GS z=BA}A|3;$#{Ep)P0nz03ykNBU<%F-bAs`8p)oXCDOAIgq(eovUF3t3m$r-zBo=U&y z12Px@qyMV{v+}L(8?PP{kd#9ytNB+TWgZcP128en*f&Gc+W!Z_+XI|ngu;6SqlZtP zAAO?b=qggfF2KmN^d9|-;!B00000NkvXXu0mjfaov3< literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..12881d31cbfc3853438bbc6bd325715beb8cef42 GIT binary patch literal 1682 zcmbV~eKgcr9LDdwzfc-4DXLi{GQBY=TPIdAsF2q}hph~YkeQgNyc|Oc$-UPlED~CNvO!w7%wF+)o&7hH8W;s>YUww_K)3t?zzvo_uTKj=lPuPRrji8VBS`m z2Mr()03b92s3l+vKq9o#I#Me%O6wRj8imGUu-I=CN5Ero1RNHN*TEBrT4`3gIz(OV zM*GP3t4ItAg(2dwxNjx@$J-g6K$9@~X12~)1D`!O^8`b?^ny~n#pbF;Bi~MFDRnP{ zLm()E`xM7QnE6vT_W2>KkK;X_@LBadN=@F5?L`5?XCML1i zhYn|C9?d#-JUi#q=`&}~6`n7;@au1vf4_3|T3Pw6+tvI#HFpKIO`>LTOY4J&ZC%~Z zp1rS}p{VrI{!a z8e?XQ)pz#6?MW~&w@b$xZZ0UTY9v@teIcX0j85YGrE3Q)I*lv~N71GZ@1st#n`FqVKbL&FzqLJP zs7J&#@rPDt+sEGt8oLlWxb}LH#LjtL|Aj?uA^TFAZkErL`6rL3$TNI;rb2|M2YG91 zeBU1&9w9dV)XFSTrt&MDn3m`c{uMRgyh?qxlXNjA&$y{+{eX1whJa*NIhe@aUvY?pg=xnqe{Z?dKomzR|Ar+G@lebb9!p@ITOKT(60B^F(LuBlHgfqvvF z_vS6&;YQoxwqQ7XQGW8F1;sOzrlGQ*$NhcEe3jX%>+z9%WnO7kg-u*S@XC%86EzD1 z+&@Z%^fXy^sTvIAdc7E$)t7wWeQ*xTyVLoFgZt@iBDTgV!lNvx3IZ z(#-rMXk>z}SPuIQMC3v--mraXz8*Vuj8b7I-In}tPTM@!j*BzO!9#X&uNaI2F+-K` zs$;D@1U%9p)q}z~$sY7;WqQ{=?W@a0k4_Va5aUMD1Mw~-SOP=j{4^-AAGTgS@loLk z-!?WufzV+#W|37PW`6tWVZoH20czc z$jXPZa7Lf)&^X%SMEp`L$y!rXpU8O}N!h0RwyQ?mZW=u^!wQWI(A_=iQ@m21io4L< z@H(O4^{br|g-Vi*OIYt8S!z&PtEf~lIYN^g*-`0MLhr4r0?n7QwyBv2Wii#jD0XHx zms4M~as{KE+ws0a4bW4tl_IiNGM;L~@w5;J@3uTxX%N+kvfja3;b+!RvPNX)fni93 zSO#I4eWARvF{w?@RMGsQiD}=~xa|Vs6DIfm-9G9pR5=VAe^ zSIs5`om^wrz${6~_&TMaratbUE2Mj8CXsRVi%ehcuULHL?)_&sb!&_Z_3|d&*`Ro? SAqD`<_Qg}H;uK`{%Rd1=_3V-W literal 0 HcmV?d00001 From a45bd08e92df8639a2c91031d96b9f6ade84c288 Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Thu, 14 Dec 2023 17:31:11 +0100 Subject: [PATCH 26/53] feat(145): distance to objects --- code/agent/src/agent/agent.py | 2 +- code/perception/launch/perception.launch | 12 ++++++------ code/perception/src/lidar_distance.py | 21 ++++++++++++++++++--- 3 files changed, 25 insertions(+), 10 deletions(-) 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/launch/perception.launch b/code/perception/launch/perception.launch index 3adc596e..e9f9b272 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -30,11 +30,11 @@ - + + - + --> @@ -74,7 +74,7 @@ - + diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 71e94d31..9b20ad0a 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -4,7 +4,8 @@ import numpy as np import lidar_filter_utility from sensor_msgs.msg import PointCloud2, Range - +from pylot_point_cloud import PointCloud +from numpy.linalg import inv class LidarDistance(): """ See doc/06_perception/03_lidar_distance_utility.md on @@ -53,6 +54,21 @@ def callback(self, data): lidar_filter_utility.remove_field_name(coordinates, 'intensity') .tolist() ) + + # camera: width -> 300, height -> 200, fov -> 100 + im = np.identity(3) + im[0, 2] = (300 - 1) / 2.0 + im[1, 2] = (200 - 1) / 2.0 + im[0, 0] = im[1, 1] = (300 - 1) / (2.0 * np.tan(100 * np.pi / 360.0)) + ex = np.identity(3) + #distance = coordinates_xyz[0][0] + point = np.array([20, 20, 1]) + + c = np.matmul(point, inv(im)) + c = np.matmul(c, inv(ex)) + + print(c) + distances = np.array( [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz]) @@ -87,11 +103,10 @@ def listener(self): 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() - if __name__ == '__main__': lidar_distance = LidarDistance() lidar_distance.listener() From b0e73880a619b656fda8f3da7034d0c9ce85f2a7 Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Thu, 14 Dec 2023 17:31:31 +0100 Subject: [PATCH 27/53] feat(145): distance of objects --- code/perception/src/lidar_distance.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 9b20ad0a..fe4e204f 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -4,9 +4,9 @@ import numpy as np import lidar_filter_utility from sensor_msgs.msg import PointCloud2, Range -from pylot_point_cloud import PointCloud from numpy.linalg import inv + class LidarDistance(): """ See doc/06_perception/03_lidar_distance_utility.md on how to configute this node @@ -61,14 +61,14 @@ def callback(self, data): im[1, 2] = (200 - 1) / 2.0 im[0, 0] = im[1, 1] = (300 - 1) / (2.0 * np.tan(100 * np.pi / 360.0)) ex = np.identity(3) - #distance = coordinates_xyz[0][0] + # distance = coordinates_xyz[0][0] point = np.array([20, 20, 1]) - + c = np.matmul(point, inv(im)) c = np.matmul(c, inv(ex)) print(c) - + distances = np.array( [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz]) @@ -103,10 +103,11 @@ def listener(self): 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() + if __name__ == '__main__': lidar_distance = LidarDistance() lidar_distance.listener() From 60979a56c7ee869e1f605f2df9ca3503c5459a6a Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Thu, 14 Dec 2023 18:26:11 +0100 Subject: [PATCH 28/53] feat: Change Publishers --- .../behaviours/behavior_speed.py | 68 +++++++++++++++++++ .../behavior_agent/behaviours/intersection.py | 60 +++++++--------- .../behavior_agent/behaviours/lane_change.py | 46 ++++++------- .../behavior_agent/behaviours/maneuvers.py | 13 ++++ .../local_planner/src/behavior_speed.py | 67 ++++++++++++++++++ 5 files changed, 195 insertions(+), 59 deletions(-) create mode 100755 code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py create mode 100755 code/planning/local_planner/src/behavior_speed.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py new file mode 100755 index 00000000..24a35eb1 --- /dev/null +++ b/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py @@ -0,0 +1,68 @@ + +from collections import namedtuple + + +def convert_to_ms(speed): + return speed / 3.6 + + +Behavior = namedtuple("Behavior", ("name", "speed")) + +# Change target_speed_pub to curr_behavior_pub + +# TODO: Cruise is in manuveurs -> ADD Publishers + +# 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/behavior_agent/src/behavior_agent/behaviours/intersection.py index b8d7e8db..22697f27 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/behavior_agent/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/behavior_agent/src/behavior_agent/behaviours/lane_change.py index d4b84d19..4433cc2e 100644 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py +++ b/code/planning/behavior_agent/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/behavior_agent/src/behavior_agent/behaviours/maneuvers.py index ce672563..5a17c25a 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/behavior_agent/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/local_planner/src/behavior_speed.py b/code/planning/local_planner/src/behavior_speed.py new file mode 100755 index 00000000..5af18e4a --- /dev/null +++ b/code/planning/local_planner/src/behavior_speed.py @@ -0,0 +1,67 @@ + +from collections import namedtuple + + +def convert_to_ms(speed): + return speed / 3.6 + + +Behavior = namedtuple("Behavior", ("name", "speed")) + +# Change target_speed_pub to curr_behavior_pub + +# TODO: Cruise is in manuveurs -> ADD Publishers + +# 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", -1) + +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)) + +# No Traffic Light or Sign -> stop dynamically at Stopline +lc_app_blocked = Behavior("lc_app_blocked", -1) + +# 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) From a3a38151aa5870e1dffeae88453555784044ccc6 Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Thu, 14 Dec 2023 18:26:53 +0100 Subject: [PATCH 29/53] feat: Add Node for Motion Planning --- .../launch/global_planner.launch | 6 +- .../local_planner/launch/local_planner.launch | 5 + .../local_planner/src/motion_planning.py | 209 ++++++++++++++++++ 3 files changed, 217 insertions(+), 3 deletions(-) create mode 100755 code/planning/local_planner/src/motion_planning.py diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index d26b4873..6a13825f 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -1,12 +1,12 @@ - + --> diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index c469c05e..4819306e 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -3,4 +3,9 @@ + + + + + diff --git a/code/planning/local_planner/src/motion_planning.py b/code/planning/local_planner/src/motion_planning.py new file mode 100755 index 00000000..dde1e9b0 --- /dev/null +++ b/code/planning/local_planner/src/motion_planning.py @@ -0,0 +1,209 @@ +#!/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.logdebug("MotionPlanning started") + + 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) + + def update_target_speed(self): + be_speed = self.get_speed_by_behavior(self.__curr_behavior) + + self.target_speed = min(be_speed, self.__acc_speed) + self.velocity_pub.publish(self.target_speed) + self.logerr(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 + # self.logerr(self.__curr_behavior) + + 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.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() From 22b1a60f656358a797ecdee1cd7a2affd3fd6d08 Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Fri, 15 Dec 2023 16:08:18 +0100 Subject: [PATCH 30/53] fix: small adjustments --- .../src/behavior_agent/behaviours/behavior_speed.py | 4 +--- code/planning/local_planner/src/behavior_speed.py | 11 +++++------ code/planning/local_planner/src/motion_planning.py | 10 ++++------ 3 files changed, 10 insertions(+), 15 deletions(-) diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py index 24a35eb1..4eb9cf47 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py @@ -8,9 +8,7 @@ def convert_to_ms(speed): Behavior = namedtuple("Behavior", ("name", "speed")) -# Change target_speed_pub to curr_behavior_pub - -# TODO: Cruise is in manuveurs -> ADD Publishers +# Changed target_speed_pub to curr_behavior_pub # Intersection - Behaviors diff --git a/code/planning/local_planner/src/behavior_speed.py b/code/planning/local_planner/src/behavior_speed.py index 5af18e4a..4eb9cf47 100755 --- a/code/planning/local_planner/src/behavior_speed.py +++ b/code/planning/local_planner/src/behavior_speed.py @@ -8,9 +8,7 @@ def convert_to_ms(speed): Behavior = namedtuple("Behavior", ("name", "speed")) -# Change target_speed_pub to curr_behavior_pub - -# TODO: Cruise is in manuveurs -> ADD Publishers +# Changed target_speed_pub to curr_behavior_pub # Intersection - Behaviors @@ -19,7 +17,7 @@ def convert_to_ms(speed): 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", -1) +int_app_no_sign = Behavior("int_app_no_sign", -2) int_app_green = Behavior("int_app_green", convert_to_ms(30.0)) @@ -46,8 +44,9 @@ def convert_to_ms(speed): lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0)) -# No Traffic Light or Sign -> stop dynamically at Stopline -lc_app_blocked = Behavior("lc_app_blocked", -1) + +# TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128) +lc_app_blocked = Behavior("lc_app_blocked", 0.5) # Wait diff --git a/code/planning/local_planner/src/motion_planning.py b/code/planning/local_planner/src/motion_planning.py index dde1e9b0..7b1e5982 100755 --- a/code/planning/local_planner/src/motion_planning.py +++ b/code/planning/local_planner/src/motion_planning.py @@ -73,19 +73,17 @@ def __init__(self): f"/paf/{self.role_name}/target_velocity", qos_profile=1) - def update_target_speed(self): - be_speed = self.get_speed_by_behavior(self.__curr_behavior) + def update_target_speed(self, acc_speed, behavior): + be_speed = self.get_speed_by_behavior(behavior) - self.target_speed = min(be_speed, self.__acc_speed) + self.target_speed = min(be_speed, acc_speed) self.velocity_pub.publish(self.target_speed) - self.logerr(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 - # self.logerr(self.__curr_behavior) def __set_stopline(self, data: Waypoint) -> float: if data is not None: @@ -187,7 +185,7 @@ def run(self): """ def loop(timer_event=None): - self.update_target_speed() + self.update_target_speed(self.__acc_speed, self.__curr_behavior) self.new_timer(self.control_loop_rate, loop) self.spin() From add8190cad6d7167bac8c7c628bdadfb6909a6ff Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 15 Dec 2023 16:31:12 +0100 Subject: [PATCH 31/53] feat: test cases and test node --- .../local_planner/launch/local_planner.launch | 2 +- code/planning/local_planner/src/ACC.py | 8 +-- .../local_planner/src/collision_check.py | 67 +++++++++-------- .../src/dev_collision_publisher.py | 72 +++++++++---------- doc/07_planning/local_planning_testcase.md | 29 ++++++++ 5 files changed, 109 insertions(+), 69 deletions(-) create mode 100644 doc/07_planning/local_planning_testcase.md diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index 9aa9f4de..955f3d44 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -9,6 +9,6 @@ - + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 679b5566..b05b3a8e 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -22,7 +22,7 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) self.current_speed = 50 / 3.6 # m/ss - self.logdebug("ACC started") + self.logerr("ACC started") # TODO: Add Subscriber for Obsdacle from Collision Check self.collision_sub = self.new_subscription( Float32MultiArray, @@ -122,7 +122,7 @@ def calculate_safe_speed(self): safe_speed = self.obstacle[1] * (self.obstacle[0] / safety_distance) - self.logdebug("Safe Speed: " + str(safe_speed)) + self.logerr("Safe Speed: " + str(safe_speed)) return safe_speed else: # If safety distance is reached, drive with same speed as @@ -130,8 +130,8 @@ def calculate_safe_speed(self): # TODO: # Incooperate overtaking -> # Communicate with decision tree about overtaking - self.logdebug("saftey distance gooood; Speed from obstacle: " + - str(self.obstacle[1])) + self.logerr("saftey distance gooood; Speed from obstacle: " + + str(self.obstacle[1])) return self.obstacle[1] def __get_current_velocity(self, data: CarlaSpeedometer): diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index aaf53330..a2d67f6e 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -1,15 +1,16 @@ #!/usr/bin/env python -import rospy +# 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 geometry_msgs.msg import PoseStamped -from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo +# from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool +import time class CollisionCheck(CompatibleNode): @@ -24,14 +25,14 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) self.current_speed = 50 / 3.6 # m/ss # TODO: Add Subscriber for Speed and Obstacles - self.logdebug("CollisionCheck started") + self.logerr("CollisionCheck started") # self.obstacle_sub: Subscriber = self.new_subscription( # ) # Subscriber for current speed self.velocity_sub: Subscriber = self.new_subscription( - CarlaSpeedometer, - f"/carla/{self.role_name}/Speed", + Float32, # CarlaSpeedometer # f"/carla/{self.role_name}/Speed" + f"/paf/{self.role_name}/test_speed", self.__get_current_velocity, qos_profile=1) # Subscriber for current position @@ -43,7 +44,7 @@ def __init__(self): # Subscriber for lidar distance self.lidar_dist = self.new_subscription( Float32, - f"/carla/{self.role_name}/lidar_dist", + f"/carla/{self.role_name}/lidar_dist_dev", self.calculate_obstacle_speed, qos_profile=1) # Publisher for emergency stop @@ -61,54 +62,64 @@ def __init__(self): self.__object_last_position: tuple = None self._current_position: tuple = None - def calculate_obstacle_speed(self, new_position: Float32): + def calculate_obstacle_speed(self, new_dist: Float32): """Caluclate the speed of the obstacle in front of the ego vehicle based on the distance between to timestamps Args: new_position (Float32): new position received from the lidar """ + # Check if current speed from vehicle is not None + if self.__current_velocity is None: + self.logerr("Current Speed is None") + return # Check if this is the first time the callback is called + self.logerr("distance recieved: " + str(new_dist.data)) if self.__object_last_position is None and \ - new_position.data is not np.inf: - self.__object_last_position = (rospy.get_rostime(), - new_position.data) + new_dist.data is not np.inf: + self.__object_last_position = (time.time(), + new_dist.data) + self.logerr("First Position") return - # If speed is np.inf no car is in front - if new_position.data is np.inf: + # If distance is np.inf no car is in front + if new_dist.data is np.inf: self.__object_last_position = None return # Check if too much time has passed since last position update if self.__object_last_position[0] + \ - rospy.Duration(0.5) < rospy.get_rostime(): - self.__object_last_position = None + 0.5 < time.time(): + self.__object_last_position = (time.time(), + new_dist.data) + self.logerr("Time difference too big") return # Calculate time since last position update - current_time = rospy.get_rostime() + current_time = time.time() time_difference = current_time-self.__object_last_position[0] # Calculate distance (in m) - distance = new_position.data - self.__object_last_position[1] + distance = new_dist.data - self.__object_last_position[1] # Speed is distance/time (m/s) + self.logerr("Time Difference: " + str(time_difference)) + self.logerr("Distance: " + str(distance)) relative_speed = distance/time_difference + self.logerr("Relative Speed: " + str(relative_speed)) + self.logerr("Current Speed: " + str(self.__current_velocity)) speed = self.__current_velocity + relative_speed - self.logdebug("Relative Speed: " + str(relative_speed)) - self.logdebug("Speed: " + str(speed)) + self.logerr("Speed: " + str(speed)) # Check for crash self.check_crash((distance, speed)) - self.__object_last_position = (current_time, - self._current_position[1]) + self.__object_last_position = (current_time, new_dist.data) - def __get_current_velocity(self, data: CarlaSpeedometer): + def __get_current_velocity(self, data: Float32): """Saves current velocity of the ego vehicle Args: data (CarlaSpeedometer): Message from carla with current speed """ - self.__current_velocity = float(data.speed) + self.__current_velocity = float(data.data) def __current_position_callback(self, data: PoseStamped): """Saves current position of the ego vehicle @@ -141,7 +152,7 @@ def meters_to_collision(self, obstacle_speed, distance): float: distance (in meters) until collision with obstacle in front """ return self.time_to_collision(obstacle_speed, distance) * \ - self.self.__current_velocity + self.__current_velocity def calculate_rule_of_thumb(self, emergency): """Calculates the rule of thumb as approximation @@ -174,25 +185,25 @@ def check_crash(self, obstacle): 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) + emergency_distance2 = self.calculate_rule_of_thumb(False) if collision_time > 0: if distance < emergency_distance2: # Initiate emergency brake self.emergency_pub.publish(True) - self.logdebug("Emergency Brake") + self.logerr("Emergency Brake") return # When no emergency brake is needed publish collision distance for # ACC and Behaviour tree data = Float32MultiArray(data=[collision_meter, obstacle_speed]) self.collision_pub.publish(data) - self.logdebug("Collision Distance: " + str(collision_meter)) + self.logerr("Collision Distance: " + str(collision_meter)) # print(f"Safe Distance Thumb: {safe_distance2:.2f}") else: # If no collision is ahead publish np.Inf data = Float32MultiArray(data=[np.Inf, -1]) - self.collision_pub(data) - self.logdebug("No Collision ahead") + self.collision_pub.publish(data) + self.logerr("No Collision ahead") def run(self): """ diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/local_planner/src/dev_collision_publisher.py index c0a8a620..f99e73b9 100755 --- a/code/planning/local_planner/src/dev_collision_publisher.py +++ b/code/planning/local_planner/src/dev_collision_publisher.py @@ -9,7 +9,6 @@ # 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 @@ -23,43 +22,56 @@ 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=Float32, - topic='/paf/' + self.role_name + '/lidar_dist', + topic='/carla/' + self.role_name + '/lidar_dist_dev', qos_profile=1) - self.pub_throttle = self.new_publisher( + self.pub_test_speed = self.new_publisher( msg_type=Float32, - topic='/paf/' + self.role_name + '/throttle', + topic='/paf/' + self.role_name + '/test_speed', qos_profile=1) - self.sub_ACC = self.new_subscription( msg_type=Float32, topic='/paf/' + self.role_name + '/ACC', callback=self.callback_ACC, qos_profile=1) - self.logdebug("DevCollisionCheck started") + self.sub_manual = self.new_subscription( + msg_type=Float32, + topic='/paf/' + self.role_name + '/manual', + callback=self.callback_manual, + qos_profile=1) + self.logerr("DevCollisionCheck started") self.last_position_update = None self.simulated_speed = 12 # m/s self.distance_to_collision = 0 self.current_speed = 0 - self.velocity_sub = self.new_subscription( - CarlaSpeedometer, - f"/carla/{self.role_name}/Speed", - self.__get_current_velocity, - qos_profile=1) + self.manual_start = True + self.acc_activated = False - def callback_ACC(self, msg: Float32): - self.logdebug("ACC: " + str(msg.data)) + def callback_manual(self, msg: Float32): + if self.manual_start: + self.logerr("Manual start") + self.manual_start = False + self.pub_lidar.publish(Float32(data=25)) + time.sleep(0.2) + self.pub_lidar.publish(Float32(data=25)) + time.sleep(0.2) + self.pub_lidar.publish(Float32(data=22)) + time.sleep(0.2) + self.pub_lidar.publish(Float32(data=20)) + time.sleep(0.2) + self.pub_lidar.publish(Float32(data=20)) + time.sleep(0.2) + self.pub_lidar.publish(Float32(data=20)) - def __get_current_velocity(self, msg: CarlaSpeedometer): - """ - Callback for current velocity - :param msg: - :return: - """ - self.current_speed = msg.speed + 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): """ @@ -67,22 +79,10 @@ def run(self): :return: """ def loop(timer_event=None): - while self.current_speed < 15: - self.pub_throttle.publish(0.7) - self.pub_throttle.publish(0.4) - - self.pub_collision.publish(30) - time.sleep(0.3) - self.pub_collision.publish(28) - time.sleep(0.3) - self.pub_collision.publish(26) - time.sleep(0.3) - self.pub_collision.publish(24) - time.sleep(0.3) - self.pub_collision.publish(22) - time.sleep(0.3) - self.pub_collision.publish(20) - + if self.acc_activated is False: + self.pub_test_speed.publish(Float32(data=13.8889)) + else: + self.pub_test_speed.publish(Float32(data=self.current_speed)) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/doc/07_planning/local_planning_testcase.md b/doc/07_planning/local_planning_testcase.md new file mode 100644 index 00000000..68bcc2a7 --- /dev/null +++ b/doc/07_planning/local_planning_testcase.md @@ -0,0 +1,29 @@ +# Preplanning + +**Summary:** Definition of test cases for collision check and local planning + +--- + +## Author + +Samuel Kühnel + +## Date + +15.12.2023 + +## Test cases + +### Car with same speed in front + +A car drives with the same speed and the safety distance is kept. + +**Speed**: 50 km/h +**Distance**: 25 m (Rule of thumb) + +### Car with lower speed in front + +A car with lower speed is in front -> decreasing distance + +**Speed**: 50 km/h +**Distance**: 25 m (Rule of thumb) -> decreases as car in front is slower From 64a90cc7d8c673a86aae83801a8801ccf95cbe9d Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Fri, 15 Dec 2023 18:27:45 +0100 Subject: [PATCH 32/53] feat(145): distance to objects --- build/docker-compose.yml | 2 +- code/perception/launch/perception.launch | 6 ++-- code/perception/src/lidar_distance.py | 36 +++++++++++++++++++++--- 3 files changed, 36 insertions(+), 8 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1545f945..22176565 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -21,7 +21,7 @@ services: # based on https://github.com/ll7/paf21-1/blob/master/scenarios/docker-carla-sim-compose.yml carla-simulator: - command: /bin/bash CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound + command: /bin/bash CarlaUE4.sh -quality-level=Low -world-port=2000 -resx=800 -resy=600 -nosound # Use this image version to enable instance segmentation cameras: (it does not match the leaderboard version) # image: carlasim/carla:0.9.14 image: ghcr.io/una-auxme/paf23:leaderboard-2.0 diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index e9f9b272..ea11f397 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -30,11 +30,11 @@ - diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index befb6ca8..cde7195a 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -23,7 +23,7 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) self.current_speed = 50 / 3.6 # m/ss - self.logerr("ACC started") + self.loginfo("ACC started") # TODO: Add Subscriber for Obsdacle from Collision Check self.collision_sub = self.new_subscription( Float32MultiArray, @@ -33,8 +33,8 @@ def __init__(self): # Get current speed self.velocity_sub: Subscriber = self.new_subscription( - Float32, - f"/paf/{self.role_name}/test_speed", + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", self.__get_current_velocity, qos_profile=1) @@ -90,7 +90,6 @@ def __get_collision(self, data: Float32MultiArray): if np.isinf(data.data[0]): # No collision ahead self.collision_ahead = False - self.logerr("No Collision ahead -> ACC") else: # Collision ahead self.collision_ahead = True @@ -113,7 +112,6 @@ def calculate_safe_speed(self): # Calculate safety distance safety_distance = CollisionCheck.calculate_rule_of_thumb( False, self.__current_velocity) - self.logerr("Safety Distance: " + str(safety_distance)) if self.obstacle[0] < safety_distance: # If safety distance is reached, we want to reduce the speed to # meet the desired distance @@ -124,7 +122,6 @@ def calculate_safe_speed(self): safe_speed = self.obstacle[1] * (self.obstacle[0] / safety_distance) - self.logerr("Safe Speed: " + str(safe_speed)) return safe_speed else: # If safety distance is reached, drive with same speed as @@ -132,8 +129,6 @@ def calculate_safe_speed(self): # TODO: # Incooperate overtaking -> # Communicate with decision tree about overtaking - self.logerr("saftey distance good; Speed from obstacle: " + - str(self.obstacle[1])) return self.obstacle[1] def __get_current_velocity(self, data: CarlaSpeedometer): @@ -142,7 +137,7 @@ def __get_current_velocity(self, data: CarlaSpeedometer): Args: data (CarlaSpeedometer): _description_ """ - self.__current_velocity = float(data.data) + self.__current_velocity = float(data.speed) def __set_trajectory(self, data: Path): """Recieve trajectory from global planner diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 0aa3a9f1..db4e6add 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -6,7 +6,7 @@ from ros_compatibility.node import CompatibleNode from rospy import Subscriber from geometry_msgs.msg import PoseStamped -# from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo +from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool @@ -25,14 +25,14 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) # self.current_speed = 50 / 3.6 # m/ss # TODO: Add Subscriber for Speed and Obstacles - self.logerr("CollisionCheck started") + self.loginfo("CollisionCheck started") # self.obstacle_sub: Subscriber = self.new_subscription( # ) # Subscriber for current speed self.velocity_sub: Subscriber = self.new_subscription( - Float32, # CarlaSpeedometer # f"/carla/{self.role_name}/Speed" - f"/paf/{self.role_name}/test_speed", + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", self.__get_current_velocity, qos_profile=1) # Subscriber for current position @@ -44,7 +44,7 @@ def __init__(self): # Subscriber for lidar distance self.lidar_dist = self.new_subscription( Float32, - f"/carla/{self.role_name}/lidar_dist_dev", + f"/carla/{self.role_name}/lidar_dist_dev", # TODO: Change to lidar topic self.calculate_obstacle_speed, qos_profile=1) # Publisher for emergency stop @@ -71,15 +71,12 @@ def calculate_obstacle_speed(self, new_dist: Float32): """ # Check if current speed from vehicle is not None if self.__current_velocity is None: - self.logerr("Current Speed is None") return # Check if this is the first time the callback is called - self.logerr("distance recieved: " + str(new_dist.data)) if self.__object_last_position is None and \ np.isinf(new_dist.data) is not True: self.__object_last_position = (time.time(), new_dist.data) - self.logerr("First Position") return # If distance is np.inf no car is in front @@ -87,14 +84,12 @@ def calculate_obstacle_speed(self, new_dist: Float32): self.__object_last_position = None data = Float32MultiArray(data=[np.Inf, -1]) self.collision_pub.publish(data) - self.logerr("No car in front: dist recieved is inf") return # Check if too much time has passed since last position update if self.__object_last_position[0] + \ 0.5 < time.time(): self.__object_last_position = (time.time(), new_dist.data) - self.logerr("Time difference too big") return # Calculate time since last position update current_time = time.time() @@ -104,25 +99,20 @@ def calculate_obstacle_speed(self, new_dist: Float32): distance = new_dist.data - self.__object_last_position[1] # Speed is distance/time (m/s) - self.logerr("Time Difference: " + str(time_difference)) - self.logerr("Distance difference: " + str(distance)) relative_speed = distance/time_difference - self.logerr("Relative Speed: " + str(relative_speed)) - self.logerr("Current Speed: " + str(self.__current_velocity)) speed = self.__current_velocity + relative_speed - self.logerr("Speed: " + str(speed)) # Check for crash self.check_crash((new_dist.data, speed)) self.__object_last_position = (current_time, new_dist.data) - def __get_current_velocity(self, data: Float32): + def __get_current_velocity(self, data: CarlaSpeedometer,): """Saves current velocity of the ego vehicle Args: data (CarlaSpeedometer): Message from carla with current speed """ - self.__current_velocity = float(data.data) + self.__current_velocity = float(data.speed) def __current_position_callback(self, data: PoseStamped): """Saves current position of the ego vehicle @@ -189,28 +179,23 @@ def check_crash(self, obstacle): collision_time = self.time_to_collision(obstacle_speed, distance) # collision_meter = self.meters_to_collision(obstacle_speed, distance) - self.logerr("Collision Time: " + str(collision_time)) # safe_distance2 = self.calculate_rule_of_thumb(False) emergency_distance2 = self.calculate_rule_of_thumb( True, self.__current_velocity) - self.logerr("Emergency Distance: " + str(emergency_distance2)) if collision_time > 0: if distance < emergency_distance2: # Initiate emergency brake self.emergency_pub.publish(True) - self.logerr("Emergency Brake") return # When no emergency brake is needed publish collision distance for # ACC and Behaviour tree data = Float32MultiArray(data=[distance, obstacle_speed]) self.collision_pub.publish(data) - self.logerr("Collision published") # print(f"Safe Distance Thumb: {safe_distance2:.2f}") else: # If no collision is ahead publish np.Inf data = Float32MultiArray(data=[np.Inf, obstacle_speed]) self.collision_pub.publish(data) - self.logerr("No Collision ahead") def run(self): """ diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/local_planner/src/dev_collision_publisher.py index fe8c3368..65fb1c2e 100755 --- a/code/planning/local_planner/src/dev_collision_publisher.py +++ b/code/planning/local_planner/src/dev_collision_publisher.py @@ -44,7 +44,7 @@ def __init__(self): topic='/paf/' + self.role_name + '/manual', callback=self.callback_manual, qos_profile=1) - self.logerr("DevCollisionCheck started") + self.loginfo("DevCollisionCheck started") self.last_position_update = None self.simulated_speed = 12 # m/s self.distance_to_collision = 0 @@ -54,7 +54,6 @@ def __init__(self): def callback_manual(self, msg: Float32): if self.manual_start: - self.logerr("Manual start") self.manual_start = False self.pub_lidar.publish(Float32(data=25)) time.sleep(0.2) From 5376bbc83bea0f9bf1a7a82ba4bf92523a28874e Mon Sep 17 00:00:00 2001 From: JMil <85188404+JuliusMiller@users.noreply.github.com> Date: Sun, 17 Dec 2023 16:56:15 +0100 Subject: [PATCH 38/53] docs: Add documentation for motion planning --- doc/07_planning/motion_planning.md | 56 ++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 doc/07_planning/motion_planning.md 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) From 7f9c51f75baa19d27d4a03ff2339c1d318e03c72 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 17 Dec 2023 18:22:36 +0100 Subject: [PATCH 39/53] fix: linter error --- code/planning/local_planner/src/collision_check.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index db4e6add..082e257e 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -42,9 +42,10 @@ def __init__(self): callback=self.__current_position_callback, qos_profile=1) # Subscriber for lidar distance + # TODO: Change to real lidar distance self.lidar_dist = self.new_subscription( Float32, - f"/carla/{self.role_name}/lidar_dist_dev", # TODO: Change to lidar topic + f"/carla/{self.role_name}/lidar_dist_dev", self.calculate_obstacle_speed, qos_profile=1) # Publisher for emergency stop From c17713e6f7b44518fdca1abb68cdd94a380babbc Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 17 Dec 2023 18:27:38 +0100 Subject: [PATCH 40/53] feat: no ACC when emergency --- code/planning/local_planner/src/ACC.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index cde7195a..445ec998 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -8,7 +8,7 @@ from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo from nav_msgs.msg import Path # from std_msgs.msg import String -from std_msgs.msg import Float32MultiArray, Float32 +from std_msgs.msg import Float32MultiArray, Float32, Bool from collision_check import CollisionCheck @@ -55,6 +55,12 @@ def __init__(self): # Get current position to determine current speed limit self.current_pos_sub: Subscriber = self.new_subscription( msg_type=PoseStamped, + topic="/paf/" + self.role_name + "/emergency", + callback=self.emergency_callback, + qos_profile=1) + + self.emergency_sub: Subscriber = self.new_subscription( + msg_type=Bool, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, qos_profile=1) @@ -80,6 +86,16 @@ def __init__(self): # Current speed limit self.speed_limit: float = None # m/s + def emergency_callback(self, data: Bool): + """Callback for emergency stop + Turn of ACC when emergency stop is triggered + + Args: + data (Bool): Emergency stop + """ + if data.data is True: + self.collision_ahead = True + def __get_collision(self, data: Float32MultiArray): """Check if collision is ahead From 37fbe5ec5909a19c5f09076154c2683a3a08937d Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Sun, 17 Dec 2023 18:35:38 +0100 Subject: [PATCH 41/53] fix: remove unused subscriber --- .../local_planner/src/collision_check.py | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 0aa3a9f1..c2916c19 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -5,7 +5,6 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Subscriber -from geometry_msgs.msg import PoseStamped # from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray @@ -35,12 +34,6 @@ def __init__(self): f"/paf/{self.role_name}/test_speed", self.__get_current_velocity, qos_profile=1) - # Subscriber for current position - 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) # Subscriber for lidar distance self.lidar_dist = self.new_subscription( Float32, @@ -60,7 +53,6 @@ def __init__(self): # Variables to save vehicle data self.__current_velocity: float = None self.__object_last_position: tuple = None - self._current_position: tuple = None def calculate_obstacle_speed(self, new_dist: Float32): """Caluclate the speed of the obstacle in front of the ego vehicle @@ -124,14 +116,6 @@ def __get_current_velocity(self, data: Float32): """ self.__current_velocity = float(data.data) - def __current_position_callback(self, data: PoseStamped): - """Saves current position of the ego vehicle - - Args: - data (PoseStamped): Message from Perception with current position - """ - self._current_position = (data.pose.position.x, data.pose.position.y) - def time_to_collision(self, obstacle_speed, distance): """calculates the time to collision with the obstacle in front From e394ad002527a906ff63b4e2613ee3d5bb5ac2f6 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Sun, 17 Dec 2023 19:36:37 +0100 Subject: [PATCH 42/53] doc: ACC documentation added --- code/planning/local_planner/src/ACC.py | 2 +- doc/07_planning/local_planning.md | 103 +++++++++++++++++++++++++ 2 files changed, 104 insertions(+), 1 deletion(-) create mode 100644 doc/07_planning/local_planning.md diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 445ec998..44fad046 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -58,7 +58,7 @@ def __init__(self): topic="/paf/" + self.role_name + "/emergency", callback=self.emergency_callback, qos_profile=1) - + self.emergency_sub: Subscriber = self.new_subscription( msg_type=Bool, topic="/paf/" + self.role_name + "/current_pos", diff --git a/doc/07_planning/local_planning.md b/doc/07_planning/local_planning.md new file mode 100644 index 00000000..3027498f --- /dev/null +++ b/doc/07_planning/local_planning.md @@ -0,0 +1,103 @@ +# 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. From f0f208d882927b8542db05c793a0c8c2e4ba1f25 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Sun, 17 Dec 2023 19:38:51 +0100 Subject: [PATCH 43/53] Update local_planning.md --- doc/07_planning/local_planning.md | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/07_planning/local_planning.md b/doc/07_planning/local_planning.md index 3027498f..cefa0a5e 100644 --- a/doc/07_planning/local_planning.md +++ b/doc/07_planning/local_planning.md @@ -63,6 +63,7 @@ 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} $$ From 868885e897534cf1270dc0c04a7b7e1e98002a46 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Sun, 17 Dec 2023 19:39:59 +0100 Subject: [PATCH 44/53] doc: removed old file --- doc/07_planning/local_planning_testcase.md | 29 ---------------------- 1 file changed, 29 deletions(-) delete mode 100644 doc/07_planning/local_planning_testcase.md diff --git a/doc/07_planning/local_planning_testcase.md b/doc/07_planning/local_planning_testcase.md deleted file mode 100644 index 68bcc2a7..00000000 --- a/doc/07_planning/local_planning_testcase.md +++ /dev/null @@ -1,29 +0,0 @@ -# Preplanning - -**Summary:** Definition of test cases for collision check and local planning - ---- - -## Author - -Samuel Kühnel - -## Date - -15.12.2023 - -## Test cases - -### Car with same speed in front - -A car drives with the same speed and the safety distance is kept. - -**Speed**: 50 km/h -**Distance**: 25 m (Rule of thumb) - -### Car with lower speed in front - -A car with lower speed is in front -> decreasing distance - -**Speed**: 50 km/h -**Distance**: 25 m (Rule of thumb) -> decreases as car in front is slower From d4332a232c3ad1fae4751ca036a01182dd045a66 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Mon, 18 Dec 2023 17:35:10 +0100 Subject: [PATCH 45/53] feat: switched velocity subscribers and steering now subscribing to target_velocity instead of max_velocity pure_pursuit_controller works better alone than stanley stanley deactivated vehicle_controller uses flat steering input for now --- code/acting/src/acting/Acting_DebuggerNode.py | 72 +++++----- .../src/acting/pure_pursuit_controller.py | 11 +- code/acting/src/acting/vehicle_controller.py | 13 +- code/acting/src/acting/velocity_controller.py | 135 ++++-------------- 4 files changed, 82 insertions(+), 149 deletions(-) diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index 3509aa31..d2556604 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -28,24 +28,24 @@ # TEST_TYPE to choose which kind of Test to run: # 0: Test Velocity Controller with constant one velocity -# const. velocity = MAX_VELOCITY_LOW +# 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: MAX_VELOCITY_LOW/_HIGH +# 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 = MAX_VELOCITY_LOW TODO: maybe use velocity publisher? +# 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 = MAX_VELOCITY_LOW +# const velocity = TARGET_VELOCITY_1 # const steering = 0 # no trajectory # Triggers emergency break after 15 Seconds @@ -53,14 +53,14 @@ # 4: Test Steering-PID in vehicleController # TODO TODO -TEST_TYPE = 1 # aka. TT +TEST_TYPE = 2 # aka. TT STEERING: float = 0.0 # for TT0: steering -> always straight -MAX_VELOCITY_LOW: float = 3 # for TT0/TT1: low velocity -MAX_VELOCITY_HIGH: float = 14 # for TT1: high velocity +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 = 1 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve +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): @@ -88,7 +88,7 @@ def __init__(self): # Publisher for Dummy Velocity self.velocity_pub: Publisher = self.new_publisher( Float32, - f"/paf/{self.role_name}/max_velocity", + f"/paf/{self.role_name}/target_velocity", qos_profile=1) # Stanley: Publisher for Dummy Stanley-Steer @@ -118,11 +118,11 @@ def __init__(self): qos_profile=1) # ---> EVALUATION/TUNING: Subscribers for plotting - # Subscriber for max_velocity for plotting - self.max_velocity_sub: Subscriber = self.new_subscription( + # Subscriber for target_velocity for plotting + 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) # Subscriber for current_velocity for plotting @@ -169,7 +169,7 @@ def __init__(self): # Initialize all needed "global" variables here self.current_trajectory = [] self.switchVelocity = False - self.driveVel = MAX_VELOCITY_LOW + self.driveVel = TARGET_VELOCITY_1 self.switch_checkpoint_time = rospy.get_time() self.switch_time_set = False @@ -287,7 +287,7 @@ def __current_position_callback(self, data: PoseStamped): self.z = agent.z # TODO use this to get spawnpoint? necessary? - def __get_max_velocity(self, data: Float32): + def __get_target_velocity(self, data: Float32): self.__max_velocities.append(float(data.data)) def __get_current_velocity(self, data: CarlaSpeedometer): @@ -319,38 +319,43 @@ 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 = MAX_VELOCITY_LOW + 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 = MAX_VELOCITY_LOW + 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() - 7.5): + 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 = MAX_VELOCITY_HIGH + self.driveVel = TARGET_VELOCITY_2 else: - self.driveVel = MAX_VELOCITY_LOW + 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 = MAX_VELOCITY_LOW + 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 = MAX_VELOCITY_LOW + self.drive_Vel = TARGET_VELOCITY_1 if not self.time_set: self.checkpoint_time = rospy.get_time() self.time_set = True @@ -361,10 +366,13 @@ def loop(timer_event=None): 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 = MAX_VELOCITY_LOW - self.stanley_steer_pub.publish(STEERING) - self.pure_pursuit_steer_pub.publish(STEERING) + 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) @@ -378,15 +386,15 @@ def loop(timer_event=None): self.time_set = True # Uncomment the prints of the data you want to plot - if (self.checkpoint_time < rospy.get_time() - 22.5): + 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.__max_velocities) + # print(self.__current_velocities) # print(self.__throttles) - # print(self.__purepursuit_steers) - # print(self.__stanley_steers) - # print(self.__vehicle_steers) + print(self.__purepursuit_steers) + print(self.__stanley_steers) + print(self.__vehicle_steers) print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") self.new_timer(self.control_loop_rate, loop) diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index 6296c6cf..65e3481a 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -11,7 +11,7 @@ from std_msgs.msg import Float32 from acting.msg import Debug import rospy -import numpy as np +# import numpy as np from helper_functions import vector_angle from trajectory_interpolation import points_to_vector @@ -140,17 +140,18 @@ def __calculate_steer(self) -> float: k_ld = 0.1 # TODO: tune look_ahead_dist = LOOK_AHEAD_DIS # offset so that ld is never zero - """if self.__velocity < 0: + 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 += k_ld * (self.__velocity - MIN_LD_V) + + # look_ahead_dist = np.clip(k_ld * self.__velocity, + # MIN_L_A_DIS, MAX_L_A_DIS) - 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] diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 67d6a443..314e5bcd 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -143,7 +143,7 @@ def run(self): self.status_pub.publish(True) self.loginfo('VehicleController node running') # currently pid for steering is not used, needs fixing - pid = PID(0.5, 0.01, 0) # PID(0.5, 0.1, 0.1, setpoint=0) + 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) @@ -175,6 +175,11 @@ 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() @@ -184,8 +189,10 @@ def loop(timer_event=None) -> None: message.hand_brake = False message.manual_gear_shift = False # sets target_steer to steer - pid.setpoint = self.__map_steering(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(), @@ -203,7 +210,7 @@ def __map_steering(self, steering_angle: float) -> float: :param steering_angle: calculated by a controller in [-pi/2 , pi/2] :return: float for steering in [-1, 1] """ - tune_k = 1 # -5 factor for tuning TODO: tune but why? + 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 diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index b026bc7e..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 = 10 # 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,17 +38,6 @@ 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", @@ -60,14 +48,6 @@ def __init__(self): f"/paf/{self.role_name}/brake", 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 - # TODO not true anymore? -> obsolete? - self.velocity_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/velocity_as_float", - qos_profile=1) - # needed to prevent the car from driving before a path to follow is # available. Might be needed later to slow down in curves self.trajectory_sub: Subscriber = self.new_subscription( @@ -76,28 +56,9 @@ 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) - - # TODO: currently used to determine position on OpenDrive Map - # to set speed_limits correctly. Planning soon? -> Obsolete - 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): """ @@ -107,12 +68,12 @@ def run(self): self.loginfo('VelocityController node running') # PID for throttle pid_t = PID(0.60, 0.00076, 0.63) - pid_t.output_limits = (0.0, 1.0) + pid_t.output_limits = (-1.0, 1.0) # new PID for braking, much weaker than throttle controller! - pid_b = PID(-0, -0, -0) # TODO tune? BUT current P can be good + # 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.0, 1.0) + # pid_b.output_limits = (.0, 1.0) def loop(timer_event=None): """ @@ -122,11 +83,11 @@ def loop(timer_event=None): :param timer_event: Timer event from ROS :return: """ - if self.__max_velocity is None: - self.logdebug("VelocityController 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}") - self.__max_velocity = SPEED_LIMIT_DEFAULT + self.__target_velocity = SPEED_LIMIT_DEFAULT if self.__current_velocity is None: self.logdebug("VelocityController hasn't received " @@ -140,32 +101,25 @@ def loop(timer_event=None): "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: + if self.__target_velocity < 0: self.logerr("VelocityController doesn't support backward " "driving yet.") return - # TODO: soon Planning wants to calculate and publish max_velocity - v = min(self.__max_velocity, self.__max_tree_v) - v = min(v, self.__speed_limit) - v = self.__max_velocity + v = self.__target_velocity pid_t.setpoint = v throttle = pid_t(self.__current_velocity) - pid_b.setpoint = v - brake = pid_b(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) @@ -175,50 +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): - """ Use the current position to determine where - on the map the agent currently is. - This is needed to determine the Speed limits from - the OpenDrive Map. - :param data (PoseStamped): the Position-Message recieved - :return: - """ - 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): """ From 0a86002a6ab9e6853e9aa9c07779806f45119fd1 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Tue, 19 Dec 2023 01:17:35 +0100 Subject: [PATCH 46/53] fix: Typos and partially implemented Feedback from sprint review --- .../launch/global_planner.launch | 4 +- code/planning/local_planner/src/ACC.py | 66 ++++++++----------- .../local_planner/src/collision_check.py | 8 ++- 3 files changed, 37 insertions(+), 41 deletions(-) diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index 8d65c20c..cdb6dec9 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -1,12 +1,12 @@ - + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 44fad046..2e72554e 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -10,6 +10,8 @@ # from std_msgs.msg import String from std_msgs.msg import Float32MultiArray, Float32, Bool from collision_check import CollisionCheck +import time +from perception.msg import MinDistance class ACC(CompatibleNode): @@ -23,14 +25,6 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) self.current_speed = 50 / 3.6 # m/ss - self.loginfo("ACC started") - # TODO: Add Subscriber for Obsdacle from Collision Check - self.collision_sub = self.new_subscription( - Float32MultiArray, - f"/paf/{self.role_name}/collision", - self.__get_collision, - qos_profile=1) - # Get current speed self.velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, @@ -38,6 +32,13 @@ def __init__(self): 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"/carla/{self.role_name}/LIDAR_range", + self._set_distance, + qos_profile=1) # Get initial set of speed limits self.speed_limit_OD_sub: Subscriber = self.new_subscription( Float32MultiArray, @@ -52,20 +53,17 @@ def __init__(self): self.__set_trajectory, qos_profile=1) - # Get current position to determine current speed limit - self.current_pos_sub: Subscriber = self.new_subscription( - msg_type=PoseStamped, - topic="/paf/" + self.role_name + "/emergency", - callback=self.emergency_callback, - qos_profile=1) - self.emergency_sub: Subscriber = self.new_subscription( msg_type=Bool, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, qos_profile=1) - - # Publish desiored speed to acting + 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", @@ -82,37 +80,29 @@ def __init__(self): # Is an obstacle ahead where we would collide with? self.collision_ahead: bool = False # Distance and speed from possible collsion object - self.obstacle: tuple = None + self.obstacle_speed: tuple = None + # Obstalce distance + self.obstacle_distance = None # Current speed limit self.speed_limit: float = None # m/s - def emergency_callback(self, data: Bool): - """Callback for emergency stop - Turn of ACC when emergency stop is triggered + def _set_distance(self, data: MinDistance): + """Get min distance to object in front from perception Args: - data (Bool): Emergency stop + data (MinDistance): Minimum Distance from LIDAR """ - if data.data is True: - self.collision_ahead = True + self.obstacle_distance = data.distance - def __get_collision(self, data: Float32MultiArray): - """Check if collision is ahead + 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 (Float32MultiArray): Distance and speed from possible - collsion object + data (Float32): Speed from obstacle in front """ - if np.isinf(data.data[0]): - # No collision ahead - self.collision_ahead = False - else: - # Collision ahead - self.collision_ahead = True - self.obstacle = (data.data[0], data.data[1]) - target_speed = self.calculate_safe_speed() - if target_speed is not None: - self.velocity_pub.publish(target_speed) + self.obstacle_speed = (time.time(), data.data) def calculate_safe_speed(self): """calculates the speed to meet the desired distance to the object diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 7af8ca07..606a60f9 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -51,6 +51,11 @@ def __init__(self): 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.rolename}/cc_speed", + qos_profile=1) # Variables to save vehicle data self.__current_velocity: float = None self.__object_last_position: tuple = None @@ -94,7 +99,8 @@ def calculate_obstacle_speed(self, new_dist: Float32): # Speed is distance/time (m/s) relative_speed = distance/time_difference speed = self.__current_velocity + relative_speed - + # Publish distance and speed to ACC for permanent distance check + self.speed_publisher(Float32(data=speed)) # Check for crash self.check_crash((new_dist.data, speed)) self.__object_last_position = (current_time, new_dist.data) From 30007b5c003f886517a827d0eef8e0538bdeace5 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 19 Dec 2023 12:45:13 +0100 Subject: [PATCH 47/53] fix: implemented Feedback from review ACC now permanently checking the distance --- .../local_planner/launch/local_planner.launch | 2 +- code/planning/local_planner/src/ACC.py | 72 +++++++++---------- .../local_planner/src/collision_check.py | 31 ++++---- 3 files changed, 47 insertions(+), 58 deletions(-) diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index a27956cd..c75bc24e 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -5,7 +5,7 @@ - + diff --git a/code/planning/local_planner/package.xml b/code/planning/local_planner/package.xml index 6dc4269a..a1e4978c 100644 --- a/code/planning/local_planner/package.xml +++ b/code/planning/local_planner/package.xml @@ -32,7 +32,6 @@ - @@ -48,12 +47,14 @@ + perception + perception + perception catkin - diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 18ab7c24..d13357fa 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -7,7 +7,7 @@ from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo from nav_msgs.msg import Path # from std_msgs.msg import String -from std_msgs.msg import Float32MultiArray, Float32, Bool +from std_msgs.msg import Float32MultiArray, Float32 from collision_check import CollisionCheck import time from perception.msg import MinDistance @@ -53,7 +53,7 @@ def __init__(self): qos_profile=1) self.emergency_sub: Subscriber = self.new_subscription( - msg_type=Bool, + msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, qos_profile=1) @@ -140,7 +140,6 @@ def __current_position_callback(self, data: PoseStamped): pose.position next_wp = self.__trajectory.poses[self.__current_wp_index + 1].\ pose.position - # distances from agent to current and next waypoint d_old = abs(agent.x - current_wp.x) + abs(agent.y - current_wp.y) d_new = abs(agent.x - next_wp.x) + abs(agent.y - next_wp.y) @@ -182,10 +181,12 @@ def loop(timer_event=None): # If safety distance is reached, drive with same speed as # Object in front self.velocity_pub.publish(self.obstacle_speed[1]) + elif self.speed_limit is not None: # If we have no obstacle, we want to drive with the current # speed limit self.velocity_pub.publish(self.speed_limit) + self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 86388e05..5fe0d95a 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -55,7 +55,7 @@ def __init__(self): # Approx speed publisher for ACC self.speed_publisher = self.new_publisher( Float32, - f"/paf/{self.rolename}/cc_speed", + f"/paf/{self.role_name}/cc_speed", qos_profile=1) # Variables to save vehicle data self.__current_velocity: float = None @@ -99,7 +99,7 @@ def calculate_obstacle_speed(self, new_dist: MinDistance): relative_speed = distance/time_difference speed = self.__current_velocity + relative_speed # Publish speed to ACC for permanent distance check - self.speed_publisher(Float32(data=speed)) + self.speed_publisher.publish(Float32(data=speed)) # Check for crash self.check_crash((new_dist.distance, speed)) self.__object_last_position = (current_time, new_dist.distance) diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/local_planner/src/dev_collision_publisher.py index 65fb1c2e..9bbd267a 100755 --- a/code/planning/local_planner/src/dev_collision_publisher.py +++ b/code/planning/local_planner/src/dev_collision_publisher.py @@ -9,7 +9,9 @@ # from nav_msgs.msg import Path # from std_msgs.msg import String from std_msgs.msg import Float32 +from carla_msgs.msg import CarlaSpeedometer import time +from perception.msg import MinDistance # import numpy as np @@ -25,13 +27,13 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) self.pub_lidar = self.new_publisher( - msg_type=Float32, - topic='/carla/' + self.role_name + '/lidar_dist_dev', + msg_type=MinDistance, + topic=f'/paf/{self.role_name}/Center/min_distance', qos_profile=1) self.pub_test_speed = self.new_publisher( - msg_type=Float32, - topic='/paf/' + self.role_name + '/test_speed', + msg_type=CarlaSpeedometer, + topic='/carla/' + self.role_name + '/test_speed', qos_profile=1) self.sub_ACC = self.new_subscription( msg_type=Float32, @@ -55,11 +57,11 @@ def __init__(self): def callback_manual(self, msg: Float32): if self.manual_start: self.manual_start = False - self.pub_lidar.publish(Float32(data=25)) + self.pub_lidar.publish(MinDistance(distance=25)) time.sleep(0.2) - self.pub_lidar.publish(Float32(data=25)) + self.pub_lidar.publish(MinDistance(distance=25)) time.sleep(0.2) - self.pub_lidar.publish(Float32(data=24)) + self.pub_lidar.publish(MinDistance(distance=24)) # time.sleep(0.2) # self.pub_lidar.publish(Float32(data=20)) # time.sleep(0.2) @@ -69,8 +71,8 @@ def callback_manual(self, msg: Float32): def callback_ACC(self, msg: Float32): self.acc_activated = True - # self.logerr("Timestamp: " + time.time().__str__()) - # self.logerr("ACC: " + str(msg.data)) + self.logerr("Timestamp: " + time.time().__str__()) + self.logerr("ACC: " + str(msg.data)) self.current_speed = msg.data def run(self): @@ -79,7 +81,7 @@ def run(self): :return: """ def loop(timer_event=None): - self.pub_test_speed.publish(Float32(data=13.8889)) + self.pub_test_speed.publish(CarlaSpeedometer(speed=13.8889)) self.new_timer(self.control_loop_rate, loop) self.spin() From 8f16333880f829e3a481df794f9b8427c6b682eb Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 21 Dec 2023 16:35:32 +0100 Subject: [PATCH 49/53] feat: restructure planning package --- build/docker-compose.yml | 4 +- code/agent/launch/agent.launch | 2 +- .../{local_planner => }/CMakeLists.txt | 22 +- .../src/behavior_agent => }/__init__.py | 0 code/planning/behavior_agent/CMakeLists.txt | 21 -- .../launch/behavior_agent.launch | 8 - code/planning/behavior_agent/package.xml | 18 -- code/planning/behavior_agent/readme.md | 190 ---------------- code/planning/behavior_agent/setup.py | 15 -- code/planning/global_planner/CMakeLists.txt | 202 ----------------- .../launch/global_planner.launch | 15 -- code/planning/global_planner/package.xml | 59 ----- code/planning/launch/planning.launch | 35 +++ code/planning/local_planner/__init__.py | 0 .../local_planner/launch/local_planner.launch | 18 -- code/planning/local_planner/setup.py | 7 - code/planning/{local_planner => }/package.xml | 17 +- code/planning/planning_runner/CMakeLists.txt | 205 ------------------ .../launch/planning_runner.launch | 7 - code/planning/planning_runner/package.xml | 65 ------ code/planning/{global_planner => }/setup.py | 0 .../behavior_agent}/__init__.py | 0 .../src/behavior_agent/behavior_tree.py | 2 +- .../src/behavior_agent/behaviours/__init__.py | 0 .../behaviours/behavior_speed.py | 0 .../behavior_agent/behaviours/intersection.py | 0 .../behavior_agent/behaviours/lane_change.py | 0 .../behavior_agent/behaviours/maneuvers.py | 0 .../src/behavior_agent/behaviours/meta.py | 0 .../behaviours/road_features.py | 0 .../behaviours/topics2blackboard.py | 0 .../behaviours/traffic_objects.py | 0 .../global_planner}/dev_global_route.py | 0 .../global_planner}/global_planner.py | 2 +- .../global_planner}/global_route.txt | 0 .../global_planner}/help_functions.py | 0 .../global_planner}/preplanning_trajectory.py | 0 .../src => src/local_planner}/ACC.py | 2 + .../local_planner}/behavior_speed.py | 0 .../local_planner}/collision_check.py | 4 +- .../local_planner}/dev_collision_publisher.py | 0 .../local_planner}/motion_planning.py | 2 +- 42 files changed, 75 insertions(+), 847 deletions(-) rename code/planning/{local_planner => }/CMakeLists.txt (93%) mode change 100644 => 100755 rename code/planning/{behavior_agent/src/behavior_agent => }/__init__.py (100%) mode change 100644 => 100755 delete mode 100644 code/planning/behavior_agent/CMakeLists.txt delete mode 100644 code/planning/behavior_agent/launch/behavior_agent.launch delete mode 100644 code/planning/behavior_agent/package.xml delete mode 100644 code/planning/behavior_agent/readme.md delete mode 100755 code/planning/behavior_agent/setup.py delete mode 100644 code/planning/global_planner/CMakeLists.txt delete mode 100644 code/planning/global_planner/launch/global_planner.launch delete mode 100644 code/planning/global_planner/package.xml create mode 100644 code/planning/launch/planning.launch delete mode 100644 code/planning/local_planner/__init__.py delete mode 100644 code/planning/local_planner/launch/local_planner.launch delete mode 100644 code/planning/local_planner/setup.py rename code/planning/{local_planner => }/package.xml (87%) mode change 100644 => 100755 delete mode 100644 code/planning/planning_runner/CMakeLists.txt delete mode 100755 code/planning/planning_runner/launch/planning_runner.launch delete mode 100644 code/planning/planning_runner/package.xml rename code/planning/{global_planner => }/setup.py (100%) mode change 100644 => 100755 rename code/planning/{global_planner => src/behavior_agent}/__init__.py (100%) mode change 100644 => 100755 rename code/planning/{behavior_agent => }/src/behavior_agent/behavior_tree.py (98%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/__init__.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/behavior_speed.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/intersection.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/lane_change.py (100%) mode change 100644 => 100755 rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/maneuvers.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/meta.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/road_features.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/topics2blackboard.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/traffic_objects.py (100%) rename code/planning/{global_planner/src => src/global_planner}/dev_global_route.py (100%) rename code/planning/{global_planner/src => src/global_planner}/global_planner.py (99%) rename code/planning/{global_planner/src => src/global_planner}/global_route.txt (100%) mode change 100644 => 100755 rename code/planning/{global_planner/src => src/global_planner}/help_functions.py (100%) mode change 100644 => 100755 rename code/planning/{global_planner/src => src/global_planner}/preplanning_trajectory.py (100%) mode change 100644 => 100755 rename code/planning/{local_planner/src => src/local_planner}/ACC.py (99%) rename code/planning/{local_planner/src => src/local_planner}/behavior_speed.py (100%) rename code/planning/{local_planner/src => src/local_planner}/collision_check.py (98%) rename code/planning/{local_planner/src => src/local_planner}/dev_collision_publisher.py (100%) rename code/planning/{local_planner/src => src/local_planner}/motion_planning.py (99%) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index a44d725e..078d97bc 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -57,8 +57,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" + command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + # command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" logging: driver: "local" 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/planning/local_planner/CMakeLists.txt b/code/planning/CMakeLists.txt old mode 100644 new mode 100755 similarity index 93% rename from code/planning/local_planner/CMakeLists.txt rename to code/planning/CMakeLists.txt index eda4abbe..4997236d --- a/code/planning/local_planner/CMakeLists.txt +++ b/code/planning/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(local_planner) +project(planning) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -7,18 +7,21 @@ project(local_planner) ## 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 ## @@ -102,7 +105,7 @@ catkin_package( # LIBRARIES planning # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib - CATKIN_DEPENDS perception + 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/global_planner/CMakeLists.txt b/code/planning/global_planner/CMakeLists.txt deleted file mode 100644 index 3344bc47..00000000 --- a/code/planning/global_planner/CMakeLists.txt +++ /dev/null @@ -1,202 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(planning) - -## 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/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch deleted file mode 100644 index cdb6dec9..00000000 --- a/code/planning/global_planner/launch/global_planner.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - diff --git a/code/planning/global_planner/package.xml b/code/planning/global_planner/package.xml deleted file mode 100644 index 9a2f8896..00000000 --- a/code/planning/global_planner/package.xml +++ /dev/null @@ -1,59 +0,0 @@ - - - planning - 0.0.0 - The planning package - - - - - carla - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - - - - - - 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/__init__.py b/code/planning/local_planner/__init__.py deleted file mode 100644 index e69de29b..00000000 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 5fb371a5..00000000 --- a/code/planning/local_planner/launch/local_planner.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - 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/package.xml b/code/planning/package.xml old mode 100644 new mode 100755 similarity index 87% rename from code/planning/local_planner/package.xml rename to code/planning/package.xml index a1e4978c..a321a109 --- a/code/planning/local_planner/package.xml +++ b/code/planning/package.xml @@ -1,8 +1,8 @@ - local_planner + planning 0.0.0 - The local planning package + The planning package @@ -47,9 +47,20 @@ - perception + 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 98% rename from code/planning/behavior_agent/src/behavior_agent/behavior_tree.py rename to code/planning/src/behavior_agent/behavior_tree.py index 1ac702c1..480abba9 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -92,7 +92,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/behavior_agent/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py rename to code/planning/src/behavior_agent/behaviours/__init__.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py rename to code/planning/src/behavior_agent/behaviours/behavior_speed.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py rename to code/planning/src/behavior_agent/behaviours/intersection.py 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 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py rename to code/planning/src/behavior_agent/behaviours/lane_change.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py rename to code/planning/src/behavior_agent/behaviours/maneuvers.py 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..e61bd0fd 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.logerr('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/local_planner/src/ACC.py b/code/planning/src/local_planner/ACC.py similarity index 99% rename from code/planning/local_planner/src/ACC.py rename to code/planning/src/local_planner/ACC.py index d13357fa..6a19518d 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -83,6 +83,8 @@ def __init__(self): # Current speed limit self.speed_limit: float = None # m/s + self.logerr("ACC initialized") + def _set_distance(self, data: MinDistance): """Get min distance to object in front from perception diff --git a/code/planning/local_planner/src/behavior_speed.py b/code/planning/src/local_planner/behavior_speed.py similarity index 100% rename from code/planning/local_planner/src/behavior_speed.py rename to code/planning/src/local_planner/behavior_speed.py diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/src/local_planner/collision_check.py similarity index 98% rename from code/planning/local_planner/src/collision_check.py rename to code/planning/src/local_planner/collision_check.py index 5fe0d95a..8bea2f82 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -23,9 +23,7 @@ def __init__(self): super(CollisionCheck, self).__init__('CollisionCheck') self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 1) - # self.current_speed = 50 / 3.6 # m/ss - # TODO: Add Subscriber for Speed and Obstacles - self.loginfo("CollisionCheck started") + self.logerr("CollisionCheck started") # self.obstacle_sub: Subscriber = self.new_subscription( # ) diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/src/local_planner/dev_collision_publisher.py similarity index 100% rename from code/planning/local_planner/src/dev_collision_publisher.py rename to code/planning/src/local_planner/dev_collision_publisher.py diff --git a/code/planning/local_planner/src/motion_planning.py b/code/planning/src/local_planner/motion_planning.py similarity index 99% rename from code/planning/local_planner/src/motion_planning.py rename to code/planning/src/local_planner/motion_planning.py index 7b1e5982..3f969b2d 100755 --- a/code/planning/local_planner/src/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -34,7 +34,7 @@ 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.logdebug("MotionPlanning started") + self.logerr("MotionPlanning started") self.target_speed = 0.0 self.__curr_behavior = None From f5aab0dc1e445ab112bc7ba0db02d3b8a0d4eb5c Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 21 Dec 2023 17:12:06 +0100 Subject: [PATCH 50/53] fix: startup behavior agent --- code/planning/CMakeLists.txt | 14 +++++++------- code/planning/src/behavior_agent/behavior_tree.py | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/code/planning/CMakeLists.txt b/code/planning/CMakeLists.txt index 4997236d..34c3ccd0 100755 --- a/code/planning/CMakeLists.txt +++ b/code/planning/CMakeLists.txt @@ -204,11 +204,11 @@ 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} -) +# 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/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 480abba9..990b9e1e 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -1,14 +1,14 @@ #!/usr/bin/env python import functools -import behavior_agent +# 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 From fad590da354db18d2a45330799977bc6bf826884 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Fri, 22 Dec 2023 12:01:58 +0100 Subject: [PATCH 51/53] fix: Logging fxed --- code/planning/src/global_planner/global_planner.py | 2 +- code/planning/src/local_planner/ACC.py | 2 +- code/planning/src/local_planner/collision_check.py | 2 +- code/planning/src/local_planner/motion_planning.py | 3 ++- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index e61bd0fd..a113c787 100755 --- a/code/planning/src/global_planner/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.logerr('PrePlanner-Node started') + self.logdebug('PrePlanner-Node started') def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 6a19518d..3849dc6a 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -83,7 +83,7 @@ def __init__(self): # Current speed limit self.speed_limit: float = None # m/s - self.logerr("ACC initialized") + self.logdebug("ACC initialized") def _set_distance(self, data: MinDistance): """Get min distance to object in front from perception diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 8bea2f82..3cc107d3 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -23,7 +23,6 @@ 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.logerr("CollisionCheck started") # self.obstacle_sub: Subscriber = self.new_subscription( # ) @@ -58,6 +57,7 @@ def __init__(self): # 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 diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 3f969b2d..92949cad 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -34,7 +34,6 @@ 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.logerr("MotionPlanning started") self.target_speed = 0.0 self.__curr_behavior = None @@ -73,6 +72,8 @@ def __init__(self): 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) From 83971192c3ea9636b1eb7329540832c6cef6214a Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 11 Jan 2024 17:06:06 +0100 Subject: [PATCH 52/53] fix: removed old folder --- .../global_planner/launch/global_planner.launch | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100644 code/planning/global_planner/launch/global_planner.launch 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 cdb6dec9..00000000 --- a/code/planning/global_planner/launch/global_planner.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - From bda01122a77319b52a9f1cca7dff33d3a4e9b094 Mon Sep 17 00:00:00 2001 From: MaxJa4 <74194322+MaxJa4@users.noreply.github.com> Date: Sat, 13 Jan 2024 15:35:18 +0100 Subject: [PATCH 53/53] 110 - Implement traffic light detection (#148) * Add publisher * Publish segmented traffic lights * Implement TrafficLightNode * Add TrafficLightState msg. WIP * Add publisher * Publish segmented traffic lights * Implement TrafficLightNode * Add TrafficLightState msg. WIP * Added side view classification * Finish traffic light node * Add manual control launch file * Make linter happy * Add documentation * Add missing traffic light detection model * Fix color issues in rviz * Limit simulator's max. RAM usage to prevent system crash * fix: Linter fixes for other team's code --- .gitignore | 2 + build/docker-compose.yml | 4 ++ code/agent/launch/agent_manual.launch | 31 ++++++++++ code/perception/CMakeLists.txt | 1 + code/perception/launch/perception.launch | 18 ++++-- code/perception/msg/TrafficLightState.msg | 5 ++ .../src/traffic_light_detection/.gitignore | 1 - .../src/traffic_light_detection/dataset.dvc | 2 +- .../src/traffic_light_detection/dvc.lock | 23 +++---- .../dvclive/metrics.json | 11 ++++ .../models/model_acc_92.48_val_91.88.pt | Bin 0 -> 11071 bytes .../src/traffic_light_detection/params.yaml | 2 +- .../src/traffic_light_config.py | 2 +- .../traffic_light_inference.py | 12 ++-- .../traffic_light_training.py | 4 +- .../transforms.py | 0 code/perception/src/traffic_light_node.py | 57 ++++++++++++++++++ code/perception/src/vision_node.py | 39 ++++++++++-- .../src/behavior_agent/behavior_tree.py | 8 +-- .../src/behavior_agent/behaviours/__init__.py | 2 - .../13_traffic_light_detection.md | 37 ++++++++++++ .../object-detection-model_evaluation/yolo.py | 8 ++- 22 files changed, 228 insertions(+), 41 deletions(-) create mode 100644 code/agent/launch/agent_manual.launch create mode 100644 code/perception/msg/TrafficLightState.msg create mode 100644 code/perception/src/traffic_light_detection/dvclive/metrics.json create mode 100644 code/perception/src/traffic_light_detection/models/model_acc_92.48_val_91.88.pt rename code/perception/src/traffic_light_detection/src/{data_generation => traffic_light_detection}/transforms.py (100%) create mode 100755 code/perception/src/traffic_light_node.py create mode 100644 doc/06_perception/13_traffic_light_detection.md 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 92f54ef1..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 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/perception/CMakeLists.txt b/code/perception/CMakeLists.txt index 564502a6..70e340c4 100644 --- a/code/perception/CMakeLists.txt +++ b/code/perception/CMakeLists.txt @@ -52,6 +52,7 @@ add_message_files( FILES Waypoint.msg LaneChange.msg + TrafficLightState.msg MinDistance.msg ) diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index be3c7d27..300976a3 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -2,7 +2,7 @@ - + - - + - yolov8x-seg + --> + + + + + + + + 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/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 index 73aa6bd3..2f10a027 100644 --- a/code/perception/src/traffic_light_detection/dataset.dvc +++ b/code/perception/src/traffic_light_detection/dataset.dvc @@ -1,5 +1,5 @@ outs: -- md5: 3a559397ebc58c1ecf142dea18d03367.dir +- md5: 86f14bb96e1ac5735051b8e873f07a9f.dir size: 13745063 nfiles: 2723 hash: md5 diff --git a/code/perception/src/traffic_light_detection/dvc.lock b/code/perception/src/traffic_light_detection/dvc.lock index d9f625ce..6438d251 100644 --- a/code/perception/src/traffic_light_detection/dvc.lock +++ b/code/perception/src/traffic_light_detection/dvc.lock @@ -4,31 +4,32 @@ stages: cmd: python src/traffic_light_detection/traffic_light_training.py deps: - path: dataset - md5: 3a559397ebc58c1ecf142dea18d03367.dir + hash: md5 + md5: 86f14bb96e1ac5735051b8e873f07a9f.dir size: 13745063 nfiles: 2723 - path: src hash: md5 - md5: b6c9cb867c89ad6e86403d9c33538136.dir - size: 23777 - nfiles: 10 + md5: 34c981a61e886a858d135daee17a82ba.dir + size: 35849 + nfiles: 17 params: params.yaml: train: - epochs: 100 + epochs: 20 batch_size: 32 outs: - path: dvclive/metrics.json hash: md5 - md5: af33de699558fbfd3edee1607ba88f81 - size: 218 + md5: 8566265bcdc76cb55d17230f82fc1517 + size: 219 - path: dvclive/plots hash: md5 - md5: 774919de9e9d6820ac6821d0819829c1.dir - size: 8900 + md5: f8f99f42fc42e0ed3c80c8e8f05c1528.dir + size: 8870 nfiles: 4 - path: models hash: md5 - md5: ee67bac2f189d2cc5a199d91ba3295ac.dir - size: 10815 + md5: 16f96ecc475d20123051719908af9d4d.dir + size: 11071 nfiles: 1 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 0000000000000000000000000000000000000000..584f75f5cc2f038cf86088583d5d9ed96f7d9c55 GIT binary patch literal 11071 zcmb`N2Ut``*T7uxVG?mcQ=!o>Vgs4zabW&P!YG{U?3M*%37NjKkUYew4cuX`a-|-a{uY}}qQ9#E@ z6;>fgMpDOyRSZ&;+DMNIceP=a{07U2Smkj7o|!;iASX}|D2Q0TnLr@uFZnFFq*4?s zRi-Lp)r9OdmOdhM*0n-w( zy@hO_APosDB3u+18=91yn)vCC`*wlavHdzj`%9tPAE7!@sBQ;zfQZ!-vIB#9eg{? z*bze3{gc<){pqzHU9=BnM|Q?|`jtvJUJ{Pf>Et_{&f827^{3N~>Y)3G*wI2(@X6`y ztlK-&w+q{u_3O+YBW;(lAKS%W+AiZdU_ucaAY{jXl4}2_)Cpa1E^J_D+(bdChzKqZ9EoA2gss4xB-u3gkG9B2Q&dgkC@XBjP=CS!wkp&&NLJ?ae zWap=6u?vFu5~O70io&Hqoh@#UZY5?xlCwN^VHUfnJ(6Zbr>03lG+Qc=NefbtTxt)= z?BXo8EVlieR9L6b#g=EW6_P8m|EX=qF3DmmJ8Da9CWe0#nv$HHV9PGeVyimN|4FuI ztFzc;f30iBew)SCbgk>aF3)0X|613ct;=HTJI+g!!L)>Mkt9d34O#4p|D26VO-@Np z7qQ=Eu`4^yN|QlH$FZxj*wyVP)6*iX{RK+O>n*2}Mo9%}@&DW0X~^^VLhg*n6h$XX z6P=g)u>UD}O7d<(oJf=qEmf8WH~UMKOUvIY)8`HTf-8PfU9KQGvl*O$Z|(2P9whJ);V|3%cUHDzF{Jc z+i?{g8$ICWoIy~eCr2{W8X$f5b2rCe4sGV~h`gEunnl==5d()eZ4flUy*rMi?*b>H zH`fjW%a^((9Md6gzou|z>HW!YD|5G}Yiu~5G6kaG`wZUJwQ?($R^XYo>3HF2H9R`| zmRa|B1*f@r1q^SkWMn~z%JUY&?D+4wM>$c9mHZ7Z>Utu2^|yw~fYss)k8X%>2o-75 z=oRA8DNQ&bcnMB>70X2(|C-q&Z~(cWT>K;E2{&HzG)6|Ab}8LogCk?lf!&~dW`45| zcsm2$xPPswy60ui*k~*~xcm|(FVdjvuOC8hhwI3*^mUv0Tn+Qo4KboB7J{|1F`()O zH`~4(Pv#ZD&Z|pt`_ao#(C;{EFV$kEHJ*k+VeX7yT`pePh+ISTDiqCBrPbda=58r@ zLS%NEcy^FH1}2@yfFr$`qhdYUQ~xVE)B$@!^O-PlMPHoP&7=J>xs2&gWGB6cnE*?4CZ$y|K7_y|{VFac@HUq}b1X0u zJh_LfZ^6&~Wk{M|DlQ+m4~{8q0;~AB;OV2}`s?rOK-O{{9FS+x*W(Fiug-_%#re=j z_7wE#` zoC=CddeD&VLtq&H9ye_7E;#S<3a##z;8nMW+`+tk=n%7gX_Vc$t|5y0E>-eXuZ-h zurQkfUbk)0Z^U&bzrjT8dm$Jc+}b!9?#JEdwiSR)>U=KLVLi(9@?^BVS3*I(s@sFY zbEr7(gu~YKNTy<(4n8#4#r=?;An}`X@N5}}0fU-x@;rWzGsfAcrs zrtlVw_ti7@mKzyalYTH;s{uFDEKof$2TYA$qRdfyocB@}d;hW$?`W~GTZo+g@2k1s zlzbeh#lu+&iugUw!@1|ypira=*?uc9NV5cwyc!E{Qsn3Zlh&qhZcM=g)!Mjyxfcj^ z?{Z0dwXv7DnMv`y2n_0hX5I<7SENn+VroGeDW@H*J-9_O>yef(MN0SXBBiaDJNd_v z^1n}#Dwp%=_zicM)`U=+>wkuk>Ki#k#Fsv^OUvKu8^jybtv*R8AMj;?qh1|{a<4~> zn$1lVXz_{Ee&FbcmVd{-q=CAz?`S%M9E{{+^feP&KkYRantu!CKeyp(eZuLoiAnUz zZ>7Y!*A5s~Uy6seomlS6AsS&GAC%Js47~eS&IwG%%{&v2GUVnN9jver) za=(~R6{Zn9zkUVk1w~lII|Ebq`I09i_ro!-v811^Avqb~zqg^q=0nh2_ zGVveq2TZXPqQ}BLC^v93=i4@rUdxZ7XPzF%%CPA)?74tO4St1Ee--RJa(uXCIw37z z8ZWlpju$&GcdIXtm(NG>s+WA~xa4OvP*%m~$3^zJv9o-q5&)1nc$;aFl=F~TUxIi z-2Gxc*^(LotJh1OTLuJ>?cc}}?adeQ^*SE4ycACFrklYEph%swF{0ox?)d2xbEN-I zVDM`avupDpn3QKp>aLj(pEZ^6BrTEnEii@ZHp8Y_y#8SQav|q=_82Ps77D@Jc+mQ? zC-wWK2RfQ7a2rqP5|`{{VDjuG=QLg!@8<;4=C|W;^jUxxrgNG1lJVPQ|HxI?<21U% zN_?@p3G~X{Nb9cyI6+1ntf;SoO{&*%;tVx9;`~KC(W?;0G$dj}iU!qKd;>?FTf0YV zff7AFv6$GIAB0nNi||my9QtgU4CKZ&1CF={ep=P|kiE zHi)vOqp8^ddsGc5!<&OcxPU4VoK|-Rw@DggibGE_d;eVcP*V!4ldEuDjWHTNt3&pF ze_XfnIv#pbfODp*(15Zaj2!JjC-v-BNivsHOxB+8iOr-;|Wpod88eWw5QP z!1-l=V6wIr%xP-|xk5#%ej@?Y8hP0NW&?O$)&rGh9w{%qjH_NwBJ8D}WaR@bqLMtA z>>ThKW0&rL4GuOm=CBjx`Nq=4v+StW>MO8do&hO3?}mwo2Ga3Yyy-NzvDCMu0e8Jz zjHJ$(>M1@&EA2is?_Dpn^B7L1FLQ!#)YszUx-2LP;lXq+3{@8wbB=8x82WCWIQPwQ z)Tqm29^O=g%s3S5So=}C&3t;*dkoz)#}qXWPJrgwCvjpgReJTe9L8(PKJom$qv?~H z-l%$T0_VZ=qmliTu%?tpFaP50s$1^M{dDy?R9|o>5l?Li^R)$iAH5rT>6jCvtWo62 zQ$7+MXU41_<&K|Q43*n1BZtvkY?=mrq|tzOQ)bYWGw0(^F>3V1;5!(){~9RI8%SP` za%6m3dr)4u2Zk7zp^?`-lHT?lrfjl+*{`!9@V*P#GHVtdyYPr}c+mpQJA2XxS@k%f ztO2~Axsq|Fdr>Pi7Q76$fU?VD*ejz7%U%Cy>ic~iaTLAwiU*jRzg~ZfYiyI$F0Mbix0fo z&O9jKlSLkD@k&r2zMsR8aM4jFo_mfzTwlRNeLoZKrQ|m`e|rG<+ujY`Y*9vLm})Td zyVg}?Rf!(H)g<0{Tk;P0&AMn*K? zi&<%~em2M4X)2|CHdJs${Z`-&^|RP-$#*ovPJ;^GPj^!dIE--U2_Dy-M@6?5fUd6q z>H=+vw;+Og9x^6OtpaJydCEi_=t0A5J*ZXr02IsR(dw<|aONpBe6%1Pb0%2O$@W!X zxN|O47LB8el$4qqGs~HRVSP!#l$khr;A*P9bUQwb@?owXD*{%bCzZzk>$gZFPx|6O zQvSF2@9_VQ|D)RqNYmi8+zh4`uko|RmV1}A=cG&<#YioQBx(73P5L~&?sAeOC24;l zC0!u3!)gN;5Boi-df1sqD~3tiLuJ=x>+bW9KF^>#+miI7_XXS1Txglwhp95zhOguK z^cQW@_UG%xY*O&QVqel7-Pz9_Rm&{RT1qT-X1TSE&}_F~xYd2jzhd99d;3%R8!`8T zH9&Z(*mdtbXwUzbdLQ2Uuh@6$-hNbXiG#KKn3D2{{n7H$g&b-5lFwOm*Y88U+)cXa z_dlP5&iEaL72BI(j-?riuTdcnJ*JS&IR<3hPFWJTLznnmyaqAuHst>4wc_*=W9Cqr z1;p9D1cx$1GW}jXw{L2y+WDJKEbmG%SBwKxHnV~$j^*xR%+ct3b&Q74V z%@e7Ok|#A&{0<+n?)25`{doMfD$ZWegFaZY3T>j5@nCXk(*wV9WL)>7$y!^w?D4N? zbt{lA3Twe*oE2T|B1;c*4cNBBj+QMu%Dwo`h!&MTMAPA_bXA@XPE_%yx<5{)r&hY+ zrOBBnlkqhjW~)n8$175&L_IoN{u+*c8b-GU4rof{oWXmOI_dpkGKnhJfqNI^$ihL! zz@?d!@++2PN$DEcoYcbX-Dn7JYh6h14ZFcR|94mAR5P&a-+=Lc-vYfVN0J$D*Y(Zn zFy@jOefA<2iswax-sx$u^UzM%v&DqG2|LZG-E`uXD6fFcYvZ8)Ts1RihZ^xSTF+$8 zJ_hcW_rU($?&LcC7D9e{3{&Dvh|?-{;!%GE#%npiubbY(erGFEwC6H_XQo(7(gN1Y z!;v4ViTo4k%v2q9=0~&PcW!hwyhDI)z)ORQQqa|EbzsKqAZ;1V*(*hv+=HI9Lc|z zM#Nx8g1q%e@unki0DBV`m`V1Qh@tVUJ)y5Al1V<1#7ucADanx`TOP<6Y0C_T=+yvMMCThVA-Sb zjy_wN+4YOE!c^6!v6XJ%=$9@3akTjB0^b9o2639Mf zCo<`V9@)20nN*+jCj%|d!$=1+qO>I(CZ8>FPMGipo_gdG{yqzGQ?vw6r~HUdv<8#P z;CIk?WeD*#EOlE@lR(4l-SBHm7ra@u3>-Xu=EjHpfVLl=;)jBKJp9}Uwiq|zjN<9E z^0zx~RVLrzp!f@Dm=(eCn~ZVzN>93RT^w#tInNm1=0l$sQKSdm6{h4ss9t^=Rs(YP|p5e9XI{17Gd?z#T3Rq*oY!dOdh8r&OU$+iu@QuS6@VG}f8^ z@RKEJeDDN(RAhL{eRx21cx$U4Wo?Nq^(z?C;LFXe z-NQtPP3f~!#xAY1Jcz?7J$iCnJ-*p?7x#va0yg*ohTC3;jJE^H=zZ2=U2zOkw`?NG zonc26cKA`fg_-oQM-+D})}8V<>eCSiOX0xjUod}vHka=Ho=Y6yL8hHCWWof5?MeU3F3_a}F8ZQ1O{tF{Cd$nq>@2v8Y&eOwklf*8xZgIxoA(aFk6q%1iA54!Nc*CL8k z>L-xo-P6cXy+ZO1%ZYiNGX1Il2QFPOn7*5L2=cO3-4ZU3L@uiaBQ_2qs!N73@f&lv zj3Lpu@#+>RU&RB{1q;ZSRRz#{W+1s=$uOmowXdtXlj-5$Y4n7r2W=6IBELOtfjvWw zaO0i>_;9Ezj#k%f_gC5R%RK(q_g^2EcmLn>AA1$)jN^1|%mnFrzO?-P2)5zre_;e$ zw=X`XMW@DvC&bMNm+l7&jf?Vf*Zty%ws9J2>tyF*XEW5n#?isW*~ZDn$=1o)(caO{ z-pOUCvxCdfp|%drPEwU;uCF>_CQ&FYl6??Tv7dH6bzavskY4C=biWVkb6)4wSi>$X zMzR$B_qI`W>2DrSMpFC*3euf5JWa`uw0=k3{{uXIDZu~$ literal 0 HcmV?d00001 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/traffic_light_inference.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py index 4631660b..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 @@ -2,11 +2,12 @@ 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(): @@ -49,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): """ 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 100% 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 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 06dab72a..e9681a4c 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -85,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" @@ -127,6 +128,13 @@ def setup_camera_publishers(self): qos_profile=1 ) + 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": @@ -140,12 +148,10 @@ def handle_camera_image(self, image): # 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, @@ -174,10 +180,35 @@ def predict_ultralytics(self, image): desired_encoding='passthrough') cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) - 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): diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 990b9e1e..cacf8619 100755 --- a/code/planning/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 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,7 +59,7 @@ def grow_a_tree(role_name): ("Leave Change") ]) ]), - + ]), behaviours.maneuvers.Cruise("Cruise") ]) diff --git a/code/planning/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py index f8984b65..e69de29b 100755 --- a/code/planning/src/behavior_agent/behaviours/__init__.py +++ b/code/planning/src/behavior_agent/behaviours/__init__.py @@ -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/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/experiments/object-detection-model_evaluation/yolo.py b/doc/06_perception/experiments/object-detection-model_evaluation/yolo.py index f7ff342d..39d727b7 100644 --- a/doc/06_perception/experiments/object-detection-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