feat: hotfixing steering for leaderboard

code/acting/launch/acting.launch

@@ -31,12 +31,12 @@
     </node>
     -->
 
-    <!-- ______OBSOLETES______
     <node pkg="acting" type="stanley_controller.py" name="stanley_controller" output="screen">
         <param name="control_loop_rate" value="$(arg control_loop_rate)" />
         <param name="role_name" value="$(arg role_name)" />
     </node>
 
+    <!-- ______OBSOLETES______
     <node pkg="acting" type="acting_velocity_publisher.py" name="acting_velocity_publisher" output="screen">
         <param name="role_name" value="$(arg role_name)" />
         <param name="control_loop_rate" value="0.2" />
@@ -56,6 +56,6 @@
 
     <!-- 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/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_steering" args="/paf/hero/pure_pursuit_steer /carla/hero/vehicle_control_cmd/steer /paf/hero/pure_p_debug/l_distance"/-->
 
 </launch>

code/acting/src/acting/pure_pursuit_controller.py

@@ -20,9 +20,10 @@
 MIN_LA_DISTANCE = 2
 MAX_LA_DISTANCE = 25
 # Tuneable Factor before Publishing
-# "-" because it is inverted to the steering carla expects
-# "4.75" proved to be important for a good steering (see tuning-documentation)
-K_PUB = (-4.75)
+# "-1" because it is inverted to the steering carla expects
+# "4.75" proved to be important for a good steering
+# ONLY IN DEV.LAUNCH? (see documentation)
+K_PUB = -0.85  # (-4.75)
 # Constant: wheelbase of car
 L_VEHICLE = 2.85
 

code/acting/src/acting/OBSOLETE/stanley_controller.py → code/acting/src/acting/stanley_controller.py

@@ -15,7 +15,7 @@
 from helper_functions import vector_angle
 from trajectory_interpolation import points_to_vector
 
-K_CROSSERR = 1.24
+K_CROSSERR = 0.4  # 1.24
 
 
 class StanleyController(CompatibleNode):
@@ -250,11 +250,8 @@ def __get_cross_err(self, pos: Point) -> float:
         :return:
         """
         dist = self.__dist_to(pos)
-
-        # +1.4 in Headingdirection = get front axle instead of the cars middle
-        # heading_deg = self.__heading * 180 / math.pi
-        x = self.__position[0]  # + 1.4 * cos(heading_deg)
-        y = self.__position[1]  # + 1.4 * sin(heading_deg)
+        x = self.__position[0]
+        y = self.__position[1]
 
         alpha = 0
         if self.__heading is not None:

code/acting/src/acting/vehicle_controller.py

@@ -90,11 +90,18 @@ def __init__(self):
             self.__set_pure_pursuit_steer,
             qos_profile=1)
 
+        self.stanley_sub: Subscriber = self.new_subscription(
+            Float32,
+            f"/paf/{self.role_name}/stanley_steer",
+            self.__set_stanley_steer,
+            qos_profile=1)
+
         self.__emergency: bool = False
         self.__velocity: float = 0.0
         self.__brake: float = 0.0
         self.__throttle: float = 0.0
-        self.__steer: float = 0.0
+        self._p_steer: float = 0.0
+        self._s_steer: float = 0.0
 
     def run(self):
         """
@@ -116,14 +123,19 @@ def loop(timer_event=None) -> None:
                 # emergency is already handled in  __emergency_brake()
                 self.__emergency_brake(True)
                 return
+
+            if self.__velocity > 5:
+                steer = self._s_steer
+            else:
+                steer = self._p_steer
             message = CarlaEgoVehicleControl()
             message.reverse = False
             message.hand_brake = False
             message.manual_gear_shift = False
             message.gear = 1
             message.throttle = self.__throttle
             message.brake = self.__brake
-            message.steer = self.__steer
+            message.steer = steer
             message.header.stamp = roscomp.ros_timestamp(self.get_time(),
                                                          from_sec=True)
             self.control_publisher.publish(message)
@@ -203,7 +215,11 @@ def __set_brake(self, data):
 
     def __set_pure_pursuit_steer(self, data: Float32):
         r = (math.pi / 2)  # convert from RAD to [-1;1]
-        self.__steer = (data.data / r)
+        self._p_steer = (data.data / r)
+
+    def __set_stanley_steer(self, data: Float32):
+        r = (math.pi / 2)  # convert from RAD to [-1;1]
+        self._s_steer = (data.data / r)
 
 
 def main(args=None):