From d987e43242001581333de2a3a75bdb11bbb2c405 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 11 Jan 2024 23:38:12 +0100 Subject: [PATCH 1/8] feat: New behavior added, testing pending --- .../src/behavior_agent/behavior_tree.py | 1 + .../behavior_agent/behaviours/maneuvers.py | 91 +++++++++++++++++++ .../src/local_planner/behavior_speed.py | 3 + .../src/local_planner/collision_check.py | 2 +- .../src/local_planner/motion_planning.py | 26 +++++- 5 files changed, 120 insertions(+), 3 deletions(-) diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 990b9e1e..931999fe 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -27,6 +27,7 @@ def grow_a_tree(role_name): children=[ Selector("Road Features", children=[ + behaviours.maneuvers.LeaveParkingSpace("LeaveParkingSpace"), Sequence("Intersection", children=[ behaviours.road_features.IntersectionAhead diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 5a17c25a..6b4c8eb6 100755 --- a/code/planning/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -1,6 +1,7 @@ import py_trees import rospy from std_msgs.msg import String +import numpy as np from . import behavior_speed as bs # from behavior_agent.msg import BehaviorSpeed @@ -10,6 +11,96 @@ """ +class LeaveParkingSpace(py_trees.behaviour.Behaviour): + """ + This behavior is triggered in the beginning when the vehicle needs + to leave the parking space. + """ + def __init__(self, name): + """ + Minimal one-time initialisation. A good rule of thumb is to only + include the initialisation relevant for being able to insert this + behaviour in a tree for offline rendering to dot graphs. + + :param name: name of the behaviour + """ + super(SwitchLaneLeft, self).__init__(name) + + def setup(self, timeout): + """ + Delayed one-time initialisation that would otherwise interfere with + offline rendering of this behaviour in a tree to dot graph or + validation of the behaviour's configuration. + + This initializes the blackboard to be able to access data written to it + by the ROS topics and gathers the time to check how much time has passed. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as there is nothing to set up. + """ + self.blackboard = py_trees.blackboard.Blackboard() + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", + String, queue_size=1) + self.initRosTime = rospy.get_rostime() + return True + + def initialise(self): + """ + When is this called? + The first time your behaviour is ticked and anytime the status is not + RUNNING thereafter. + + What to do here? + Any initialisation you need before putting your behaviour to work. + Get initial position to check how far vehicle has moved during execution + """ + self.initPosition = self.blackboard.get("/paf/hero/current_pos") + + 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 + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + + This behaviour runs until the agent has left the parking space. + This is checked by calculating the euclidian distance thath the agent has moved since the start + + :return: py_trees.common.Status.RUNNING, while the agent is leaving the parking space + py_trees.common.Status.SUCCESS, never to continue with intersection + py_trees.common.Status.FAILURE, if not in parking + lane + """ + position = self.blackboard.get("/paf/hero/current_pos") + # calculate distance between start and current position + startPos = np.array([position.pose.position.x, position.pose.position.y]) + endPos = np.array([self.initPosition.pose.position.x, self.initPosition.pose.position.y]) + distance = np.linalg.norm(startPos - endPos) + # Additionally the behavior is only executed for 10 seconds + if distance < 3 and rospy.get_rostime() - self.initRosTime < rospy.Time(10): + self.curr_behavior_pub.publish(bs.parking) + return py_trees.common.Status.RUNNING + else: + return py_trees.common.Status.FAILURE + + def terminate(self, new_status): + """ + When is this called? + Whenever your behaviour switches to a non-running state. + - SUCCESS || FAILURE : your behaviour's work cycle has finished + - INVALID : a higher priority branch has interrupted, or shutting + down + + writes a status message to the console when the behaviour terminates + """ + self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % + (self.name, self.status, new_status)) + + class SwitchLaneLeft(py_trees.behaviour.Behaviour): """ This behavior triggers the replanning of the path in the local planner to diff --git a/code/planning/src/local_planner/behavior_speed.py b/code/planning/src/local_planner/behavior_speed.py index 4eb9cf47..f117c248 100755 --- a/code/planning/src/local_planner/behavior_speed.py +++ b/code/planning/src/local_planner/behavior_speed.py @@ -10,6 +10,9 @@ def convert_to_ms(speed): # Changed target_speed_pub to curr_behavior_pub +# Leave Parking space + +parking = Behavior("parking", convert_to_ms(7.0)) # Intersection - Behaviors # Approach diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 3cc107d3..2aadff2f 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -42,7 +42,7 @@ def __init__(self): # Publisher for emergency stop self.emergency_pub = self.new_publisher( Bool, - f"/paf/{self.role_name}/emergency", + f"/paf/{self.role_name}/unchecked_emergency", qos_profile=1) # Publisher for distance to collision self.collision_pub = self.new_publisher( diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 92949cad..49d3c43f 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -4,7 +4,7 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber -from std_msgs.msg import String, Float32 +from std_msgs.msg import String, Float32, Bool import numpy as np # from behavior_agent.msg import BehaviorSpeed @@ -47,7 +47,11 @@ def __init__(self): f"/paf/{self.role_name}/curr_behavior", self.__set_curr_behavior, qos_profile=1) - + self.emergency_sub: Subscriber = self.new_subscription( + Bool, + f"/paf/{self.role_name}/unchecked_emergency", + self.__check_emergency, + qos_profile=1) self.acc_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/acc_velocity", @@ -72,8 +76,24 @@ def __init__(self): f"/paf/{self.role_name}/target_velocity", qos_profile=1) + # Publisher for emergency stop + self.emergency_pub = self.new_publisher( + Bool, + f"/paf/{self.role_name}/emergency", + qos_profile=1) + self.logdebug("MotionPlanning started") + def __check_emergency(self, data: Bool): + """If an emergency stop is needed first check if we are + in parking behavior. If we are ignore the emergency stop. + + Args: + data (Bool): True if emergency stop detected by collision check + """ + if self.__curr_behavior is not bs.parking.name: + self.emergency_pub.publish(data) + def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) @@ -103,6 +123,8 @@ def get_speed_by_behavior(self, behavior: str) -> float: speed = self.__get_speed_intersection(behavior) elif short_behavior == "lc": speed = self.__get_speed_lanechange(behavior) + elif short_behavior == "parking": + speed = bs.parking.speed else: speed = self.__get_speed_cruise() From 480a652082ec824ee881fd9af545a305f2fd0002 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 12 Jan 2024 18:00:39 +0100 Subject: [PATCH 2/8] fix: behavior adjusted --- .flake8 | 5 +- build/docker-compose.yml | 2 +- code/acting/src/acting/vehicle_controller.py | 2 +- .../perception/src/Position_Publisher_Node.py | 2 +- .../src/behavior_agent/behavior_tree.py | 3 +- .../behaviours/behavior_speed.py | 3 + .../behavior_agent/behaviours/maneuvers.py | 36 +++++++---- .../behaviours/topics2blackboard.py | 5 +- code/planning/src/local_planner/ACC.py | 6 +- .../src/local_planner/collision_check.py | 61 +++++++++++-------- .../src/local_planner/motion_planning.py | 11 ++-- 11 files changed, 83 insertions(+), 53 deletions(-) diff --git a/.flake8 b/.flake8 index 15b36b64..1c9d8ead 100644 --- a/.flake8 +++ b/.flake8 @@ -1,3 +1,4 @@ [flake8] -exclude= code/planning/behavior_agent/src/behavior_agent/behavior_tree.py, - code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py +exclude= code/planning/src/behavior_agent/behavior_tree.py, + code/planning/src/behavior_agent/behaviours/__init__.py, + code/planning/src/behavior_agent/behaviours diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 92f54ef1..a44d725e 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -57,7 +57,7 @@ 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 && 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: diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 314e5bcd..ea062909 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -210,7 +210,7 @@ def __map_steering(self, steering_angle: float) -> float: :param steering_angle: calculated by a controller in [-pi/2 , pi/2] :return: float for steering in [-1, 1] """ - tune_k = 1 # factor for tuning TODO: tune but why? + tune_k = -1 # factor for tuning TODO: tune but why? # negative because carla steer and our steering controllers are flipped r = 1 / (math.pi / 2) steering_float = steering_angle * r * tune_k diff --git a/code/perception/src/Position_Publisher_Node.py b/code/perception/src/Position_Publisher_Node.py index 8c0431c2..37990750 100755 --- a/code/perception/src/Position_Publisher_Node.py +++ b/code/perception/src/Position_Publisher_Node.py @@ -163,7 +163,7 @@ def update_imu_data(self, data: Imu): # --------------------------------------------------------------- heading = (raw_heading - (math.pi / 2)) % (2 * math.pi) - math.pi self.__heading = heading - self.__heading_publisher.publish(self.__heading) + self.__heading_publisher.publish(Float32(data=yaw)) def update_gps_data(self, data: NavSatFix): """ diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 931999fe..42ca1ea9 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -27,7 +27,7 @@ def grow_a_tree(role_name): children=[ Selector("Road Features", children=[ - behaviours.maneuvers.LeaveParkingSpace("LeaveParkingSpace"), + behaviours.maneuvers.LeaveParkingSpace("Leave Parking Space"), Sequence("Intersection", children=[ behaviours.road_features.IntersectionAhead @@ -104,6 +104,5 @@ def main(): except rospy.ROSInterruptException: pass - if __name__ == "__main__": main() diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index 4eb9cf47..41e75530 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -9,6 +9,9 @@ def convert_to_ms(speed): Behavior = namedtuple("Behavior", ("name", "speed")) # Changed target_speed_pub to curr_behavior_pub +# Leave Parking space + +parking = Behavior("parking", convert_to_ms(30.0)) # Intersection - Behaviors diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 6b4c8eb6..b8074698 100755 --- a/code/planning/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -2,7 +2,7 @@ import rospy from std_msgs.msg import String import numpy as np - +import time from . import behavior_speed as bs # from behavior_agent.msg import BehaviorSpeed @@ -24,7 +24,8 @@ def __init__(self, name): :param name: name of the behaviour """ - super(SwitchLaneLeft, self).__init__(name) + super(LeaveParkingSpace, self).__init__(name) + rospy.loginfo("LeaveParkingSpace started") def setup(self, timeout): """ @@ -38,11 +39,12 @@ def setup(self, timeout): successful :return: True, as there is nothing to set up. """ - self.blackboard = py_trees.blackboard.Blackboard() self.curr_behavior_pub = rospy.Publisher("/paf/hero/" "curr_behavior", String, queue_size=1) - self.initRosTime = rospy.get_rostime() + self.initRosTime = time.time() + # rospy.loginfo("LEAVEPARKINGSPACE setup" + str(self.initRosTime)) + self.blackboard = py_trees.blackboard.Blackboard() return True def initialise(self): @@ -76,16 +78,24 @@ def update(self): lane """ position = self.blackboard.get("/paf/hero/current_pos") - # calculate distance between start and current position - startPos = np.array([position.pose.position.x, position.pose.position.y]) - endPos = np.array([self.initPosition.pose.position.x, self.initPosition.pose.position.y]) - distance = np.linalg.norm(startPos - endPos) - # Additionally the behavior is only executed for 10 seconds - if distance < 3 and rospy.get_rostime() - self.initRosTime < rospy.Time(10): - self.curr_behavior_pub.publish(bs.parking) - return py_trees.common.Status.RUNNING + rospy.logerr(position) + rospy.logerr(self.initPosition) + # calculate distance between start and current position + if position is not None and self.initPosition is not None: + rospy.logerr(position.pose.x + " " + position.pose.y) + startPos = np.array([position.pose.position.x, position.pose.position.y]) + endPos = np.array([self.initPosition.pose.position.x, self.initPosition.pose.position.y]) + distance = np.linalg.norm(startPos - endPos) + # Additionally the behavior is only executed for 10 seconds + if distance < 4: + self.curr_behavior_pub.publish(bs.parking) + self.initPosition = position + return py_trees.common.Status.RUNNING + else: + return py_trees.common.Status.SUCCESS else: - return py_trees.common.Status.FAILURE + self.initPosition = position + return py_trees.common.Status.RUNNING def terminate(self, new_status): """ diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index e4842f11..8dd4014c 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -7,6 +7,7 @@ from std_msgs.msg import Float32, Bool from carla_msgs.msg import CarlaSpeedometer from sensor_msgs.msg import Range +from geometry_msgs.msg import PoseStamped from mock.msg import Traffic_light, Stop_sign from perception.msg import Waypoint, LaneChange @@ -47,7 +48,9 @@ def create_node(role_name): {'name': f"/paf/{role_name}/speed_limit", 'msg': Float32, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/lane_change_distance", 'msg': LaneChange, - 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE} + 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, + {'name': f"/paf/{role_name}/current_pos", 'msg': PoseStamped, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} ] topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard") diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 3849dc6a..84853c54 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -22,7 +22,7 @@ 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.current_speed = None # m/ss # Get current speed self.velocity_sub: Subscriber = self.new_subscription( @@ -35,7 +35,7 @@ def __init__(self): # TODO: Change to real lidar distance self.lidar_dist = self.new_subscription( MinDistance, - f"/paf/{self.role_name}/Center/min_distance", + f"/paf/{self.role_name}/LIDAR_range", self._set_distance, qos_profile=1) # Get initial set of speed limits @@ -52,7 +52,7 @@ def __init__(self): self.__set_trajectory, qos_profile=1) - self.emergency_sub: Subscriber = self.new_subscription( + self.pose_sub: Subscriber = self.new_subscription( msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 2aadff2f..6a385031 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -36,8 +36,8 @@ def __init__(self): # TODO: Change to real lidar distance self.lidar_dist = self.new_subscription( MinDistance, - f"/paf/{self.role_name}/Center/min_distance", - self.calculate_obstacle_speed, + f"/paf/{self.role_name}/LIDAR_range", + self.__set_distance, qos_profile=1) # Publisher for emergency stop self.emergency_pub = self.new_publisher( @@ -56,42 +56,46 @@ def __init__(self): qos_profile=1) # Variables to save vehicle data self.__current_velocity: float = None + self.__object_first_position: tuple = None self.__object_last_position: tuple = None self.logdebug("CollisionCheck started") - def calculate_obstacle_speed(self, new_dist: MinDistance): - """Caluclate the speed of the obstacle in front of the ego vehicle - based on the distance between to timestamps + def __set_distance(self, data: MinDistance): + """Saves the distance from the lidar Args: - new_position (MinDistance): new position received from the lidar + data (MinDistance): Message from lidar with distance """ - # Check if current speed from vehicle is not None - if self.__current_velocity is None: + if np.isinf(data.distance): + self.__object_last_position = None + self.__object_first_position = None return - # Check if this is the first time the callback is called - if self.__object_last_position is None and \ - np.isinf(new_dist.distance) is not True: - self.__object_last_position = (time.time(), - new_dist.distance) + if self.__object_first_position is None: + self.__object_first_position = (time.time(), data.distance) return - - # If distance is np.inf no car is in front - if np.isinf(new_dist.distance): - self.__object_last_position = None + if self.__object_last_position is None: + self.__object_last_position = (time.time(), data.distance) return - # Check if too much time has passed since last position update - if self.__object_last_position[0] + \ - 0.5 < time.time(): - self.__object_last_position = (time.time(), - new_dist.distance) + self.__object_first_position = self.__object_last_position + self.__object_last_position = (time.time(), data.distance) + + def calculate_obstacle_speed(self): + """Caluclate the speed of the obstacle in front of the ego vehicle + based on the distance between to timestamps + """ + # Check if current speed from vehicle is not None + if self.__current_velocity is None or \ + self.__object_first_position is None or \ + self.__object_last_position is None: return + # If distance is np.inf no car is in front + # Calculate time since last position update current_time = time.time() time_difference = current_time-self.__object_last_position[0] # Calculate distance (in m) - distance = new_dist.distance - self.__object_last_position[1] + distance = self.__object_last_position[1] - self.__object_first_position[1] # Speed is distance/time (m/s) relative_speed = distance/time_difference @@ -99,8 +103,7 @@ def calculate_obstacle_speed(self, new_dist: MinDistance): # Publish speed to ACC for permanent distance check self.speed_publisher.publish(Float32(data=speed)) # Check for crash - self.check_crash((new_dist.distance, speed)) - self.__object_last_position = (current_time, new_dist.distance) + self.check_crash((self.__object_last_position[1], speed)) def __get_current_velocity(self, data: CarlaSpeedometer,): """Saves current velocity of the ego vehicle @@ -174,6 +177,7 @@ def check_crash(self, obstacle): 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 object data = Float32MultiArray(data=[distance, obstacle_speed]) @@ -188,6 +192,13 @@ def run(self): Control loop :return: """ + def loop(timer_event=None): + """ + Checks if distance to a possible object is too small and + publishes the desired speed to motion planning + """ + self.calculate_obstacle_speed() + self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 49d3c43f..0c502d96 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -81,11 +81,11 @@ def __init__(self): Bool, f"/paf/{self.role_name}/emergency", qos_profile=1) - + self.logdebug("MotionPlanning started") def __check_emergency(self, data: Bool): - """If an emergency stop is needed first check if we are + """If an emergency stop is needed first check if we are in parking behavior. If we are ignore the emergency stop. Args: @@ -96,8 +96,11 @@ def __check_emergency(self, data: Bool): def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) - - self.target_speed = min(be_speed, acc_speed) + if self.__curr_behavior is not bs.parking.name: + self.target_speed = min(be_speed, acc_speed) + else: + self.target_speed = be_speed + self.logerr("parking") self.velocity_pub.publish(self.target_speed) def __set_acc_speed(self, data: Float32): From 3e23e1565136055105bc456924a16d5150699687 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 14 Jan 2024 17:35:12 +0100 Subject: [PATCH 3/8] fix: Behavior tested --- code/planning/launch/planning.launch | 2 +- .../behavior_agent/behaviours/maneuvers.py | 61 ++++++++++++------- .../src/local_planner/behavior_speed.py | 2 +- .../src/local_planner/collision_check.py | 3 +- .../src/local_planner/motion_planning.py | 6 +- 5 files changed, 46 insertions(+), 28 deletions(-) diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index c6377c79..ba842d1e 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -5,7 +5,7 @@ - + - +