Skip to content

Commit

Permalink
Merge pull request #164 from una-auxme/155-feature-tune-pure-pursuit-…
Browse files Browse the repository at this point in the history 
…steering-algorithm

155 feature tune pure pursuit steering algorithm
  • Loading branch information
@hellschwalex
hellschwalex authored Jan 25, 2024
2 parents 6fd0475 + 8b0fe00 commit 19686b0
Show file tree
Hide file tree
Showing 8 changed files with 102 additions and 125 deletions.
18 changes: 9 additions & 9 deletions code/acting/launch/acting.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,11 @@
<arg name="role_name" default="hero" />
<arg name="control_loop_rate" default="0.1" />

<!--node pkg="acting" type="Acting_DebuggerNode.py" name="Acting_Debugger" output="screen">
<param name="control_loop_rate" value="0.05" />
<param name="role_name" value="$(arg role_name)" />
</node-->

<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)" />
Expand All @@ -29,11 +34,6 @@
<param name="role_name" value="$(arg role_name)" />
</node>

<!-- <node pkg="acting" type="Acting_DebuggerNode.py" name="Acting_Debugger" output="screen">
<param name="control_loop_rate" value="0.05" />
<param name="role_name" value="$(arg role_name)" />
</node> -->

<node pkg="acting" type="MainFramePublisher.py" name="MainFramePublisher" output="screen">
<param name="control_loop_rate" value="0.05" />
<param name="role_name" value="$(arg role_name)" />
Expand All @@ -44,15 +44,15 @@
<param name="enabled" value="False" /> <!-- set to True to publish dummy velocities for testing-->
</node>

<node pkg="acting" type="acc.py" name="Acc" output="screen">
<!--node pkg="acting" type="acc.py" name="Acc" output="screen">
<param name="control_loop_rate" value="$(arg control_loop_rate)" />
<param name="role_name" value="$(arg role_name)" />
</node>
</node-->

<!-- Some plot nodes for debugging speed/steering etc. -->
<!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_speed" args="/carla/hero/Speed /paf/hero/max_velocity /paf/hero/throttle /paf/hero/brake"/-->
<!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_speed" args="/carla/hero/Speed /paf/hero/target_velocity /paf/hero/throttle /paf/hero/brake"/-->
<!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_steering" args="/paf/hero/pure_pursuit_steer /carla/hero/vehicle_control_cmd/steer /paf/hero/pure_p_debug/l_distance"/-->
<!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_trajectoryfollowing" args="/paf/hero/current_x /paf/hero/current_target_wp"/-->
<!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_heading_yaw" args="/paf/hero/yaw /paf/hero/current_heading" /-->
<!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_CONTROLLER" args="/paf/hero/controller"/-->

</launch>
112 changes: 66 additions & 46 deletions code/acting/src/acting/Acting_DebuggerNode.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,14 +53,16 @@

# 4: Test Steering-PID in vehicleController
# TODO TODO
TEST_TYPE = 2 # aka. TT
TEST_TYPE = 2 # aka. TT

STEERING: float = 0.0 # for TT0: steering -> always straight
TARGET_VELOCITY_1: float = 7 # for TT0/TT1: low velocity
FIXED_STEERING: float = 0 # for TT0: steering 0.0 = always straight
TARGET_VELOCITY_1: float = 5 # for TT0/TT1: low velocity
TARGET_VELOCITY_2: float = 0 # for TT1: high velocity

STEERING_CONTROLLER_USED = 1 # for TT2: 0 = both ; 1 = PP ; 2 = Stanley
TRAJECTORY_TYPE = 0 # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve
TRAJECTORY_TYPE = 2 # for TT2: 0 = Straight ; 1 = Curve ; 2 = SineWave

PRINT_AFTER_TIME = 20.0 # How long after Simulationstart to print data


class Acting_Debugger(CompatibleNode):
Expand Down Expand Up @@ -125,6 +127,14 @@ def __init__(self):
self.__get_target_velocity,
qos_profile=1)

# Subscriber for current_headng for plotting
self.heading_sub: Subscriber = self.new_subscription(
Float32,
f"/paf/{self.role_name}/current_heading",
self.__get_heading,
qos_profile=1
)

# Subscriber for current_velocity for plotting
self.current_velocity_sub: Subscriber = self.new_subscription(
CarlaSpeedometer,
Expand Down Expand Up @@ -181,10 +191,15 @@ def __init__(self):
self.__max_velocities = []
self.__throttles = []

self.__current_headings = []
self.__yaws = []

self.__purepursuit_steers = []
self.__stanley_steers = []
self.__vehicle_steers = []

self.positions = []

self.path_msg = Path()
self.path_msg.header.stamp = rospy.Time.now()
self.path_msg.header.frame_id = "global"
Expand All @@ -201,16 +216,33 @@ def __init__(self):
]
self.updated_trajectory(self.current_trajectory)

elif (TRAJECTORY_TYPE == 1): # Sinewave Serpentines trajectory
elif (TRAJECTORY_TYPE == 1): # straight into 90° Curve
self.current_trajectory = [
(984.5, -5442.0),

(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.0),
(1070.0, -5580.0)
]
self.updated_trajectory(self.current_trajectory)

elif (TRAJECTORY_TYPE == 2): # Sinewave Serpentines trajectory
# Generate a sine-wave with the global Constants to
# automatically generate a trajectory with serpentine waves
cycles = 4 # how many sine cycles
resolution = 50 # how many datapoints to generate
resolution = 70 # how many datapoints to generate

length = np.pi * 2 * cycles
step = length / resolution # spacing between values
my_wave = np.sin(np.arange(0, length, step))
x_wave = 2 * my_wave # to have a serpentine line with +/- 2 meters
x_wave = 0.15 * my_wave # to have a serpentine line with +/-1.5 m
# to have the serpentine line drive around the middle
# of the road/start point of the car
x_wave += startx
Expand All @@ -228,33 +260,6 @@ def __init__(self):
self.current_trajectory = trajectory_wave
self.updated_trajectory(self.current_trajectory)

elif (TRAJECTORY_TYPE == 2): # straight into 90° Curve
self.current_trajectory = [
(986.0, -5442.0),
(986.0, -5463.2),
(984.5, -5493.2),

(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.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.current_trajectory)

def updated_trajectory(self, target_trajectory):
"""
Updates the published Path message with the new target trajectory.
Expand All @@ -272,7 +277,7 @@ def updated_trajectory(self, target_trajectory):
pos.header.frame_id = "global"
pos.pose.position.x = wp[0]
pos.pose.position.y = wp[1]
pos.pose.position.z = 37.6 # why??
pos.pose.position.z = 35 # why??
# currently not used therefore zeros
pos.pose.orientation.x = 0
pos.pose.orientation.y = 0
Expand All @@ -285,7 +290,16 @@ def __current_position_callback(self, data: PoseStamped):
self.x = agent.x
self.y = agent.y
self.z = agent.z

# TODO use this to get spawnpoint? necessary?
# use to plot current_position to trajectory for steering test
self.positions.append((self.x, self.y))

def __get_heading(self, data: Float32):
self.__current_headings.append(float(data.data))

def __get_yaw(self, data: Float32):
self.__yaws.append(float(data.data))

def __get_target_velocity(self, data: Float32):
self.__max_velocities.append(float(data.data))
Expand Down Expand Up @@ -321,9 +335,9 @@ def loop(timer_event=None):
"""
# Drive const. velocity on fixed straight steering
if (TEST_TYPE == 0):
self.drive_Vel = TARGET_VELOCITY_1
self.stanley_steer_pub.publish(STEERING)
self.pure_pursuit_steer_pub.publish(STEERING)
self.driveVel = TARGET_VELOCITY_1
self.stanley_steer_pub.publish(FIXED_STEERING)
self.pure_pursuit_steer_pub.publish(FIXED_STEERING)
self.velocity_pub.publish(self.driveVel)

# Drive alternating velocities on fixed straight steering
Expand All @@ -339,8 +353,8 @@ def loop(timer_event=None):
self.driveVel = TARGET_VELOCITY_2
else:
self.driveVel = TARGET_VELOCITY_1
self.stanley_steer_pub.publish(STEERING)
self.pure_pursuit_steer_pub.publish(STEERING)
self.stanley_steer_pub.publish(FIXED_STEERING)
self.pure_pursuit_steer_pub.publish(FIXED_STEERING)
self.velocity_pub.publish(self.driveVel)

# drive const. velocity on trajectoy with steering controller
Expand All @@ -362,8 +376,8 @@ def loop(timer_event=None):
if (self.checkpoint_time < rospy.get_time() - 15.0):
self.checkpoint_time = rospy.get_time()
self.emergency_pub.publish(True)
self.stanley_steer_pub.publish(STEERING)
self.pure_pursuit_steer_pub.publish(STEERING)
self.stanley_steer_pub.publish(FIXED_STEERING)
self.pure_pursuit_steer_pub.publish(FIXED_STEERING)
self.velocity_pub.publish(self.driveVel)

# drive const. velocity and follow trajectory by
Expand All @@ -384,17 +398,23 @@ def loop(timer_event=None):
if not self.time_set:
self.checkpoint_time = rospy.get_time()
self.time_set = True
print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<")
# print(self.current_trajectory)
print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<")

# Uncomment the prints of the data you want to plot
if (self.checkpoint_time < rospy.get_time() - 10.0):
if (self.checkpoint_time < rospy.get_time() - PRINT_AFTER_TIME):
self.checkpoint_time = rospy.get_time()
print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<")
# print(self.__max_velocities)
# print(self.__current_velocities)
# print(self.__throttles)
print(self.__purepursuit_steers)
print(self.__stanley_steers)
print(self.__vehicle_steers)
# print(self.__purepursuit_steers)
# print(self.__stanley_steers)
# print(self.__vehicle_steers)
# print(self.__current_headings)
# print(self.__yaws)
# print(self.positions)
print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<")

self.new_timer(self.control_loop_rate, loop)
Expand Down
71 changes: 15 additions & 56 deletions code/acting/src/acting/pure_pursuit_controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,22 +4,23 @@

import ros_compatibility as roscomp
from carla_msgs.msg import CarlaSpeedometer
from geometry_msgs.msg import Point, PoseStamped, Pose
from geometry_msgs.msg import Point, PoseStamped
from nav_msgs.msg import Path
from ros_compatibility.node import CompatibleNode
from rospy import Publisher, Subscriber
from std_msgs.msg import Float32
from acting.msg import Debug
import rospy
# import numpy as np
import numpy as np

from helper_functions import vector_angle
from trajectory_interpolation import points_to_vector

MIN_LD_V: float = 3.0
LOOK_AHEAD_DIS = 3
MIN_L_A_DIS = 1
MAX_L_A_DIS = 15
# Tuneable Values
K_LAD = 0.85
MIN_LA_DISTANCE = 2
MAX_LA_DISTANCE = 25
# Constants
L_VEHICLE = 2.85 # wheelbase


class PurePursuitController(CompatibleNode):
Expand Down Expand Up @@ -60,26 +61,11 @@ def __init__(self):
f"/paf/{self.role_name}/pure_pursuit_steer",
qos_profile=1)

self.pure_pursuit_steer_target_pub: Publisher = self.new_publisher(
Pose,
f"/paf/{self.role_name}/pure_pursuit_steer_target_wp",
qos_profile=1)

self.debug_publisher: Publisher = self.new_publisher(
Debug,
f"/paf/{self.role_name}/pure_p_debug",
qos_profile=1)

self.targetwp_publisher: Publisher = self.new_publisher(
Float32,
f"/paf/{self.role_name}/current_target_wp",
qos_profile=1)

self.currentx_publisher: Publisher = self.new_publisher(
Float32,
f"/paf/{self.role_name}/current_x",
qos_profile=1)

self.__position: (float, float) = None # x, y
self.__last_pos: (float, float) = None
self.__path: Path = None
Expand Down Expand Up @@ -109,6 +95,7 @@ def loop(timer_event=None):
self.logdebug("PurePursuitController hasn't received a path "
"yet and can therefore not publish steering")
return

if self.__position is None:
self.logdebug("PurePursuitController hasn't received the "
"position of the vehicle yet "
Expand All @@ -126,6 +113,7 @@ def loop(timer_event=None):
"velocity of the vehicle yet "
"and can therefore not publish steering")
return

self.pure_pursuit_steer_pub.publish(self.__calculate_steer())

self.new_timer(self.control_loop_rate, loop)
Expand All @@ -136,22 +124,10 @@ def __calculate_steer(self) -> float:
Calculates the steering angle based on the current information
:return:
"""
l_vehicle = 2.85 # wheelbase
k_ld = 0.1 # TODO: tune
look_ahead_dist = LOOK_AHEAD_DIS # offset so that ld is never zero

if self.__velocity < 0:
# backwards driving is not supported, TODO why check this here?
return 0.0
elif round(self.__velocity, 1) < MIN_LD_V:
# Offset for low velocity state
look_ahead_dist += 0.0 # no offset
else:
look_ahead_dist += k_ld * (self.__velocity - MIN_LD_V)

# look_ahead_dist = np.clip(k_ld * self.__velocity,
# MIN_L_A_DIS, MAX_L_A_DIS)

# Simplified alot to increase understanding
# la_dist = MIN_LA_DISTANCE <= K_LAD * velocity <= MAX_LA_DISTANCE
look_ahead_dist = np.clip(K_LAD * self.__velocity,
MIN_LA_DISTANCE, MAX_LA_DISTANCE)
# Get the target position on the trajectory in look_ahead distance
self.__tp_idx = self.__get_target_point_index(look_ahead_dist)
target_wp: PoseStamped = self.__path.poses[self.__tp_idx]
Expand All @@ -164,11 +140,9 @@ def __calculate_steer(self) -> float:
target_vector_heading = vector_angle(target_v_x, target_v_y)
# Get the error between current heading and target heading
alpha = target_vector_heading - self.__heading
# Steering_angle is arctan (l_vehicle / R)
# R is look_ahead_dist / 2 * sin(alpha)
# https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html

steering_angle = atan((2 * l_vehicle * sin(alpha)) / look_ahead_dist)
steering_angle = atan((2 * L_VEHICLE * sin(alpha)) / look_ahead_dist)

# for debugging ->
debug_msg = Debug()
Expand All @@ -178,21 +152,6 @@ def __calculate_steer(self) -> float:
debug_msg.steering_angle = steering_angle
self.debug_publisher.publish(debug_msg)
# <-
self.pure_pursuit_steer_target_pub.publish(target_wp.pose)

# not beautiful but works, eliminates the first
# second because for some reason
# the positional data from the GNSS is completely broken at the start
if not self.time_set:
self.checkpoint_time = rospy.get_time()
self.time_set = True
if not self.checker and (self.checkpoint_time < rospy.get_time() - 1):
self.checker = True

if self.checker:
self.targetwp_publisher.publish((target_wp.pose.position.x-984.5))
self.currentx_publisher.publish(self.__position[0]-984.5)

return steering_angle

def __set_position(self, data: PoseStamped, min_diff=0.001):
Expand Down
Loading

0 comments on commit 19686b0

Please sign in to comment.