diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index f608a54c..5c549f47 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -29,7 +29,7 @@ def grow_a_tree(role_name): Sequence("Intersection", children=[ behaviours.road_features.IntersectionAhead - ("Intersection Ahead"), + ("Intersection Ahead?"), Sequence("Intersection Actions", children=[ behaviours.intersection.Approach @@ -47,7 +47,7 @@ def grow_a_tree(role_name): Sequence("Laneswitch", children=[ behaviours.road_features.LaneChangeAhead - ("Lane Change Ahead"), + ("Lane Change Ahead?"), Sequence("Lane Change Actions", children=[ behaviours.lane_change.Approach @@ -60,6 +60,22 @@ def grow_a_tree(role_name): ("Leave Change") ]) ]), + Sequence("Overtaking", + children=[ + behaviours.road_features.OvertakeAhead + ("Overtake Ahead?"), + Sequence("Overtake Actions", + children=[ + behaviours.overtake.Approach + ("Approach Overtake"), + behaviours.overtake.Wait + ("Wait Overtake"), + behaviours.overtake.Enter + ("Enter Overtake"), + behaviours.overtake.Leave + ("Leave Overtake") + ]) + ]), ]), behaviours.maneuvers.Cruise("Cruise") diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 33e35946..673cb99a 100755 --- a/code/planning/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -285,85 +285,6 @@ def terminate(self, new_status): (self.name, self.status, new_status)) -class Overtake(py_trees.behaviour.Behaviour): - """ - This behavior handles the overtaking process. - """ - 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(Overtake, 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. - :param timeout: an initial timeout to see if the tree generation is - successful - :return: True, as there is nothing to set up. - """ - self.Success = False - self.blackboard = py_trees.blackboard.Blackboard() - 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. - - :return: True - """ - 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 - - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - - This behaviour runs until the agent is on a different lane as in the - start of this behavior - - :return: py_trees.common.Status.SUCCESS, if this behavior is - implemented - py_trees.common.Status.FAILURE, otherwise - """ - if self.Success: - return py_trees.common.Status.SUCCESS - else: - return py_trees.common.Status.RUNNING - - 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 Cruise(py_trees.behaviour.Behaviour): """ This behaviour is the lowest priority one and will be executed when no diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py new file mode 100644 index 00000000..322f25df --- /dev/null +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -0,0 +1,404 @@ +import py_trees +from std_msgs.msg import String + +import rospy + +from . import behavior_speed as bs + +""" +Source: https://github.com/ll7/psaf2 +""" + + +def convert_to_ms(speed): + return speed / 3.6 + + +class Approach(py_trees.behaviour.Behaviour): + """ + This behaviour is executed when the ego vehicle is in close proximity of + an object which needs to be overtaken and + behaviours.road_features.overtake_ahead is triggered. + It than handles the procedure for overtaking. + """ + def __init__(self, name): + """ + Minimal one-time initialisation. Other one-time initialisation + requirements should be met via the setup() method. + :param name: name of the behaviour + """ + super(Approach, self).__init__(name) + rospy.loginfo("Init -> Overtake Behavior: Approach") + + 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 the current behavior publisher. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", + String, queue_size=1) + self.blackboard = py_trees.blackboard.Blackboard() + 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. + + This initializes the overtaking distance to a default value. + """ + rospy.loginfo("Approaching Overtake") + self.ot_distance = 30 + + self.ot_option = 1 # self.blackboard.get("paf/hero/...") + if self.ot_option == 0: + self.clear_distance = 15 + self.side = "LIDAR_range_rear_left" + elif self.ot_option == 1: + self.clear_distance = 30 + self.side = "LIDAR_range" + + 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] + + Gets the current distance to overtake, the current lane status and the + distance to collsion object. + :return: py_trees.common.Status.RUNNING, if too far from overtaking + py_trees.common.Status.SUCCESS, if stopped behind the blocking + object or entered the process. + py_trees.common.Status.FAILURE, + """ + # Update distance to collision object + _dis = self.blackboard.get("/paf/hero/LIDAR_range") + if _dis is not None: + self.ot_distance = _dis.data + rospy.loginfo(f"Overtake distance: {self.ot_distance}") + + # slow down before overtake if blocked + if self.ot_distance < 15.0: + distance_lidar = self.blackboard. \ + get(f"/carla/hero/{self.side}") + if distance_lidar is not None: + distance_lidar = distance_lidar.data + else: + distance_lidar = None + + if distance_lidar is not None and \ + distance_lidar > self.clear_distance: + rospy.loginfo("Overtake is free not slowing down!") + return py_trees.common.Status.SUCCESS + else: + rospy.loginfo("Overtake blocked slowing down") + self.curr_behavior_pub.publish(bs.ot_app_blocked.name) + + # get speed + speedometer = self.blackboard.get("/carla/hero/Speed") + if speedometer is not None: + speed = speedometer.speed + else: + rospy.logwarn("no speedometer connected") + return py_trees.common.Status.RUNNING + + if self.ot_distance > 15.0: + # too far + rospy.loginfo("still approaching") + return py_trees.common.Status.RUNNING + elif speed < convert_to_ms(2.0) and \ + self.ot_distance < 5.0: + # stopped + rospy.loginfo("stopped") + return py_trees.common.Status.SUCCESS + else: + rospy.logerr("Overtake Approach: Default Case") + return py_trees.common.Status.RUNNING + + 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 + :param new_status: new state after this one is terminated + """ + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, + self.status, + new_status)) + + +class Wait(py_trees.behaviour.Behaviour): + """ + This behavior handles the waiting in front of object, + which is blocking the road. + The Ego vehicle is waiting to get a clear path for overtaking. + """ + def __init__(self, name): + """ + Minimal one-time initialisation. Other one-time initialisation + requirements should be met via the setup() method. + :param name: name of the behaviour + """ + super(Wait, 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 the current behavior publisher. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", String, + queue_size=1) + self.blackboard = py_trees.blackboard.Blackboard() + 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. + This just prints a state status message. + """ + rospy.loginfo("Waiting for Overtake") + 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 + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + + Waits behind the road object until the lidar distance meets the + requirement for the clear distance. + + :return: py_trees.common.Status.RUNNING, while clear distance is bigger + than lidar_distance + py_trees.common.Status.SUCCESS,if no lidar distance is present + or the lidar distance is bigger than the clear distance + """ + + # Update distance to collison and distance for clear + self.ot_option = 1 # self.blackboard.get("paf/hero/...") + if self.ot_option == 0: + distance_lidar = self.blackboard. \ + get("/carla/hero/LIDAR_range_rear_left") + clear_distance = 15 + elif self.ot_option == 1: + distance_lidar = self.blackboard. \ + get("/carla/hero/LIDAR_range") + distance_lidar = distance_lidar.data + clear_distance = 30 + else: + distance_lidar = None + + if distance_lidar is not None: + collision_distance = distance_lidar.data + if collision_distance > clear_distance: + rospy.loginfo("Overtake is free!") + return py_trees.common.Status.SUCCESS + else: + rospy.loginfo("Overtake still blocked") + self.curr_behavior_pub.publish(bs.ot_wait_stopped.name) + return py_trees.commom.Status.RUNNING + else: + rospy.loginfo("No Lidar Distance") + return py_trees.common.Success + + 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 + :param new_status: new state after this one is terminated + """ + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, + self.status, + new_status)) + + +class Enter(py_trees.behaviour.Behaviour): + """ + This behavior handles the switching to a new lane in the + overtaking procedure. + """ + def __init__(self, name): + """ + Minimal one-time initialisation. Other one-time initialisation + requirements should be met via the setup() method. + :param name: name of the behaviour + """ + super(Enter, 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 the current behavior publisher. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", String, + queue_size=1) + self.blackboard = py_trees.blackboard.Blackboard() + 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. + + This prints a state status message and publishes the behavior to + trigger the replanning + """ + rospy.loginfo("Enter Overtake") + self.curr_behavior_pub.publish(bs.ot_enter_init.name) + + 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] + + + :return: py_trees.common.Status.RUNNING, + py_trees.common.Status.SUCCESS, + py_trees.common.Status.FAILURE, + """ + + return py_trees.common.Status.SUCCESS + + # Currently not in use + # Can be used to check if we can go back to the original lane + + 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 + :param new_status: new state after this one is terminated + """ + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, + self.status, + new_status)) + + +class Leave(py_trees.behaviour.Behaviour): + """ + This behaviour defines the leaf of this subtree, if this behavior is + reached, the vehicle peformed the overtake. + """ + def __init__(self, name): + """ + Minimal one-time initialisation. Other one-time initialisation + requirements should be met via the setup() method. + :param name: name of the behaviour + """ + super(Leave, 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 the current behavior publisher. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", String, + queue_size=1) + self.blackboard = py_trees.blackboard.Blackboard() + 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. + This prints a state status message and publishes the behavior + """ + rospy.loginfo("Leave Overtake") + self.curr_behavior_pub.publish(bs.ot_leave.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 + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + Abort this subtree + :return: py_trees.common.Status.FAILURE, to exit this subtree + """ + 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 + :param new_status: new state after this one is terminated + """ + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, + self.status, + new_status)) diff --git a/code/planning/src/behavior_agent/behaviours/road_features.py b/code/planning/src/behavior_agent/behaviours/road_features.py index dbe255c6..0e6786db 100755 --- a/code/planning/src/behavior_agent/behaviours/road_features.py +++ b/code/planning/src/behavior_agent/behaviours/road_features.py @@ -2,6 +2,7 @@ # -*- coding: utf-8 -*- import py_trees +import numpy as np """ @@ -177,6 +178,91 @@ def terminate(self, new_status): (self.name, self.status, new_status)) +class OvertakeAhead(py_trees.behaviour.Behaviour): + """ + This behaviour checks whether an object that needs to be overtaken is + ahead + """ + 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(OvertakeAhead, 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. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.blackboard = py_trees.blackboard.Blackboard() + 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. + """ + 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 + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + + Gets the current distance and speed to object in front. + :return: py_trees.common.Status.SUCCESS, if the vehicle is within range + for the overtaking procedure + py_trees.common.Status.FAILURE, if we are too far away for + the overtaking procedure + """ + + obstacle_msg = self.blackboard.get("/paf/hero/collision") + if obstacle_msg is None: + return py_trees.common.Status.FAILURE + + obstacle_distance = obstacle_msg.data[0] + obstacle_speed = obstacle_msg.data[1] + + if obstacle_distance == np.Inf: + return py_trees.common.Status.FAILURE + + if obstacle_speed < 2 and obstacle_distance < 30: + return py_trees.common.Status.SUCCESS + 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 + :param new_status: new state after this one is terminated + """ + self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % + (self.name, self.status, new_status)) + + class MultiLane(py_trees.behaviour.Behaviour): """ This behavior decides if the road the agent is currently on, has more than diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index 8dd4014c..81ce9eb1 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -4,7 +4,7 @@ import py_trees import py_trees_ros -from std_msgs.msg import Float32, Bool +from std_msgs.msg import Float32, Bool, Float32MultiArray from carla_msgs.msg import CarlaSpeedometer from sensor_msgs.msg import Range from geometry_msgs.msg import PoseStamped @@ -49,6 +49,8 @@ def create_node(role_name): '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}, + {'name': f"/paf/{role_name}/collision", 'msg': Float32MultiArray, + 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, {'name': f"/paf/{role_name}/current_pos", 'msg': PoseStamped, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} ]