diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index fd0aabb3..ba12415c 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -31,12 +31,12 @@ --> - - + diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index dded87b7..e53801b9 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/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 diff --git a/code/acting/src/acting/OBSOLETE/stanley_controller.py b/code/acting/src/acting/stanley_controller.py similarity index 97% rename from code/acting/src/acting/OBSOLETE/stanley_controller.py rename to code/acting/src/acting/stanley_controller.py index d73c9874..34567787 100755 --- a/code/acting/src/acting/OBSOLETE/stanley_controller.py +++ b/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: diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 5da73ad8..36900c00 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/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,6 +123,11 @@ 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 @@ -123,7 +135,7 @@ def loop(timer_event=None) -> None: 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):