From 7818f108f1f0660df46ba9208ad340cc5b3ee03d Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Tue, 9 Jan 2024 09:27:20 +0100 Subject: [PATCH 1/4] feat: Ready for Acting-Testing --- build/docker-compose.yml | 4 ++-- code/acting/launch/acting.launch | 4 ++-- code/acting/src/acting/Acting_DebuggerNode.py | 6 +++--- code/agent/config/dev_objects.json | 6 +++--- code/agent/config/rviz_config.rviz | 2 +- code/perception/launch/perception.launch | 8 ++++---- code/planning/behavior_agent/launch/behavior_agent.launch | 4 ++-- code/planning/global_planner/launch/global_planner.launch | 4 ++-- code/planning/local_planner/launch/local_planner.launch | 8 ++++---- 9 files changed, 23 insertions(+), 23 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 92f54ef1..98e8f19c 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/acting/launch/acting.launch b/code/acting/launch/acting.launch index 975d0b0e..b1262c4b 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -29,10 +29,10 @@ - + diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index d2556604..518f6611 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -55,8 +55,8 @@ # TODO TODO TEST_TYPE = 2 # aka. TT -STEERING: float = 0.0 # for TT0: steering -> always straight -TARGET_VELOCITY_1: float = 7 # for TT0/TT1: low velocity +STEERING: float = 0.0 # for TT0: steering -> always straight +TARGET_VELOCITY_1: float = 7 # for TT0/TT1: low velocity TARGET_VELOCITY_2: float = 0 # for TT1: high velocity STEERING_CONTROLLER_USED = 1 # for TT2: 0 = both ; 1 = PP ; 2 = Stanley @@ -272,7 +272,7 @@ def updated_trajectory(self, target_trajectory): pos.header.frame_id = "global" pos.pose.position.x = wp[0] pos.pose.position.y = wp[1] - pos.pose.position.z = 37.6 # why?? + pos.pose.position.z = 666 # why?? # currently not used therefore zeros pos.pose.orientation.x = 0 pos.pose.orientation.y = 0 diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index 90b24361..470921e2 100644 --- a/code/agent/config/dev_objects.json +++ b/code/agent/config/dev_objects.json @@ -146,9 +146,9 @@ "y": 0.0, "z": 1.60 }, - "noise_alt_stddev": 0.000005, - "noise_lat_stddev": 0.000005, - "noise_lon_stddev": 0.000005, + "noise_alt_stddev": 0.000000, + "noise_lat_stddev": 0.000000, + "noise_lon_stddev": 0.000000, "noise_alt_bias": 0.0, "noise_lat_bias": 0.0, "noise_lon_bias": 0.0 diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 5c10eca9..87052b70 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -166,7 +166,7 @@ Visualization Manager: Offset: X: 0 Y: 0 - Z: 39 + Z: 666 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 be3c7d27..03f62545 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -30,9 +30,9 @@ - + - + - - + diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index 398c35ce..41e2bb6a 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -7,9 +7,9 @@ --> - + \ No newline at end of file diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index 5fb371a5..09772836 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -1,18 +1,18 @@ - + - + From deae16ff9f637ad7e3a641fdd3ff0cbe2f981cd5 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Tue, 9 Jan 2024 12:11:13 +0100 Subject: [PATCH 2/4] feat: Steering-testability enabled --- code/acting/launch/acting.launch | 15 +++-- code/acting/src/acting/Acting_DebuggerNode.py | 64 +++++++++++++++---- .../src/acting/pure_pursuit_controller.py | 4 +- code/acting/src/acting/vehicle_controller.py | 2 +- code/acting/src/acting/velocity_controller.py | 4 +- code/agent/config/dev_objects.json | 12 ++-- code/agent/config/rviz_config.rviz | 2 +- .../perception/src/Position_Publisher_Node.py | 10 ++- 8 files changed, 80 insertions(+), 33 deletions(-) diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index b1262c4b..4dececc8 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -8,10 +8,10 @@ - + @@ -44,15 +44,16 @@ - + - - - + + + + diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index 518f6611..0acbbda5 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -53,14 +53,14 @@ # 4: Test Steering-PID in vehicleController # TODO TODO -TEST_TYPE = 2 # aka. TT +TEST_TYPE = 2 # aka. TT -STEERING: float = 0.0 # for TT0: steering -> always straight +FIXED_STEERING: float = -math.pi/2 # for TT0: steering 0.0 = always straight TARGET_VELOCITY_1: float = 7 # for TT0/TT1: low velocity TARGET_VELOCITY_2: float = 0 # for TT1: high velocity STEERING_CONTROLLER_USED = 1 # for TT2: 0 = both ; 1 = PP ; 2 = Stanley -TRAJECTORY_TYPE = 0 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve +TRAJECTORY_TYPE = 1 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve class Acting_Debugger(CompatibleNode): @@ -125,6 +125,22 @@ def __init__(self): self.__get_target_velocity, qos_profile=1) + # Subscriber for current_headng for plotting + self.heading_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/current_heading", + self.__get_heading, + qos_profile=1 + ) + + # Subscriber for yaw for plotting + self.yaw_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/yaw", + self.__get_yaw, + qos_profile=1 + ) + # Subscriber for current_velocity for plotting self.current_velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, @@ -181,10 +197,15 @@ def __init__(self): self.__max_velocities = [] self.__throttles = [] + self.__current_headings = [] + self.__yaws = [] + self.__purepursuit_steers = [] self.__stanley_steers = [] self.__vehicle_steers = [] + self.positions = [] + self.path_msg = Path() self.path_msg.header.stamp = rospy.Time.now() self.path_msg.header.frame_id = "global" @@ -272,7 +293,7 @@ def updated_trajectory(self, target_trajectory): pos.header.frame_id = "global" pos.pose.position.x = wp[0] pos.pose.position.y = wp[1] - pos.pose.position.z = 666 # why?? + pos.pose.position.z = 35 # why?? # currently not used therefore zeros pos.pose.orientation.x = 0 pos.pose.orientation.y = 0 @@ -285,7 +306,16 @@ def __current_position_callback(self, data: PoseStamped): self.x = agent.x self.y = agent.y self.z = agent.z + # TODO use this to get spawnpoint? necessary? + # use to plot current_position to trajectory for steering test + self.positions.append((self.x, self.y)) + + def __get_heading(self, data: Float32): + self.__current_headings.append(float(data.data)) + + def __get_yaw(self, data: Float32): + self.__yaws.append(float(data.data)) def __get_target_velocity(self, data: Float32): self.__max_velocities.append(float(data.data)) @@ -321,9 +351,9 @@ def loop(timer_event=None): """ # Drive const. velocity on fixed straight steering if (TEST_TYPE == 0): - self.drive_Vel = TARGET_VELOCITY_1 - self.stanley_steer_pub.publish(STEERING) - self.pure_pursuit_steer_pub.publish(STEERING) + self.driveVel = TARGET_VELOCITY_1 + self.stanley_steer_pub.publish(FIXED_STEERING) + self.pure_pursuit_steer_pub.publish(FIXED_STEERING) self.velocity_pub.publish(self.driveVel) # Drive alternating velocities on fixed straight steering @@ -339,8 +369,8 @@ def loop(timer_event=None): self.driveVel = TARGET_VELOCITY_2 else: self.driveVel = TARGET_VELOCITY_1 - self.stanley_steer_pub.publish(STEERING) - self.pure_pursuit_steer_pub.publish(STEERING) + self.stanley_steer_pub.publish(FIXED_STEERING) + self.pure_pursuit_steer_pub.publish(FIXED_STEERING) self.velocity_pub.publish(self.driveVel) # drive const. velocity on trajectoy with steering controller @@ -362,8 +392,8 @@ def loop(timer_event=None): 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.stanley_steer_pub.publish(FIXED_STEERING) + self.pure_pursuit_steer_pub.publish(FIXED_STEERING) self.velocity_pub.publish(self.driveVel) # drive const. velocity and follow trajectory by @@ -384,6 +414,9 @@ def loop(timer_event=None): if not self.time_set: self.checkpoint_time = rospy.get_time() self.time_set = True + print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<") + print(self.current_trajectory) + print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<") # Uncomment the prints of the data you want to plot if (self.checkpoint_time < rospy.get_time() - 10.0): @@ -392,9 +425,12 @@ def loop(timer_event=None): # 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(self.__current_headings) + # print(self.__yaws) + print(self.positions) 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 65e3481a..a71fc197 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -109,6 +109,7 @@ def loop(timer_event=None): self.logdebug("PurePursuitController hasn't received a path " "yet and can therefore not publish steering") return + if self.__position is None: self.logdebug("PurePursuitController hasn't received the " "position of the vehicle yet " @@ -126,6 +127,7 @@ def loop(timer_event=None): "velocity of the vehicle yet " "and can therefore not publish steering") return + self.pure_pursuit_steer_pub.publish(self.__calculate_steer()) self.new_timer(self.control_loop_rate, loop) @@ -137,7 +139,7 @@ def __calculate_steer(self) -> float: :return: """ l_vehicle = 2.85 # wheelbase - k_ld = 0.1 # TODO: tune + k_ld = 0.5 # TODO: tune look_ahead_dist = LOOK_AHEAD_DIS # offset so that ld is never zero if self.__velocity < 0: diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 314e5bcd..ea062909 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -210,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 # 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 754cbdc2..f00962cc 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -95,12 +95,12 @@ def loop(timer_event=None): "publish a throttle value") return - if self.__trajectory is None: + """if self.__trajectory is None: self.logdebug("VelocityController hasn't received " "trajectory yet and can therefore not" "publish a throttle value (to prevent stupid)") return - + """ if self.__target_velocity < 0: self.logerr("VelocityController doesn't support backward " "driving yet.") diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index 470921e2..f4dc0825 100644 --- a/code/agent/config/dev_objects.json +++ b/code/agent/config/dev_objects.json @@ -164,12 +164,12 @@ "pitch": 0.0, "yaw": 0.0 }, - "noise_accel_stddev_x": 0.001, - "noise_accel_stddev_y": 0.001, - "noise_accel_stddev_z": 0.015, - "noise_gyro_stddev_x": 0.001, - "noise_gyro_stddev_y": 0.001, - "noise_gyro_stddev_z": 0.001 + "noise_accel_stddev_x": 0.000, + "noise_accel_stddev_y": 0.000, + "noise_accel_stddev_z": 0.000, + "noise_gyro_stddev_x": 0.000, + "noise_gyro_stddev_y": 0.000, + "noise_gyro_stddev_z": 0.000 }, { "type": "sensor.pseudo.speedometer", diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 87052b70..9f894cbb 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -166,7 +166,7 @@ Visualization Manager: Offset: X: 0 Y: 0 - Z: 666 + Z: 35 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 diff --git a/code/perception/src/Position_Publisher_Node.py b/code/perception/src/Position_Publisher_Node.py index 8c0431c2..04aa91e4 100755 --- a/code/perception/src/Position_Publisher_Node.py +++ b/code/perception/src/Position_Publisher_Node.py @@ -85,6 +85,12 @@ def __init__(self): f"/paf/{self.role_name}/current_heading", qos_profile=1) + self.__yaw: float = 0 + self.__yaw_publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/yaw", + qos_profile=1) + def get_geoRef(self, opendrive: String): """_summary_ Reads the reference values for lat and lon from the carla OpenDriveMap @@ -163,7 +169,9 @@ def update_imu_data(self, data: Imu): # --------------------------------------------------------------- heading = (raw_heading - (math.pi / 2)) % (2 * math.pi) - math.pi self.__heading = heading - self.__heading_publisher.publish(self.__heading) + # self.__heading_publisher.publish(self.__heading) + self.__heading_publisher.publish(yaw) + self.__yaw_publisher.publish(yaw) def update_gps_data(self, data: NavSatFix): """ From c1ea16f094a7f227fc1aa70ffe0be5308bc33f52 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 11 Jan 2024 14:32:24 +0100 Subject: [PATCH 3/4] feat: Tune PurePursuit_Con and check stanley_Con --- code/acting/launch/acting.launch | 7 +- code/acting/src/acting/Acting_DebuggerNode.py | 64 ++++++++---------- .../src/acting/pure_pursuit_controller.py | 67 ++++--------------- code/acting/src/acting/stanley_controller.py | 3 +- code/acting/src/acting/vehicle_controller.py | 16 ++--- code/agent/config/rviz_config.rviz | 2 +- .../perception/src/Position_Publisher_Node.py | 1 - 7 files changed, 54 insertions(+), 106 deletions(-) diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 4dececc8..bd4166b5 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -8,10 +8,10 @@ - + @@ -51,8 +51,7 @@ - - + diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index 0acbbda5..4741ce68 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -55,12 +55,14 @@ # TODO TODO TEST_TYPE = 2 # aka. TT -FIXED_STEERING: float = -math.pi/2 # for TT0: steering 0.0 = always straight -TARGET_VELOCITY_1: float = 7 # for TT0/TT1: low velocity +FIXED_STEERING: float = 0 # for TT0: steering 0.0 = always straight +TARGET_VELOCITY_1: float = 5 # 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 +TRAJECTORY_TYPE = 2 # for TT2: 0 = Straight ; 1 = Curve ; 2 = SineWave + +PRINT_AFTER_TIME = 20.0 # How long after Simulationstart to print data class Acting_Debugger(CompatibleNode): @@ -222,16 +224,33 @@ def __init__(self): ] self.updated_trajectory(self.current_trajectory) - elif (TRAJECTORY_TYPE == 1): # Sinewave Serpentines trajectory + elif (TRAJECTORY_TYPE == 1): # straight into 90° Curve + self.current_trajectory = [ + (984.5, -5442.0), + + (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) + ] + self.updated_trajectory(self.current_trajectory) + + elif (TRAJECTORY_TYPE == 2): # 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 + resolution = 70 # 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 + x_wave = 0.15 * my_wave # to have a serpentine line with +/-1.5 m # to have the serpentine line drive around the middle # of the road/start point of the car x_wave += startx @@ -249,33 +268,6 @@ def __init__(self): 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. @@ -415,11 +407,11 @@ def loop(timer_event=None): self.checkpoint_time = rospy.get_time() self.time_set = True print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<") - print(self.current_trajectory) + # print(self.current_trajectory) print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<") # Uncomment the prints of the data you want to plot - if (self.checkpoint_time < rospy.get_time() - 10.0): + if (self.checkpoint_time < rospy.get_time() - PRINT_AFTER_TIME): self.checkpoint_time = rospy.get_time() print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") # print(self.__max_velocities) @@ -430,7 +422,7 @@ def loop(timer_event=None): # print(self.__vehicle_steers) # print(self.__current_headings) # print(self.__yaws) - print(self.positions) + # print(self.positions) 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 a71fc197..a9adb282 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -4,22 +4,23 @@ import ros_compatibility as roscomp from carla_msgs.msg import CarlaSpeedometer -from geometry_msgs.msg import Point, PoseStamped, Pose +from geometry_msgs.msg import Point, PoseStamped from nav_msgs.msg import Path from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber 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 -MIN_LD_V: float = 3.0 -LOOK_AHEAD_DIS = 3 -MIN_L_A_DIS = 1 -MAX_L_A_DIS = 15 +# Tuneable Values +K_LAD = 0.85 +MIN_LA_DISTANCE = 2 +MAX_LA_DISTANCE = 25 +# Constants +L_VEHICLE = 2.85 # wheelbase class PurePursuitController(CompatibleNode): @@ -60,26 +61,11 @@ def __init__(self): f"/paf/{self.role_name}/pure_pursuit_steer", qos_profile=1) - self.pure_pursuit_steer_target_pub: Publisher = self.new_publisher( - Pose, - f"/paf/{self.role_name}/pure_pursuit_steer_target_wp", - qos_profile=1) - self.debug_publisher: Publisher = self.new_publisher( Debug, 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 @@ -138,22 +124,10 @@ def __calculate_steer(self) -> float: Calculates the steering angle based on the current information :return: """ - l_vehicle = 2.85 # wheelbase - k_ld = 0.5 # TODO: tune - look_ahead_dist = LOOK_AHEAD_DIS # offset so that ld is never zero - - if self.__velocity < 0: - # backwards driving is not supported, TODO why check this here? - return 0.0 - elif round(self.__velocity, 1) < MIN_LD_V: - # Offset for low velocity state - look_ahead_dist += 0.0 # no offset - else: - look_ahead_dist += k_ld * (self.__velocity - MIN_LD_V) - - # look_ahead_dist = np.clip(k_ld * self.__velocity, - # MIN_L_A_DIS, MAX_L_A_DIS) - + # Simplified alot to increase understanding + # la_dist = MIN_LA_DISTANCE <= K_LAD * velocity <= MAX_LA_DISTANCE + look_ahead_dist = np.clip(K_LAD * self.__velocity, + MIN_LA_DISTANCE, MAX_LA_DISTANCE) # 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] @@ -170,7 +144,7 @@ def __calculate_steer(self) -> float: # 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) + steering_angle = atan((2 * L_VEHICLE * sin(alpha)) / look_ahead_dist) # for debugging -> debug_msg = Debug() @@ -180,21 +154,6 @@ def __calculate_steer(self) -> float: debug_msg.steering_angle = steering_angle self.debug_publisher.publish(debug_msg) # <- - self.pure_pursuit_steer_target_pub.publish(target_wp.pose) - - # not beautiful but works, eliminates the first - # second because for some reason - # the positional data from the GNSS is completely broken at the start - if not self.time_set: - self.checkpoint_time = rospy.get_time() - self.time_set = True - if not self.checker and (self.checkpoint_time < rospy.get_time() - 1): - self.checker = True - - if self.checker: - self.targetwp_publisher.publish((target_wp.pose.position.x-984.5)) - self.currentx_publisher.publish(self.__position[0]-984.5) - return steering_angle def __set_position(self, data: PoseStamped, min_diff=0.001): diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py index 4d892cc5..e03e7817 100755 --- a/code/acting/src/acting/stanley_controller.py +++ b/code/acting/src/acting/stanley_controller.py @@ -169,7 +169,8 @@ def __calculate_steer(self) -> float: using the Stanley algorithm :return: steering angle """ - k_ce = 0.10 # TODO: tune + # TODO: tune both next sprint + k_ce = 0.10 k_v = 1.0 current_velocity: float diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index ea062909..466a1c24 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -50,7 +50,7 @@ def __init__(self): ) # Publisher for which steering-controller is mainly used - # 1 = PurePursuit and 2 = Stanley TODO: needed? + # 1 = PurePursuit and 2 = Stanley self.controller_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/controller", @@ -143,6 +143,7 @@ def run(self): self.status_pub.publish(True) self.loginfo('VehicleController node running') # currently pid for steering is not used, needs fixing + # or maybe deletion since it is not that useful 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) @@ -188,11 +189,9 @@ def loop(timer_event=None) -> None: message.brake = self.__brake message.hand_brake = False message.manual_gear_shift = False - # sets target_steer to steer - # pid.setpoint = self.__map_steering(steer) message.steer = self.__map_steering(steer) - # TEST pure steering: message.steer = self.__map_steering(steer) # Original Code: + # pid.setpoint = self.__map_steering(steer) # message.steer = pid(self.__current_steer) message.gear = 1 message.header.stamp = roscomp.ros_timestamp(self.get_time(), @@ -210,11 +209,10 @@ 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 # 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) + # No tuning needed tune_k = 1 factor for tuning TODO: tune but why? + r = - 1 / (math.pi / 2) # -1 to invert for carla steering + steering_float = steering_angle * r # No Tuning needed * tune_k + self.pidpoint_publisher.publish(steering_float) # TODO needed? return steering_float def __emergency_break(self, data) -> None: diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 9f894cbb..b3ca885b 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -166,7 +166,7 @@ Visualization Manager: Offset: X: 0 Y: 0 - Z: 35 + Z: 668 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 diff --git a/code/perception/src/Position_Publisher_Node.py b/code/perception/src/Position_Publisher_Node.py index 04aa91e4..8e352fa6 100755 --- a/code/perception/src/Position_Publisher_Node.py +++ b/code/perception/src/Position_Publisher_Node.py @@ -85,7 +85,6 @@ def __init__(self): f"/paf/{self.role_name}/current_heading", qos_profile=1) - self.__yaw: float = 0 self.__yaw_publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/yaw", From c6c4eebb3df1480b235e8361800c9419d87b4bfe Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Mon, 22 Jan 2024 14:58:22 +0100 Subject: [PATCH 4/4] feat: Ready for Leaderboard-Use --- build/docker-compose.yml | 4 ++-- code/acting/launch/acting.launch | 10 +++++----- code/acting/src/acting/Acting_DebuggerNode.py | 8 -------- .../src/acting/pure_pursuit_controller.py | 2 -- code/acting/src/acting/vehicle_controller.py | 2 +- code/agent/config/dev_objects.json | 18 +++++++++--------- code/agent/config/rviz_config.rviz | 2 +- code/perception/launch/perception.launch | 9 ++++----- code/perception/src/Position_Publisher_Node.py | 16 ++++------------ .../launch/behavior_agent.launch | 4 ++-- .../launch/global_planner.launch | 4 ++-- .../local_planner/launch/local_planner.launch | 8 ++++---- 12 files changed, 34 insertions(+), 53 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 98e8f19c..92f54ef1 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/acting/launch/acting.launch b/code/acting/launch/acting.launch index bd4166b5..3a705e5d 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -3,6 +3,11 @@ + + @@ -29,11 +34,6 @@ - - - - - diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index 4741ce68..9de92f6c 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -135,14 +135,6 @@ def __init__(self): qos_profile=1 ) - # Subscriber for yaw for plotting - self.yaw_sub: Subscriber = self.new_subscription( - Float32, - f"/paf/{self.role_name}/yaw", - self.__get_yaw, - qos_profile=1 - ) - # Subscriber for current_velocity for plotting self.current_velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index a9adb282..694c0b1e 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -140,8 +140,6 @@ def __calculate_steer(self) -> float: 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) diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 466a1c24..e90d9fa4 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -179,7 +179,7 @@ def loop(timer_event=None) -> None: # 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 + # steer = self.__pure_pursuit_steer self.target_steering_publisher.publish(steer) # debugging diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index f4dc0825..90b24361 100644 --- a/code/agent/config/dev_objects.json +++ b/code/agent/config/dev_objects.json @@ -146,9 +146,9 @@ "y": 0.0, "z": 1.60 }, - "noise_alt_stddev": 0.000000, - "noise_lat_stddev": 0.000000, - "noise_lon_stddev": 0.000000, + "noise_alt_stddev": 0.000005, + "noise_lat_stddev": 0.000005, + "noise_lon_stddev": 0.000005, "noise_alt_bias": 0.0, "noise_lat_bias": 0.0, "noise_lon_bias": 0.0 @@ -164,12 +164,12 @@ "pitch": 0.0, "yaw": 0.0 }, - "noise_accel_stddev_x": 0.000, - "noise_accel_stddev_y": 0.000, - "noise_accel_stddev_z": 0.000, - "noise_gyro_stddev_x": 0.000, - "noise_gyro_stddev_y": 0.000, - "noise_gyro_stddev_z": 0.000 + "noise_accel_stddev_x": 0.001, + "noise_accel_stddev_y": 0.001, + "noise_accel_stddev_z": 0.015, + "noise_gyro_stddev_x": 0.001, + "noise_gyro_stddev_y": 0.001, + "noise_gyro_stddev_z": 0.001 }, { "type": "sensor.pseudo.speedometer", diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index b3ca885b..5c10eca9 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -166,7 +166,7 @@ Visualization Manager: Offset: X: 0 Y: 0 - Z: 668 + 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 03f62545..be0e8d78 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -30,9 +30,9 @@ - + - + - + + diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index 41e2bb6a..398c35ce 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -7,9 +7,9 @@ --> - + \ No newline at end of file diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index 09772836..5fb371a5 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -1,18 +1,18 @@ - + - +