diff --git a/code/acting/CMakeLists.txt b/code/acting/CMakeLists.txt
index ed2d1742..4529faeb 100644
--- a/code/acting/CMakeLists.txt
+++ b/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
diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch
index 41cb1957..b78b30e3 100644
--- a/code/acting/launch/acting.launch
+++ b/code/acting/launch/acting.launch
@@ -8,10 +8,10 @@
-
-
-
-
+
+
+
+
diff --git a/code/acting/msg/StanleyDebug.msg b/code/acting/msg/StanleyDebug.msg
new file mode 100644
index 00000000..f3074e4e
--- /dev/null
+++ b/code/acting/msg/StanleyDebug.msg
@@ -0,0 +1,5 @@
+float32 heading
+float32 path_heading
+float32 cross_err
+float32 heading_err
+float32 steering_angle
diff --git a/code/acting/src/acting/DummyTrajectoryPublisher.py b/code/acting/src/acting/DummyTrajectoryPublisher.py
index 841aadf1..e9c3ac86 100755
--- a/code/acting/src/acting/DummyTrajectoryPublisher.py
+++ b/code/acting/src/acting/DummyTrajectoryPublisher.py
@@ -36,10 +36,11 @@ 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),
@@ -47,12 +48,16 @@ def __init__(self):
(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)
diff --git a/code/acting/src/acting/helper_functions.py b/code/acting/src/acting/helper_functions.py
index c64319e7..517258a4 100755
--- a/code/acting/src/acting/helper_functions.py
+++ b/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]
diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py
index cae1ba02..15751dc7 100755
--- a/code/acting/src/acting/pure_pursuit_controller.py
+++ b/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:
diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py
new file mode 100755
index 00000000..2be1d74b
--- /dev/null
+++ b/code/acting/src/acting/stanley_controller.py
@@ -0,0 +1,326 @@
+#!/usr/bin/env python
+# from typing import List
+import math
+from math import atan, sqrt, sin, cos
+import numpy as np
+import ros_compatibility as roscomp
+from carla_msgs.msg import CarlaSpeedometer
+from geometry_msgs.msg import PoseStamped, Point # , Pose, Quaternion
+from nav_msgs.msg import Path
+from ros_compatibility.node import CompatibleNode
+# from ros_compatibility.qos import QoSProfile, DurabilityPolicy
+from rospy import Publisher, Subscriber
+# from sensor_msgs.msg import NavSatFix, Imu
+from std_msgs.msg import Float32 # Bool
+from acting.msg import StanleyDebug
+
+from helper_functions import vector_angle
+from trajectory_interpolation import points_to_vector
+
+
+class StanleyController(CompatibleNode):
+ def __init__(self):
+ super(StanleyController, self).__init__('stanley_controller')
+ self.loginfo('StanleyController node started')
+
+ self.control_loop_rate = self.get_param('control_loop_rate', 0.05)
+ self.role_name = self.get_param('role_name', 'ego_vehicle')
+
+ # Subscriber
+ self.position_sub: Subscriber = self.new_subscription(
+ Path,
+ f"/carla/{self.role_name}/trajectory",
+ self.__set_path,
+ qos_profile=1)
+
+ self.path_sub: Subscriber = self.new_subscription(
+ PoseStamped,
+ f"/carla/{self.role_name}/current_pos",
+ self.__set_position,
+ qos_profile=1)
+
+ self.velocity_sub: Subscriber = self.new_subscription(
+ CarlaSpeedometer,
+ f"/carla/{self.role_name}/Speed",
+ self.__set_velocity,
+ qos_profile=1)
+
+ self.heading_sub: Subscriber = self.new_subscription(
+ Float32,
+ f"/carla/{self.role_name}/current_heading",
+ self.__set_heading,
+ qos_profile=1)
+
+ # Publisher
+ self.stanley_steer_pub: Publisher = self.new_publisher(
+ Float32,
+ f"/carla/{self.role_name}/stanley_steer",
+ qos_profile=1)
+
+ self.debug_publisher: Publisher = self.new_publisher(
+ StanleyDebug,
+ f"/paf/{self.role_name}/stanley_debug",
+ qos_profile=1)
+
+ self.__position: (float, float) = None # x, y
+ self.__last_pos: (float, float) = None
+ self.__path: Path = None
+ self.__heading: float = None
+ self.__velocity: float = None
+ self.__tp_idx: int = 0 # target waypoint index
+ # error when there are no targets
+
+ def run(self):
+ """
+ Starts the main loop of the node
+ :return:
+ """
+ self.loginfo('StanleyController node running')
+
+ def loop(timer_event=None):
+ """
+ Main loop of the acting node
+ :param timer_event: Timer event from ROS
+ :return:
+ """
+ if self.__path is None:
+ self.logwarn("StanleyController hasn't received a path yet"
+ "and can therefore not publish steering")
+ return
+ if self.__position is None:
+ self.logwarn("StanleyController hasn't received the"
+ "position of the vehicle yet "
+ "and can therefore not publish steering")
+ return
+
+ if self.__heading is None:
+ self.logwarn("StanleyController hasn't received the"
+ "heading of the vehicle yet and"
+ "can therefore not publish steering")
+ return
+
+ if self.__velocity is None:
+ self.logwarn("StanleyController hasn't received the "
+ "velocity of the vehicle yet "
+ "and can therefore not publish steering")
+ return
+ self.stanley_steer_pub.publish(self.__calculate_steer())
+
+ self.new_timer(self.control_loop_rate, loop)
+ self.spin()
+
+ def __set_position(self, data: PoseStamped, min_diff=0.001):
+ """
+ Updates the current position of the vehicle
+ To avoid problems when the car is stationary, new positions will only
+ be accepted, if they are a certain distance from the current one
+ :param data: new position as PoseStamped
+ :param min_diff: minium difference between new and current point for
+ the new point to be accepted
+ :return:
+ """
+ if self.__position is None:
+ x0 = data.pose.position.x
+ y0 = data.pose.position.y
+ self.__position = (x0, y0)
+ return
+
+ # check if the new position is valid
+ dist = self.__dist_to(data.pose.position)
+ if dist < min_diff:
+ # for debugging purposes:
+ self.logdebug("New position disregarded, "
+ f"as dist ({round(dist, 3)}) to current pos "
+ f"< min_diff ({round(min_diff, 3)})")
+ return
+
+ old_x = self.__position[0]
+ old_y = self.__position[1]
+ self.__last_pos = (old_x, old_y)
+ new_x = data.pose.position.x
+ new_y = data.pose.position.y
+ self.__position = (new_x, new_y)
+
+ def __set_path(self, data: Path):
+ self.__path = data
+
+ def __set_heading(self, data: Float32):
+ self.__heading = data.data
+
+ def __set_velocity(self, data: CarlaSpeedometer):
+ self.__velocity = data.speed
+
+ def __calculate_steer(self) -> float:
+ """
+ Calculates the steering angle based on the current information
+ using the Stanly algorithm
+ :return: steering angle
+ """
+ k_ce = 0.10 # todo: tune
+ k_v = 1.0
+
+ current_velocity: float
+ if self.__velocity <= 1:
+ current_velocity = 1
+ else:
+ current_velocity = self.__velocity
+
+ closest_point_idx = self.__get_closest_point_index()
+ closest_point: PoseStamped = self.__path.poses[closest_point_idx]
+ traj_heading = self.__get_path_heading(closest_point_idx)
+
+ cross_err = self.__get_cross_err(closest_point.pose.position)
+ heading_err = self.__heading - traj_heading
+
+ steering_angle = heading_err + atan((k_ce * cross_err) /
+ current_velocity * k_v)
+ steering_angle *= - 1
+
+ # for debugging ->
+ debug_msg = StanleyDebug()
+ debug_msg.heading = self.__heading
+ debug_msg.path_heading = traj_heading
+ debug_msg.cross_err = abs(cross_err)
+ debug_msg.heading_err = heading_err
+ debug_msg.steering_angle = steering_angle
+ self.debug_publisher.publish(debug_msg)
+ # <-
+
+ return steering_angle
+
+ def __get_closest_point_index(self) -> int:
+ """
+ Returns index of the nearest point of the trajectory
+ :return: Index of the closest point
+ """
+ if len(self.__path.poses) < 2:
+ return -1
+
+ min_dist = 10e100
+ min_dist_idx = -1
+
+ for i in range(0, len(self.__path.poses)):
+ temp_pose: PoseStamped = self.__path.poses[i]
+ dist = self.__dist_to(temp_pose.pose.position)
+ if min_dist > dist:
+ min_dist = dist
+ min_dist_idx = i
+ return min_dist_idx
+
+ def __get_path_heading(self, index: int) -> float:
+ """
+ Calculates the heading of the current path at index
+ :param index: point of interest
+ :return: heading at path[index]
+ """
+ cur_pos: Point = self.__path.poses[index].pose.position
+ l_path = len(self.__path.poses)
+
+ if l_path == 1:
+ return 0
+
+ heading_sum = 0
+ heading_sum_args = 0
+
+ if index > 0:
+ # Calculate heading from the previous point on the trajectory
+ prv_point: Point = self.__path.poses[index-1].pose.position
+
+ prv_v_x, prv_v_y = points_to_vector((prv_point.x, prv_point.y),
+ (cur_pos.x, cur_pos.y))
+
+ heading_sum += vector_angle(prv_v_x, prv_v_y)
+ heading_sum_args += 1
+
+ elif index < l_path - 1:
+ # Calculate heading to the following point on the trajectory
+ aft_point: Point = self.__path.poses[index + 1].pose.position
+
+ aft_v_x, aft_v_y = points_to_vector((aft_point.x, aft_point.y),
+ (cur_pos.x, cur_pos.y))
+
+ heading_sum += vector_angle(aft_v_x, aft_v_y)
+ heading_sum_args += 1
+
+ return heading_sum / heading_sum_args
+
+ def __get_cross_err(self, pos: Point) -> float:
+ """
+ Returns the Distance between current position and target position.
+ The distance is negative/positive based on whether the closest point
+ is to the left or right of the vehicle.
+ :param pos:
+ :return:
+ """
+ dist = self.__dist_to(pos)
+
+ x = self.__position[0]
+ y = self.__position[1]
+
+ # self.loginfo(f"Cur_X: {round(x, 2)} \t "
+ # f"Cur_Y: {round(y, 2)} \t "
+ # f"Trj_X: {round(pos.x, 2)} \t "
+ # f"Trj_Y: {round(pos.y, 2)} \t ")
+
+ alpha = 0
+ if self.__heading is not None:
+ alpha = self.__heading + (math.pi / 2)
+ v_e_0 = (0, 1)
+ v_e = (cos(alpha)*v_e_0[0] - sin(alpha)*v_e_0[1],
+ sin(alpha)*v_e_0[0] + cos(alpha)*v_e_0[1])
+
+ # define a vector (v_ab) with length 10 centered on the cur pos
+ # of the vehicle, with a heading parallel to that of the vehicle
+ a = (x + (v_e[0] * 2.5), y + (v_e[1] * 2.5))
+ b = (x - (v_e[0] * 2.5), y - (v_e[1] * 2.5))
+
+ v_ab = (b[0] - a[0], b[1] - a[1])
+ v_am = (pos.x - a[0], pos.y - a[1])
+
+ c = np.array([[v_ab[0], v_am[0]],
+ [v_ab[1], v_am[1]]])
+ temp_sign = np.linalg.det(c)
+
+ min_sign = 0.01 # to avoid rounding errors
+
+ if temp_sign > -min_sign:
+ sign = -1
+ else:
+ sign = 1
+
+ res = dist * sign
+
+ return res
+
+ def __dist_to(self, pos: Point) -> float:
+ """
+ Distance between current position and target position (only (x,y))
+ :param pos: targeted position
+ :return: distance
+ """
+ x_cur = self.__position[0]
+ y_cur = self.__position[1]
+ x_target = pos.x
+ y_target = pos.y
+ d = (x_target - x_cur) ** 2 + (y_target - y_cur) ** 2
+ return sqrt(d)
+
+
+def main(args=None):
+ """
+ Main function starts the node
+ :param args:
+ """
+ roscomp.init('stanley_controller', args=args)
+
+ try:
+ node = StanleyController()
+ node.run()
+ except KeyboardInterrupt:
+ pass
+ finally:
+ roscomp.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py
index 3bf917ae..c234f144 100755
--- a/code/acting/src/acting/vehicle_controller.py
+++ b/code/acting/src/acting/vehicle_controller.py
@@ -220,7 +220,7 @@ def __choose_controller(self) -> int:
Chooses with steering controller to use
:return:
"""
- return PURE_PURSUIT_CONTROLLER
+ return STANLEY_CONTROLLER
def main(args=None):
diff --git a/code/acting/src/acting/velocity_publisher_dummy.py b/code/acting/src/acting/velocity_publisher_dummy.py
index e165d02e..7b8f1fb0 100755
--- a/code/acting/src/acting/velocity_publisher_dummy.py
+++ b/code/acting/src/acting/velocity_publisher_dummy.py
@@ -23,10 +23,12 @@ def __init__(self):
Float32,
f"/carla/{self.role_name}/max_velocity",
qos_profile=1)
+
self.velocity = 4.0
self.delta_velocity = 0.125
self.max_velocity = 5.5
self.min_velocity = 4
+
self.__dv = self.delta_velocity
def run(self):
diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json
index 8b103317..c0817bd2 100644
--- a/code/agent/config/dev_objects.json
+++ b/code/agent/config/dev_objects.json
@@ -10,8 +10,8 @@
"id": "hero",
"role_name": "hero",
"spawn_point": {
- "x": 985.5,
- "y": -5433.2,
+ "x": 986.0,
+ "y": -5442.0,
"z": 371.0,
"yaw": -90.0,
"pitch": 0.0,
diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz
index d7633d3f..9595c6ef 100644
--- a/code/agent/config/rviz_config.rviz
+++ b/code/agent/config/rviz_config.rviz
@@ -5,7 +5,7 @@ Panels:
Property Tree Widget:
Expanded: ~
Splitter Ratio: 0.5
- Tree Height: 519
+ Tree Height: 417
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -200,19 +200,19 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.35539835691452026
+ Pitch: 0.19039836525917053
Target Frame:
- Yaw: 1.7953970432281494
+ Yaw: 4.520427227020264
Saved: ~
Window Geometry:
Camera:
collapsed: false
Displays:
collapsed: false
- Height: 1403
+ Height: 1376
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd000000040000000000000304000004e1fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003b9000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000003fa000001220000001600ffffff000000010000010f000004e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004e1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005e1000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000022a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026b000002960000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -221,6 +221,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
- Width: 2560
- X: 0
- Y: 0
+ Width: 2488
+ X: 1992
+ Y: 27
diff --git a/doc/00_assets/controller_img/pure_pursuit.png b/doc/00_assets/controller_img/pure_pursuit.png
new file mode 100644
index 00000000..81a03aa8
Binary files /dev/null and b/doc/00_assets/controller_img/pure_pursuit.png differ
diff --git a/doc/00_assets/controller_img/pure_pursuit_v_6_25.png b/doc/00_assets/controller_img/pure_pursuit_v_6_25.png
new file mode 100644
index 00000000..c415713b
Binary files /dev/null and b/doc/00_assets/controller_img/pure_pursuit_v_6_25.png differ
diff --git a/doc/00_assets/controller_img/stanley.png b/doc/00_assets/controller_img/stanley.png
new file mode 100644
index 00000000..3aacdb66
Binary files /dev/null and b/doc/00_assets/controller_img/stanley.png differ
diff --git a/doc/00_assets/controller_img/stanley_v_6_25.png b/doc/00_assets/controller_img/stanley_v_6_25.png
new file mode 100644
index 00000000..b54aa7d4
Binary files /dev/null and b/doc/00_assets/controller_img/stanley_v_6_25.png differ
diff --git a/doc/05_acting/03_lateral_controller.md b/doc/05_acting/03_lateral_controller.md
new file mode 100644
index 00000000..783594b4
--- /dev/null
+++ b/doc/05_acting/03_lateral_controller.md
@@ -0,0 +1,75 @@
+# Overview of the current status of the lateral controller
+
+**Summary:** This page provides an overview of the current status of lateral controllers
+
+---
+
+## Author
+
+Gabriel Schwald
+
+## Date
+
+20.02.2023
+
+
+* [Overview of the current status of the lateral controller](#overview-of-the-current-status-of-the-lateral-controller)
+ * [Author](#author)
+ * [Date](#date)
+ * [Lateral controller](#lateral-controller)
+ * [Baseline](#baseline)
+ * [Velocity](#velocity)
+ * [Parameters](#parameters)
+ * [Tuning](#tuning)
+
+
+## Lateral controller
+
+At the moment the control algorithm is chosen manually by editing Line 223 in vehicle_controller.py.
+Accordingly, only one controller can be used per run, this will later be changed,
+in order to use each controller in its most effective range.
+
+The testing scenario consists of a manually created L-shaped trajectory, with a short slalom section at the end.
+Testing with real trajectories will definitely help in improving the controllers,
+but is not yet available at this time.
+
+## Baseline
+
+With a somewhat constant velocity of around 4.5 m/s the following distance to the trajectory can be observed with the
+default parameters.
+
+
+Pure Pursuit Baseline
+
+
+Stanley Baseline
+
+### Velocity
+
+Increasing the velcoity by around 2 m/s to 6.25 m/s already has an observable effect on the error.
+
+
+Pure Pursuit with increased velocity
+
+
+Stanley with increased velocity
+
+### Parameters
+
+The following parameters may be considered for optimization:
+
+Stanley:
+
+* k_ce <--> cross-track error weight
+* k_v <--> velocity weight
+
+Pure-Pursuit:
+
+* k_ld <--> look-ahead distance weight
+* look_ahead_dist <--> minimum look-ahead distance (maybe remove altogether?)
+
+The PID-Controller for the steering angle may also be tuned independently of the tuning of both controllers.
+
+## Tuning
+
+This document will be updated, whenever sizeable progress is made.