fix(#189): fixed target heading, added a few more b5 commands (#191)

# Description

There was an issue with the way the target heading was calculated.
As I didn't find the issue right away, I improved a lot of other stuff
before fixing the actual problem.

Fixes #189 

## Time invested

- 7h @schwalga 

## Type of change

- Bug fix (non-breaking change which fixes an issue)

## Does this PR introduce a breaking change?

No, just improvements to already existing features 

## Most important changes

I basically redid the pure pursuit controller and fixed a critical error
in a helper function

# Checklist:

- [x] My code follows the style guidelines of this project
- [x] I have performed a self-review of my own code
- [x] I have commented my code, particularly in hard-to-understand areas
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works (might be obsolete with CI later on)
- [x] New and existing unit tests pass locally with my changes (might be
obsolete with CI later on)

build/tasks/ros.sh

@@ -104,3 +104,11 @@ task:rosversion() {
 task:rqt_graph() {
  task:roscommand "rqt_graph ${@}"
 }
+
+task:rqt_plot() {
+ task:roscommand "rqt_plot ${@}"
+}
+
+task:rqt_topic() {
+ task:roscommand "rqt_topic ${@}"
+}

code/acting/launch/acting.launch

@@ -21,7 +21,7 @@
  <node pkg="acting" type="velocity_publisher_dummy.py" name="velocity_publisher_dummy" output="screen">
  <param name="role_name" value="$(arg role_name)" />
  <param name="control_loop_rate" value="0.2" />
- <param name="enabled" value="False" /> <!-- set to True to publish dummy velocities for testing-->
+ <param name="enabled" value="True" /> <!-- set to True to publish dummy velocities for testing-->
  </node>
 
  <node pkg="acting" type="vehicle_controller.py" name="vehicle_controller" output="screen">

code/acting/src/acting/DummyTrajectoryPublisher.py

@@ -36,24 +36,25 @@ def __init__(self):
 
  # Static trajectory for testing purposes
  self.initial_trajectory = [
- (985.0, -5374.2),
- (985.0, -5394.2),
-
- (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),
-
- (1040.0, -5570.2),
- (1050.0, -5570.2),
- (1060.0, -5567.5),
- (1090.0, -5567.5),
- (1130.0, -5570.2),
- (1164.6, -5570.2),
- (1264.6, -5570.0)]
+ (985.5, -5433.2),
+ (983.5, -5443.2),
+
+ (983.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.2),
+ (1050.0, -5580.2),
+ (1060.0, -5580.5),
+ (1090.0, -5580.5),
+ (1164.6, -5583.0),
+ (1264.6, -5583.0)
+ ]
+
  self.updated_trajectory(self.initial_trajectory)
  # request for a new interpolated dummy trajectory
  # self.dummy_trajectory_request_subscriber = self.new_subscription(

code/acting/src/acting/helper_functions.py

@@ -11,22 +11,42 @@
 from scipy.spatial.transform import Rotation
 
 
-def vectors_to_angle(x1: float, y1: float, x2: float, y2: float) -> float:
+def vectors_to_angle_abs(x1: float, y1: float, x2: float, y2: float) -> float:
  """
- Returns the angle (degrees) between the two given vectors
+ Returns the angle (radians) between the two given vectors
  :param x1: v1[x]
  :param y1: v1[y]
  :param x2: v2[x]
  :param y2: v2[y]
  :return: angle between v1 and v2
  """
- v1 = [x1, y1]
- v2 = [x2, y2]
+ v1 = np.array([x1, y1])
+ v2 = np.array([x2, y2])
 
  cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
- alpha = np.arccos(cos_angle)
+ alpha = math.acos(cos_angle)
+ return alpha
 
- return math.degrees(alpha)
+
+def vector_angle(x1: float, y1: float) -> float:
+ """
+ Returns the angle (radians) between the two 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
+ l_v = math.sqrt(x1**2 + y1**2)
+ x_0 = x1 + l_v
+ y_0 = 0
+ # return the angle between the 2 vectors
+ alpha = vectors_to_angle_abs(x_0, y_0, x1, y1)
+ # check if the angle should be negative
+ if y1 < 0:
+ sign = -1
+ else:
+ sign = 0
+ return alpha * sign
 
 
 def vector_to_direction(x1, y1, x2, y2) -> float:

code/acting/src/acting/pure_pursuit_controller.py

@@ -11,7 +11,7 @@
 from std_msgs.msg import Float32
 from acting.msg import Debug
 
-from helper_functions import vectors_to_angle
+from helper_functions import vector_angle
 from trajectory_interpolation import points_to_vector
 
 
@@ -41,6 +41,13 @@ def __init__(self):
  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
+ )
+
  self.pure_pursuit_steer_pub: Publisher = self.new_publisher(
  Float32,
  f"/carla/{self.role_name}/pure_pursuit_steer",
@@ -103,7 +110,7 @@ def loop(timer_event=None):
  self.new_timer(self.control_loop_rate, loop)
  self.spin()
 
- def __set_position(self, data: PoseStamped, min_diff=0.05):
+ 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
@@ -123,9 +130,9 @@ def __set_position(self, data: PoseStamped, min_diff=0.05):
  dist = self.__dist_to(data.pose.position)
  if dist < min_diff:
  # for debugging purposes:
- self.logwarn("New position disregarded, "
- f"as dist ({round(dist, 3)}) to current pos "
- f"< min_diff ({round(min_diff, 3)})")
+ 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]
@@ -134,42 +141,16 @@ def __set_position(self, data: PoseStamped, min_diff=0.05):
  new_x = data.pose.position.x
  new_y = data.pose.position.y
  self.__position = (new_x, new_y)
- self.__set_heading()
 
  def __set_path(self, data: Path):
  self.__path = data
 
- def __set_heading(self):
+ def __set_heading(self, data: Float32):
  """
  Updates the current heading
  :return:
  """
- if self.__position is None:
- self.logwarn("__get_heading: Current Position not set")
- self.__heading = 0
- return
- if self.__last_pos is None:
- self.logwarn("__get_heading: Last Position not set")
- self.__heading = 0
- return
-
- cur_x, cur_y = points_to_vector(
- (self.__last_pos[0],
- self.__last_pos[1]),
- (self.__position[0],
- self.__position[1])
- )
- # maybe remove weight if it doesn't help (after fixing gps signal)
- # code without weight:
- # self.__heading = vectors_to_angle(cur_x, cur_y, 1, 0)
- # ->
- if self.__heading is not None:
- old_heading: float = self.__heading
- new_heading: float = vectors_to_angle(cur_x, cur_y, 1, 0)
- self.__heading = (2 * new_heading + 1 * old_heading) / 3
- else:
- self.__heading = vectors_to_angle(cur_x, cur_y, 1, 0)
- # <-
+ self.__heading = data.data
 
  def __set_velocity(self, data: CarlaSpeedometer):
  self.__velocity = data.speed
@@ -180,71 +161,39 @@ def __calculate_steer(self) -> float:
  :return:
  """
  l_vehicle = 2.85 # wheelbase
- k_ld = 5.0 # todo: tune
+ k_ld = 2.50 # todo: tune
+ look_ahead_dist = 5.0 # offset so that ld is never zero
 
- current_velocity: float
- if self.__velocity == 0:
- current_velocity = k_ld * 0.1
+ if round(self.__velocity, 1) < 0.1:
+ look_ahead_dist += 1.0
  else:
- current_velocity = self.__velocity
+ look_ahead_dist += k_ld * self.__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]
 
- target_v_x, target_v_y = points_to_vector((self.__last_pos[0],
- self.__last_pos[1]),
+ target_v_x, target_v_y = points_to_vector((self.__position[0],
+ self.__position[1]),
  (target_wp.pose.position.x,
  target_wp.pose.position.y))
 
- 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)
+ target_vector_heading = vector_angle(target_v_x, target_v_y)
 
- alpha = self.__heading - target_vector_heading
- steering_angle = atan((2 * l_vehicle * sin(math.radians(alpha))) /
- look_ahead_dist)
+ alpha = target_vector_heading - self.__heading
+ steering_angle = atan((2 * l_vehicle * sin(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
+ debug_msg.alpha = steering_angle
  self.debug_publisher.publish(debug_msg)
  # <-
 
  self.pure_pursuit_steer_target_pub.publish(target_wp.pose)
 
- # for debugging only ->
- # rqt_plot /carla/hero/current_pos/pose/position/x
- # /carla/hero/pure_pursuit_steer_target_wp/position/x
- # rqt_plot /carla/hero/current_pos/pose/position/y
- # /carla/hero/pure_pursuit_steer_target_wp/position/y
-
- # rqt_plot /carla/hero/pure_pursuit_steer_target_wp/orientation/x:y
-
- # t_x = target_wp.pose.position.x
- # 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)} \t"
- # f"Tar V Heading: {round(target_vector_heading, 3)} \t"
- # )
- # <-
-
  return steering_angle
 
  def __get_target_point_index(self, ld: float) -> int:

code/acting/src/acting/vehicle_controller.py

@@ -134,6 +134,7 @@ def loop(timer_event=None) -> None:
  message.manual_gear_shift = False
  pid.setpoint = self.__map_steering(steer)
  message.steer = pid(self.__current_steer)
+ # message.steer = 0 # zum testen mit bei gerader Fahrt
  message.gear = 1
  message.header.stamp = roscomp.ros_timestamp(self.get_time(),
  from_sec=True)

code/acting/src/acting/velocity_publisher_dummy.py

@@ -23,10 +23,10 @@ def __init__(self):
  Float32,
  f"/carla/{self.role_name}/max_velocity",
  qos_profile=1)
- self.velocity = 7
- self.delta_velocity = 0.25
- self.max_velocity = 10
- self.min_velocity = 5
+ self.velocity = 4
+ self.delta_velocity = 0.125
+ self.max_velocity = 5
+ self.min_velocity = 4
  self.__dv = self.delta_velocity
 
  def run(self):

code/agent/config/dev_objects.json

@@ -10,9 +10,9 @@
  "id": "hero",
  "role_name": "hero",
  "spawn_point": {
- "x": 982.5,
- "y": -5473.2,
- "z": 371.5,
+ "x": 985.5,
+ "y": -5433.2,
+ "z": 371.0,
  "yaw": -90.0,
  "pitch": 0.0,
  "roll": 0.0
@@ -85,7 +85,7 @@
  "type": "sensor.other.gnss",
  "id": "GPS",
  "spawn_point": {
- "x": 0.7,
+ "x": 0.0,
  "y": 0.0,
  "z": 1.60
  },

code/perception/launch/perception.launch

@@ -21,7 +21,7 @@
  <param name="freq" value="20.0"/>
  <param name="sensor_timeout" value="10.0"/>
  <param name="odom_used" value="false"/>
- <param name="imu_used" value="true"/>
+ <param name="imu_used" value="false"/>
  <param name="vo_used" value="true"/>
  <param name="debug" value="true"/>
  <param name="self_diagnose" value="true"/>