66 feature stanley controller (#200)

# Description

Implementation of the stanley controller. With the testing conducted, I
am confident, that basically all errors have been fixed.
Obviously tuning is still very much needed (#151). Due to a botched
rebase, i created a second branch for this issue and transferred the
previous progress, this should also reduces merge conflicts.

Fixes #66 

## Time invested

- 12 h @schwalga 

## Type of change

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

## Does this PR introduce a breaking change?

no

## Most important changes

stanley_controller.py plus some additional fixes in basic helper
functions.

# 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/CMakeLists.txt

@@ -53,6 +53,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(
  FILES
  Debug.msg
+ StanleyDebug.msg
 )
 
 ## Generate services in the 'srv' folder

code/acting/launch/acting.launch

@@ -8,10 +8,10 @@
  <param name="role_name" value="$(arg role_name)" />
  </node>
 
- <!--<node pkg="acting" type="stanley_controller.py" name="stanley_controller" output="screen">-->
- <!-- <param name="control_loop_rate" value="$(arg control_loop_rate)" />-->
- <!-- <param name="role_name" value="$(arg role_name)" />-->
- <!--</node>-->
+ <node pkg="acting" type="stanley_controller.py" name="stanley_controller" output="screen">
+ <param name="control_loop_rate" value="$(arg control_loop_rate)" />
+ <param name="role_name" value="$(arg role_name)" />
+ </node>
 
  <node pkg="acting" type="velocity_controller.py" name="velocity_controller" output="screen">
  <param name="control_loop_rate" value="$(arg control_loop_rate)" />

code/acting/msg/StanleyDebug.msg

@@ -0,0 +1,5 @@
+float32 heading
+float32 path_heading
+float32 cross_err
+float32 heading_err
+float32 steering_angle

code/acting/src/acting/DummyTrajectoryPublisher.py

@@ -36,23 +36,28 @@ def __init__(self):
 
  # Static trajectory for testing purposes
  self.initial_trajectory = [
- (985.5, -5433.2),
- (983.5, -5443.2),
+ (986.0, -5430.0),
+ (986.0, -5463.2),
+ (984.5, -5493.2),
 
- (983.5, -5563.5),
+ (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.2),
- (1050.0, -5580.2),
- (1060.0, -5580.5),
- (1090.0, -5580.5),
- (1164.6, -5583.0),
- (1264.6, -5583.0)
+ (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.initial_trajectory)

code/acting/src/acting/helper_functions.py

@@ -30,12 +30,12 @@ def vectors_to_angle_abs(x1: float, y1: float, x2: float, y2: float) -> float:
 
 def vector_angle(x1: float, y1: float) -> float:
  """
- Returns the angle (radians) between the two given vectors
+ Returns the angle (radians) of a given vectors
  :param x1: v1[x]
  :param y1: v1[y]
  :return: angle between v1 and x-axis [-pi/2, pi/2]
  """
- # v_0 is a vector parallel on the x-axis
+ # v_0 is a vector parallel to the x-axis
  l_v = math.sqrt(x1**2 + y1**2)
  x_0 = x1 + l_v
  y_0 = 0
@@ -45,13 +45,13 @@ def vector_angle(x1: float, y1: float) -> float:
  if y1 < 0:
  sign = -1
  else:
- sign = 0
+ sign = 1
  return alpha * sign
 
 
 def vector_to_direction(x1, y1, x2, y2) -> float:
  """
- Returns the direction (angle to x-axis) of a vector.
+ Returns the direction (angle to y-axis) of a vector.
  :param x1: tail of the vector [x]
  :param y1: tail of the vector [y]
  :param x2: head of the vector [x]

code/acting/src/acting/pure_pursuit_controller.py

@@ -161,7 +161,7 @@ def __calculate_steer(self) -> float:
  :return:
  """
  l_vehicle = 2.85 # wheelbase
- k_ld = 2.50 # todo: tune
+ k_ld = 2.0 # todo: tune
  look_ahead_dist = 5.0 # offset so that ld is never zero
 
  if round(self.__velocity, 1) < 0.1: