From a2e9cfa87347937adcd34119619b9b4abe1d7d9c Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Tue, 21 Nov 2023 15:21:05 +0100 Subject: [PATCH 01/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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/47] 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 a45bd08e92df8639a2c91031d96b9f6ade84c288 Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Thu, 14 Dec 2023 17:31:11 +0100 Subject: [PATCH 25/47] 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 26/47] 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 27/47] 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 28/47] 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 29/47] 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 30/47] 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 31/47] 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 37/47] 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 38/47] 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 39/47] 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 40/47] 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 41/47] 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 42/47] 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 43/47] 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 44/47] 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 45/47] 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 46/47] 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()