diff --git a/code/acting/src/acting/helper_functions.py b/code/acting/src/acting/helper_functions.py index 2bd6e1d0..c64319e7 100755 --- a/code/acting/src/acting/helper_functions.py +++ b/code/acting/src/acting/helper_functions.py @@ -102,10 +102,10 @@ def calc_path_yaw(path: Path, idx: int) -> float: point_current = path.poses[idx] point_next: PoseStamped point_next = path.poses[idx + 1] - angle = math.atan2(point_next.pose.position.y - - point_current.pose.position.y, - point_next.pose.position.x - - point_current.pose.position.x) + angle = math.atan2(point_next.pose.position.y - + point_current.pose.position.y, + point_next.pose.position.x - + point_current.pose.position.x) return normalize_angle(angle) diff --git a/code/test_planning_top/behavior_agent/src/behavior_agent/behaviours/maneuvers.py b/code/test_planning_top/behavior_agent/src/behavior_agent/behaviours/maneuvers.py index 1c3ebe52..ed0dbbc0 100755 --- a/code/test_planning_top/behavior_agent/src/behavior_agent/behaviours/maneuvers.py +++ b/code/test_planning_top/behavior_agent/src/behavior_agent/behaviours/maneuvers.py @@ -91,18 +91,83 @@ def terminate(self, new_status): class Cruise(py_trees.behaviour.Behaviour): + """ + This behaviour is the lowest priority one and will be executed when no + other behaviour is triggered. It doesn't do much, as in the normal cruising + the holding of the lane and speed control is done by different parts of the + project. It might be possible to put the activation/deactivation of the ACC + here. + + speed control = acting via speed limits and target_speed + following the trajectory = acting + """ 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. + + Other one-time initialisation requirements should be met via the + setup() method. + + :param name: name of the behaviour + """ super(Cruise, 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. + + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as there is nothing to set up. + """ 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 blackboard to be able to access data written to it + by the ROS topics. + """ self.blackboard = py_trees.blackboard.Blackboard() 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 doesn't do anything else than just keep running unless + there is a higher priority behaviour + + :return: py_trees.common.Status.RUNNING, keeps the decision tree from + finishing + """ + # rospy.loginfo("Cruising around") 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))