67 feature implement pure pursuit controller

code/acting/launch/acting.launch

@@ -1,15 +1,24 @@
+<!-- TODO: Insert components of acting component-->
 <launch>
  <arg name="role_name" default="ego_vehicle" />
  <arg name="control_loop_rate" default="0.1" />
 
+ <node pkg="acting" type="pure_pursuit_controller.py" name="pure_pursuit_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)" />
  <param name="role_name" value="$(arg role_name)" />
  </node>
  <node pkg="acting" type="velocity_publisher_dummy.py" name="velocity_publisher_dummy" output="screen">
  <param name="role_name" value="$(arg role_name)" />
  <param name="control_loop_rate" value="0.2" />
- <param name="enabled" value="False" /> <!-- set to True to publish dummy velocities for testing-->
+ <param name="enabled" value="True" /> <!-- set to True to publish dummy velocities for testing-->
  </node>
 
  <node pkg="acting" type="vehicle_controller.py" name="vehicle_controller" output="screen">
@@ -24,5 +33,5 @@
  <node pkg="acting" type="DummyTrajectorySub.py" name="DummyTrajectorySub" output="screen">
  <param name="control_loop_rate" value="1" />
  <param name="role_name" value="$(arg role_name)" />
- </node>
+  </node>
 </launch>

code/acting/src/acting/DummyTrajectoryPublisher.py

@@ -38,9 +38,10 @@ def __init__(self):
  initial_trajectory = [
  (985.0, -5373.2),
  (985.0, -5473.2),
- (990.0, -5474.2),
- (990.0, -5524.2),
- (990.0, -5574.2)
+ (990.0, -5493.2),
+ (985.0, -5513.2),
+ (990.0, -5533.2),
+ (985.0, -5574.2)
  ]
  self.updated_trajectory(initial_trajectory)
 
@@ -90,7 +91,6 @@ def updated_trajectory(self, target_trajectory):
  def run(self):
  """
  Control loop
-
  :return:
  """
 
@@ -105,7 +105,6 @@ def loop(timer_event=None):
 def main(args=None):
  """
  main function
-
  :param args:
  :return:
  """

code/acting/src/acting/DummyTrajectorySub.py

@@ -81,7 +81,7 @@ def output_gps_2_xyz(self, data: NavSatFix):
  lat = data.latitude
  lon = data.longitude
  h = data.altitude
- x, y, z = self.transformer.gnss_to_xyz(self, lat, lon, h)
+ x, y, z = self.transformer.gnss_to_xyz(lat, lon, h)
  self.update_pose(x, y, z)
  # self.loginfo(f"x: {x}\t y: {y}\t z:{z}")
 
@@ -106,12 +106,17 @@ def output_average_gps_2_xyz(self, data: NavSatFix):
  self.pos_average[1] += y
  self.pos_average[2] += z
 
- if self.pos_counter % 5 == 0:
- x1 = self.pos_average[0] / 5
- y1 = self.pos_average[1] / 5
- z1 = self.pos_average[2] / 5
+ w: float = 0.75 # weight of the new position compared to the old
+ avg: int = 10
+ if self.pos_counter % avg == 0:
+ x1 = self.pos_average[0] / avg
+ y1 = self.pos_average[1] / avg
+ z1 = self.pos_average[2] / avg
  self.pos_average = [0, 0, 0]
- self.update_pose(x1, y1, z1)
+ r_x = w * x1 + (1.0 - w) * self.current_pos.pose.position.x
+ r_y = w * y1 + (1.0 - w) * self.current_pos.pose.position.y
+ r_z = w * z1 + (1.0 - w) * self.current_pos.pose.position.z
+ self.update_pose(r_x, r_y, r_z)
  # self.loginfo(f"x: {x1}\t y: {y1}\t z:{z1}")
 
  self.pos_counter += 1
@@ -163,7 +168,6 @@ def update_pose(self, x, y, z):
  def run(self):
  """
  Control loop
-
  :return:
  """
  self.spin()
@@ -172,7 +176,6 @@ def run(self):
 def main(args=None):
  """
  main function
-
  :param args:
  :return:
  """

code/acting/src/acting/coordinate_transformation.py

@@ -4,7 +4,6 @@
 (the composition of the two previous functions).
 Running the script by itself runs tests.
 based on https://gist.github.com/govert/1b373696c9a27ff4c72a.
-
 A good source to read up on the different reference frames is:
 http://dirsig.cis.rit.edu/docs/new/coordinates.html
 """

code/acting/src/acting/helper_functions.py

@@ -0,0 +1,116 @@
+"""
+Helper functions for calculations
+inspired by: PSAF 2 WS 20/21 (Acting package)
+"""
+
+import math
+import numpy as np
+from geometry_msgs.msg import PoseStamped
+from tf.transformations import euler_from_quaternion
+from nav_msgs.msg import Path
+from scipy.spatial.transform import Rotation
+
+
+def vectors_to_angle(x1: float, y1: float, x2: float, y2: float) -> float:
+ """
+ Returns the angle (degrees) between the two given vectors
+ :param x1: v1[x]
+ :param y1: v1[y]
+ :param x2: v2[x]
+ :param y2: v2[y]
+ :return: angle between v1 and v2
+ """
+ v1 = [x1, y1]
+ v2 = [x2, y2]
+
+ cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
+ alpha = np.arccos(cos_angle)
+
+ return math.degrees(alpha)
+
+
+def vector_to_direction(x1, y1, x2, y2) -> float:
+ """
+ Returns the direction (angle to x-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]
+ :param y2: head of the vector [y]
+ :return: direction of the vector
+ """
+ theta = math.atan(x2 - x1 / y2 - y1)
+ return math.degrees(theta)
+
+
+def quaternion_to_heading(x: float, y: float, z: float, w: float) -> float:
+ """
+ Translates quaternion to euler heading.
+ :param x:
+ :param y:
+ :param z:
+ :param w:
+ :return: euler heading of the given quaternion
+ """
+ quaternion = (x, y, z, w)
+ rot = Rotation.from_quat(quaternion)
+ rot_euler = rot.as_euler("xyz", degrees=True)
+ return rot_euler[2]
+
+
+def heading_to_quaternion(heading: float) -> (float, float, float, float):
+ """
+ Translates euler heading to quaternion
+ :param heading: euler heading
+ :return:
+ """
+ rot = Rotation.from_euler("xyz", (0, 0, heading), degrees=True)
+ quat = rot.as_quat()
+ return quat[0], quat[1], quat[2], quat[3]
+
+
+def calc_path_yaw(path: Path, idx: int) -> float:
+ """
+ Calculates the path yaw
+ :param path: The path to calculate the yaw on
+ :param idx: The pose index
+ :return: see description
+ """
+ if idx >= len(path.poses) - 1:
+ raise RuntimeError("no target found")
+
+ point_current: PoseStamped
+ 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)
+ return normalize_angle(angle)
+
+
+def normalize_angle(angle: float) -> float:
+ """
+ Normalizes an angle to [-pi, pi]
+ :param angle: The angle to normalize
+ :return: Angle in radian
+ """
+ while angle > np.pi:
+ angle -= 2.0 * np.pi
+
+ while angle < -np.pi:
+ angle += 2.0 * np.pi
+
+ return angle
+
+
+def calc_egocar_yaw(pose: PoseStamped) -> float:
+ """
+ Calculates the yaw of the ego vehicle
+ :param pose: The current pose of the ego vehicle
+ :return: normalized yaw of the vehicle
+ """
+ quaternion = (pose.pose.orientation.x, pose.pose.orientation.y,
+ pose.pose.orientation.z, pose.pose.orientation.w)
+ _, _, yaw = euler_from_quaternion(quaternion)
+ return normalize_angle(yaw)