From 3adc42aefcdc883814bd13421132c8cab638ae22 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 30 Nov 2023 14:53:15 +0100 Subject: [PATCH] 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