diff --git a/code/acting/CMakeLists.txt b/code/acting/CMakeLists.txt
index 9330eeae..ed2d1742 100644
--- a/code/acting/CMakeLists.txt
+++ b/code/acting/CMakeLists.txt
@@ -46,12 +46,14 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
+ message_generation
)
## Generate messages in the 'msg' folder
-#add_message_files(
-# FILES
-#)
+add_message_files(
+ FILES
+ Debug.msg
+)
## Generate services in the 'srv' folder
#add_service_files(
@@ -67,10 +69,10 @@ find_package(catkin REQUIRED COMPONENTS
# )
## Generate added messages and services with any dependencies listed here
-#generate_messages(
-# DEPENDENCIES
-# std_msgs # Or other packages containing msgs
-#)
+generate_messages(
+ DEPENDENCIES
+ std_msgs # Or other packages containing msgs
+)
################################################
## Declare ROS dynamic reconfigure parameters ##
diff --git a/code/acting/msg/Debug.msg b/code/acting/msg/Debug.msg
new file mode 100644
index 00000000..e019a0b1
--- /dev/null
+++ b/code/acting/msg/Debug.msg
@@ -0,0 +1,4 @@
+float32 heading
+float32 target_heading
+float32 l_distance
+float32 alpha
diff --git a/code/acting/package.xml b/code/acting/package.xml
index 22cee582..fe4646fd 100644
--- a/code/acting/package.xml
+++ b/code/acting/package.xml
@@ -37,13 +37,13 @@
-
+ message_generation
-
+ message_runtime
diff --git a/code/acting/src/acting/DummyTrajectoryPublisher.py b/code/acting/src/acting/DummyTrajectoryPublisher.py
index b8d0f429..a7b9eba1 100755
--- a/code/acting/src/acting/DummyTrajectoryPublisher.py
+++ b/code/acting/src/acting/DummyTrajectoryPublisher.py
@@ -36,15 +36,18 @@ def __init__(self):
# Static trajectory for testing purposes
initial_trajectory = [
- (985.5, -5374.2),
- (988.0, -5394.2),
- (987.5, -5404.2),
- (988.0, -5474.2),
- (987.5, -5494.2),
- (989.0, -5514.2),
- (987.0, -5534.2),
- (986.5, -5575.2),
- (987.0, -5775.2)]
+ (985.0, -5374.2),
+ (985.0, -5394.2),
+ # Turn ->
+ (985.0, -5555.5),
+ (985.0, -5563.2),
+ (985.3, -5565.5),
+ (986.3, -5567.5),
+ (987.5, -5569.0),
+ (990.5, -5569.8),
+ (1000.0, -5570.2),
+ # <-
+ (1164.6, -5571.2)]
self.updated_trajectory(initial_trajectory)
# request for a new interpolated dummy trajectory
diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py
index 72fa1d80..cc724815 100755
--- a/code/acting/src/acting/pure_pursuit_controller.py
+++ b/code/acting/src/acting/pure_pursuit_controller.py
@@ -9,6 +9,7 @@
from ros_compatibility.node import CompatibleNode
from rospy import Publisher, Subscriber
from std_msgs.msg import Float32
+from acting.msg import Debug
from helper_functions import vectors_to_angle
from trajectory_interpolation import points_to_vector
@@ -50,6 +51,11 @@ def __init__(self):
f"/carla/{self.role_name}/pure_pursuit_steer_target_wp",
qos_profile=1)
+ self.debug_publisher: Publisher = self.new_publisher(
+ Debug,
+ f"/carla/{self.role_name}/debug",
+ qos_profile=1)
+
self.__position: (float, float) = None # x, y
self.__last_pos: (float, float) = None
self.__path: Path = None
@@ -174,15 +180,15 @@ def __calculate_steer(self) -> float:
:return:
"""
l_vehicle = 2 # dist between front and rear wheels todo: measure
- k = 10 # todo: tune
+ k_ld = 15 # todo: tune
current_velocity: float
if self.__velocity == 0:
- current_velocity = k * 0.1
+ current_velocity = k_ld * 0.1
else:
current_velocity = self.__velocity
- look_ahead_dist = k * current_velocity
+ look_ahead_dist = k_ld * current_velocity
self.__tp_idx = self.__get_target_point_index(look_ahead_dist)
target_wp: PoseStamped = self.__path.poses[self.__tp_idx]
@@ -196,9 +202,30 @@ def __calculate_steer(self) -> float:
(self.__position[0],
self.__position[1]))
- alpha = vectors_to_angle(target_v_x, target_v_y,
- cur_v_x, cur_v_y)
- steering_angle = atan((2 * l_vehicle * sin(alpha)) / look_ahead_dist)
+ # -> debugging only
+ zero_h_v_x, zero_h_v_y = points_to_vector((self.__last_pos[0],
+ self.__last_pos[1]),
+ (self.__last_pos[0] + 1,
+ self.__last_pos[1]))
+
+ target_vector_heading = vectors_to_angle(target_v_x, target_v_y,
+ zero_h_v_x, zero_h_v_y)
+ # <-
+ alpha = self.__heading - target_vector_heading
+ # alpha = vectors_to_angle(target_v_x, target_v_y,
+ # cur_v_x, cur_v_y)
+ steering_angle = atan((2 * l_vehicle * sin(math.radians(alpha))) /
+ look_ahead_dist)
+
+ # for debugging ->
+ debug_msg = Debug()
+ debug_msg.heading = self.__heading
+ debug_msg.target_heading = target_vector_heading
+ debug_msg.l_distance = look_ahead_dist
+ debug_msg.alpha = alpha
+ self.debug_publisher.publish(debug_msg)
+ # <-
+
self.pure_pursuit_steer_target_pub.publish(target_wp.pose)
# for debugging only ->
@@ -213,15 +240,16 @@ def __calculate_steer(self) -> float:
# t_y = target_wp.pose.position.y
# c_x = self.__position[0]
# c_y = self.__position[1]
- self.loginfo(
- # f"T_V: ({round(target_v_x, 3)},{round(target_v_y, 3)})\t"
- # f"T_WP: ({round(t_x,3)},{round(t_y,3)}) \t"
- # f"C_Pos: ({round(c_x,3)},{round(c_y,3)}) \t"
- f"Target Steering angle: {round(steering_angle, 4)} \t"
- f"Current alpha: {round(alpha, 3)} \t"
- # f"Target WP idx: {self.__tp_idx} \t"
- f"Current heading: {round(self.__heading, 3)}"
- )
+ # self.loginfo(
+ # f"T_V: ({round(target_v_x, 3)},{round(target_v_y, 3)})\t"
+ # f"T_WP: ({round(t_x,3)},{round(t_y,3)}) \t"
+ # f"C_Pos: ({round(c_x,3)},{round(c_y,3)}) \t"
+ # f"Target Steering angle: {round(steering_angle, 4)} \t"
+ # f"Current alpha: {round(alpha, 3)} \t"
+ # f"Target WP idx: {self.__tp_idx} \t"
+ # f"Current heading: {round(self.__heading, 3)} \t"
+ # f"Tar V Heading: {round(target_vector_heading, 3)} \t"
+ # )
# <-
return steering_angle
diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py
index c054c4a9..b9d74e0e 100755
--- a/code/acting/src/acting/vehicle_controller.py
+++ b/code/acting/src/acting/vehicle_controller.py
@@ -1,4 +1,6 @@
#!/usr/bin/env python
+import math
+
import ros_compatibility as roscomp
from ros_compatibility.node import CompatibleNode
from carla_msgs.msg import CarlaEgoVehicleControl, CarlaSpeedometer
@@ -9,7 +11,7 @@
PURE_PURSUIT_CONTROLLER: int = 1
STANLEY_CONTROLLER: int = 2
-MAX_STEER_ANGLE: float = 0.5
+MAX_STEER_ANGLE: float = 0.75
class VehicleController(CompatibleNode):
@@ -95,7 +97,7 @@ def run(self):
"""
self.status_pub.publish(True)
self.loginfo('VehicleController node running')
- pid = PID(0.25, 0, 0.1, setpoint=0) # random values -> todo: tune
+ pid = PID(0.85, 0.1, 0.1, setpoint=0) # random values -> todo: tune
pid.output_limits = (-MAX_STEER_ANGLE, MAX_STEER_ANGLE)
def loop(timer_event=None) -> None:
@@ -130,7 +132,7 @@ def loop(timer_event=None) -> None:
message.hand_brake = False
message.manual_gear_shift = False
- pid.setpoint = steer
+ pid.setpoint = self.__map_steering(steer)
message.steer = pid(self.__current_steer)
message.gear = 1
message.header.stamp = roscomp.ros_timestamp(self.get_time(),
@@ -140,6 +142,18 @@ def loop(timer_event=None) -> None:
self.new_timer(self.control_loop_rate, loop)
self.spin()
+ def __map_steering(self, steering_angle: float) -> float:
+ """
+ Takes the steering angle calculated by the controller and maps it to
+ the available steering angle
+ :param steering_angle: calculated by a controller in [-pi/2 , pi/2]
+ :return: float for steering in [-1, 1]
+ """
+ tune_k = -5 # factor for tuning todo: tune
+ k = 1 / (math.pi / 2)
+ steering_float = steering_angle * k * tune_k
+ return steering_float
+
def __emergency_break(self, data) -> None:
"""
Executes emergency stop
diff --git a/code/acting/src/acting/velocity_publisher_dummy.py b/code/acting/src/acting/velocity_publisher_dummy.py
index 2a7c735b..aca85396 100755
--- a/code/acting/src/acting/velocity_publisher_dummy.py
+++ b/code/acting/src/acting/velocity_publisher_dummy.py
@@ -25,8 +25,8 @@ def __init__(self):
qos_profile=1)
self.velocity = 4
self.delta_velocity = 0.1
- self.max_velocity = 6
- self.min_velocity = 6.1
+ self.max_velocity = 3
+ self.min_velocity = 3.1
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 e11af3f2..813080aa 100644
--- a/code/agent/config/dev_objects.json
+++ b/code/agent/config/dev_objects.json
@@ -11,7 +11,7 @@
"role_name": "hero",
"spawn_point": {
"x": 982.5,
- "y": -5373.2,
+ "y": -5473.2,
"z": 371.5,
"yaw": -90.0,
"pitch": 0.0,