176 feature refurbish decision tree crusing (#180)

# Description

started to inspect decision tree, to completely include it into the
project. Still work in progress. Started with the easiest behavior,
cruising.
Edit: the cruising behavior is now well commented and works as intended.
However, it might need to be touched again when the ACC is integrated.

Fixes #176 
Is blocked by #170, should be reviewed after that is merged and branch
is updated.

## Time invested

1,5 h Sprint 4
1 h Sprint 5

## Type of change

Please delete options that are not relevant.

- New feature (non-breaking change which adds functionality)

## Does this PR introduce a breaking change?

no

## Most important changes

maneuvers.py

# Checklist:

- [x] My code follows the style guidelines of this project
- [x] I have performed a self-review of my own code
- [x] I have commented my code, particularly in hard-to-understand areas
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works (might be obsolete with CI later on)
- [x] New and existing unit tests pass locally with my changes (might be
obsolete with CI later on)

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)
 
 

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))