fix(#67): found and fixed a few critical mistakes

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 ##

code/acting/msg/Debug.msg

@@ -0,0 +1,4 @@
+float32 heading
+float32 target_heading
+float32 l_distance
+float32 alpha

code/acting/package.xml

@@ -37,13 +37,13 @@
  <!-- <build_depend>roscpp</build_depend> -->
  <!-- <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
+ <build_depend>message_generation</build_depend>
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!-- <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!-- <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
- <!-- <exec_depend>message_runtime</exec_depend> -->
+ <exec_depend>message_runtime</exec_depend>
  <!-- Use test_depend for packages you need only for testing: -->
  <!-- <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->

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

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

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

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):

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,