diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch
index 3316cfae..3a705e5d 100644
--- a/code/acting/launch/acting.launch
+++ b/code/acting/launch/acting.launch
@@ -3,6 +3,11 @@
+
+
@@ -29,11 +34,6 @@
-
-
@@ -44,15 +44,15 @@
-
+
-
+
-
+
diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py
index d2556604..9de92f6c 100755
--- a/code/acting/src/acting/Acting_DebuggerNode.py
+++ b/code/acting/src/acting/Acting_DebuggerNode.py
@@ -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):
@@ -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,
@@ -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"
@@ -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
@@ -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.
@@ -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
@@ -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))
@@ -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
@@ -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
@@ -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
@@ -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)
diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py
index 65e3481a..694c0b1e 100755
--- a/code/acting/src/acting/pure_pursuit_controller.py
+++ b/code/acting/src/acting/pure_pursuit_controller.py
@@ -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):
@@ -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
@@ -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 "
@@ -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)
@@ -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]
@@ -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()
@@ -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):
diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py
index 4d892cc5..e03e7817 100755
--- a/code/acting/src/acting/stanley_controller.py
+++ b/code/acting/src/acting/stanley_controller.py
@@ -169,7 +169,8 @@ def __calculate_steer(self) -> float:
using the Stanley algorithm
:return: steering angle
"""
- k_ce = 0.10 # TODO: tune
+ # TODO: tune both next sprint
+ k_ce = 0.10
k_v = 1.0
current_velocity: float
diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py
index ea062909..e210ec76 100755
--- a/code/acting/src/acting/vehicle_controller.py
+++ b/code/acting/src/acting/vehicle_controller.py
@@ -50,7 +50,7 @@ def __init__(self):
)
# Publisher for which steering-controller is mainly used
- # 1 = PurePursuit and 2 = Stanley TODO: needed?
+ # 1 = PurePursuit and 2 = Stanley
self.controller_pub: Publisher = self.new_publisher(
Float32,
f"/paf/{self.role_name}/controller",
@@ -143,6 +143,7 @@ def run(self):
self.status_pub.publish(True)
self.loginfo('VehicleController node running')
# currently pid for steering is not used, needs fixing
+ # or maybe deletion since it is not that useful
pid = PID(0.5, 0.001, 0) # PID(0.5, 0.1, 0.1, setpoint=0)
# TODO: TUNE AND FIX?
pid.output_limits = (-MAX_STEER_ANGLE, MAX_STEER_ANGLE)
@@ -178,7 +179,7 @@ def loop(timer_event=None) -> None:
# only use pure_pursuit controller for now, since
# stanley seems broken with the new heading-bug
# TODO: swap back if stanley is fixed
- steer = self.__pure_pursuit_steer
+ # steer = self.__pure_pursuit_steer
self.target_steering_publisher.publish(steer) # debugging
@@ -188,11 +189,9 @@ def loop(timer_event=None) -> None:
message.brake = self.__brake
message.hand_brake = False
message.manual_gear_shift = False
- # sets target_steer to steer
- # pid.setpoint = self.__map_steering(steer)
message.steer = self.__map_steering(steer)
- # TEST pure steering: message.steer = self.__map_steering(steer)
# Original Code:
+ # pid.setpoint = self.__map_steering(steer)
# message.steer = pid(self.__current_steer)
message.gear = 1
message.header.stamp = roscomp.ros_timestamp(self.get_time(),
@@ -210,11 +209,9 @@ def __map_steering(self, steering_angle: float) -> float:
:param steering_angle: calculated by a controller in [-pi/2 , pi/2]
:return: float for steering in [-1, 1]
"""
- tune_k = -1 # factor for tuning TODO: tune but why?
- # negative because carla steer and our steering controllers are flipped
- r = 1 / (math.pi / 2)
- steering_float = steering_angle * r * tune_k
- self.pidpoint_publisher.publish(steering_float)
+ r = - 1 / (math.pi / 2) # -1 to invert for carla steering
+ steering_float = steering_angle * r # No Tuning needed * tune_k
+ self.pidpoint_publisher.publish(steering_float) # TODO needed?
return steering_float
def __emergency_break(self, data) -> None:
diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py
index 754cbdc2..f00962cc 100755
--- a/code/acting/src/acting/velocity_controller.py
+++ b/code/acting/src/acting/velocity_controller.py
@@ -95,12 +95,12 @@ def loop(timer_event=None):
"publish a throttle value")
return
- if self.__trajectory is None:
+ """if self.__trajectory is None:
self.logdebug("VelocityController hasn't received "
"trajectory yet and can therefore not"
"publish a throttle value (to prevent stupid)")
return
-
+ """
if self.__target_velocity < 0:
self.logerr("VelocityController doesn't support backward "
"driving yet.")
diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch
index 500635fb..ea653653 100644
--- a/code/perception/launch/perception.launch
+++ b/code/perception/launch/perception.launch
@@ -68,7 +68,6 @@
-
diff --git a/code/perception/src/Position_Publisher_Node.py b/code/perception/src/Position_Publisher_Node.py
index 25a56f30..719ee3ab 100755
--- a/code/perception/src/Position_Publisher_Node.py
+++ b/code/perception/src/Position_Publisher_Node.py
@@ -3,6 +3,7 @@
"""
This node publishes all relevant topics for the ekf node.
"""
+
import math
from tf.transformations import euler_from_quaternion
import numpy as np