diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1372dfdb..6c3506be 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -21,7 +21,7 @@ services: # based on https://github.com/ll7/paf21-1/blob/master/scenarios/docker-carla-sim-compose.yml carla-simulator: - command: /bin/bash CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini" + command: /bin/bash CarlaUE4.sh -quality-level=High -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini" image: ghcr.io/una-auxme/paf23:leaderboard-2.0 init: true deploy: @@ -62,7 +62,7 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" logging: diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index ba12415c..ec4e7663 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -24,12 +24,11 @@ - + --> diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 36900c00..5b50e951 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -6,7 +6,7 @@ from carla_msgs.msg import CarlaEgoVehicleControl, CarlaSpeedometer from rospy import Publisher, Subscriber from ros_compatibility.qos import QoSProfile, DurabilityPolicy -from std_msgs.msg import Bool, Float32 +from std_msgs.msg import Bool, Float32, String class VehicleController(CompatibleNode): @@ -26,6 +26,8 @@ def __init__(self): self.control_loop_rate = self.get_param('control_loop_rate', 0.05) self.role_name = self.get_param('role_name', 'ego_vehicle') + self.__curr_behavior = None # only unstuck behavior is relevant here + # Publisher for Carla Vehicle Control Commands self.control_publisher: Publisher = self.new_publisher( CarlaEgoVehicleControl, @@ -58,6 +60,13 @@ def __init__(self): durability=DurabilityPolicy.TRANSIENT_LOCAL) ) + # Subscribers + self.curr_behavior_sub: Subscriber = self.new_subscription( + String, + f"/paf/{self.role_name}/curr_behavior", + self.__set_curr_behavior, + qos_profile=1) + self.emergency_sub: Subscriber = self.new_subscription( Bool, f"/paf/{self.role_name}/emergency", @@ -84,6 +93,12 @@ def __init__(self): self.__set_brake, qos_profile=1) + self.reverse_sub: Subscriber = self.new_subscription( + Bool, + f"/paf/{self.role_name}/reverse", + self.__set_reverse, + qos_profile=1) + self.pure_pursuit_steer_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/pure_pursuit_steer", @@ -96,6 +111,7 @@ def __init__(self): self.__set_stanley_steer, qos_profile=1) + self.__reverse: bool = False self.__emergency: bool = False self.__velocity: float = 0.0 self.__brake: float = 0.0 @@ -127,9 +143,15 @@ def loop(timer_event=None) -> None: if self.__velocity > 5: steer = self._s_steer else: - steer = self._p_steer + # while doing the unstuck routine we don't want to steer + if self.__curr_behavior == "us_unstuck" or \ + self.__curr_behavior == "us_stop": + steer = 0 + else: + steer = self._p_steer + message = CarlaEgoVehicleControl() - message.reverse = False + message.reverse = self.__reverse message.hand_brake = False message.manual_gear_shift = False message.gear = 1 @@ -143,6 +165,14 @@ def loop(timer_event=None) -> None: self.new_timer(self.control_loop_rate, loop) self.spin() + def __set_curr_behavior(self, data: String) -> None: + """ + Sets the current behavior + :param data: + :return: + """ + self.__curr_behavior = data.data + def __set_emergency(self, data) -> None: """ In case of an emergency set the emergency flag to True ONCE @@ -213,6 +243,9 @@ def __set_throttle(self, data): def __set_brake(self, data): self.__brake = data.data + def __set_reverse(self, data): + self.__reverse = data.data + def __set_pure_pursuit_steer(self, data: Float32): r = (math.pi / 2) # convert from RAD to [-1;1] self._p_steer = (data.data / r) diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 11818caa..a1376e5f 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -4,8 +4,9 @@ from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber from simple_pid import PID -from std_msgs.msg import Float32 +from std_msgs.msg import Float32, Bool from nav_msgs.msg import Path +import rospy # TODO put back to 36 when controller can handle it SPEED_LIMIT_DEFAULT: float = 0 # 36.0 @@ -48,6 +49,11 @@ def __init__(self): f"/paf/{self.role_name}/brake", qos_profile=1) + self.reverse_pub: Publisher = self.new_publisher( + Bool, + f"/paf/{self.role_name}/reverse", + qos_profile=1) + # needed to prevent the car from driving before a path to follow is # available. Might be needed later to slow down in curves self.trajectory_sub: Subscriber = self.new_subscription( @@ -102,14 +108,30 @@ def loop(timer_event=None): return """ if self.__target_velocity < 0: - self.logerr("VelocityController doesn't support backward " - "driving yet.") - return + # self.logerr("VelocityController doesn't support backward " + # "driving yet.") + if self.__target_velocity == -3: + # -3 is the signal for reverse driving + + reverse = True + throttle = 1 + brake = 0 + rospy.loginfo("VelocityController: reverse driving") + + else: + # other negative values only lead to braking + reverse = False + brake = 1 + throttle = 0 + # very low target_velocities -> stand - if self.__target_velocity < 1: + elif self.__target_velocity < 1: + reverse = False brake = 1 throttle = 0 else: + reverse = False + v = self.__target_velocity pid_t.setpoint = v throttle = pid_t(self.__current_velocity) @@ -118,6 +140,8 @@ def loop(timer_event=None): throttle = 0 else: brake = 0 + + self.reverse_pub.publish(reverse) self.brake_pub.publish(brake) self.throttle_pub.publish(throttle) diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 5c549f47..eda141fe 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -23,6 +23,7 @@ def grow_a_tree(role_name): Selector ("Priorities", children=[ + behaviours.maneuvers.UnstuckRoutine("Unstuck Routine"), Selector("Road Features", children=[ behaviours.maneuvers.LeaveParkingSpace("Leave Parking Space"), diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index 248edd49..e2a02b22 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -79,3 +79,8 @@ def convert_to_ms(speed): # Cruise cruise = Behavior("Cruise", -1) + +# Unstuck Routine +us_unstuck = Behavior("us_unstuck", -3) +us_stop = Behavior("us_stop", 0) +us_overtake = Behavior("us_overtake", 0) diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 673cb99a..75044360 100755 --- a/code/planning/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -1,6 +1,6 @@ import py_trees import rospy -from std_msgs.msg import String +from std_msgs.msg import String, Float32, Bool import numpy as np from . import behavior_speed as bs # from behavior_agent.msg import BehaviorSpeed @@ -371,3 +371,251 @@ def terminate(self, new_status): """ self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) + + +def get_distance(pos_1, pos_2): + """Calculate the distance between two positions + + Args: + pos1 (np.array): Position 1 [#,#] + pos2 (np.array): Position 2 [#,#] + + Returns: + float: Distance + """ + + return np.linalg.norm(pos_1 - pos_2) + + +def pos_to_np_array(pos): + if pos is not None: + return np.array([pos.pose.position.x, pos.pose.position.y]) + else: + return None + + +TRIGGER_STUCK_DURATION = rospy.Duration(5) # default 5 (s) +TRIGGER_WAIT_STUCK_DURATION = rospy.Duration(20) # default 20 (s) +UNSTUCK_DRIVE_DURATION = rospy.Duration(1.2) # default 1.2 (s) +UNSTUCK_CLEAR_DISTANCE = 1.5 # default 1.5 (m) + + +class UnstuckRoutine(py_trees.behaviour.Behaviour): + + """ + This behavior is triggered when the vehicle is stuck and needs to be + unstuck. The behavior will then try to reverse and steer to the left or + right to get out of the stuck situation. + """ + def reset_stuck_values(self): + self.unstuck_overtake_count = 0 + self.stuck_timer = rospy.Time.now() + self.wait_stuck_timer = rospy.Time.now() + + def __init__(self, name): + """ + Minimal one-time initialisation. A good rule of thumb is to only + include the initialisation relevant for being able to insert this + behaviour in a tree for offline rendering to dot graphs. + + :param name: name of the behaviour + """ + super(UnstuckRoutine, self).__init__(name) + self.stuck_timer = rospy.Time.now() + self.wait_stuck_timer = rospy.Time.now() + self.stuck_duration = rospy.Duration(0) + self.wait_stuck_duration = rospy.Duration(0) + + self.init_pos = None + self.current_pos = None + self.current_speed = None + self.unstuck_overtake_count = 0 + dummy_pos = np.array([0, 0]) + self.last_unstuck_positions = np.array([dummy_pos, dummy_pos]) + + def setup(self, timeout): + """ + Delayed one-time initialisation that would otherwise interfere with + offline rendering of this behaviour in a tree to dot graph or + validation of the behaviour's configuration. + + This initializes the blackboard to be able to access data written to it + by the ROS topics. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as there is nothing to set up. + """ + self.curr_behavior_pub = rospy.Publisher("/paf/hero/" + "curr_behavior", + String, queue_size=1) + self.pub_unstuck_distance = rospy.Publisher("/paf/hero/" + "unstuck_distance", + Float32, queue_size=1) + self.pub_unstuck_flag = rospy.Publisher("/paf/hero/" + "unstuck_flag", + Bool, queue_size=1) + self.blackboard = py_trees.blackboard.Blackboard() + + return True + + def initialise(self): + """ + When is this called? + The first time your behaviour is ticked and anytime the status is not + RUNNING thereafter. + + What to do here? + Any initialisation you need before putting your behaviour to work. + :return: True + """ + + self.init_ros_stuck_time = rospy.Time.now() + + self.current_speed = self.blackboard.get("/carla/hero/Speed") + target_speed = self.blackboard.get("/paf/hero/target_velocity") + + # check for None values and initialize if necessary + if self.current_speed is None: + rospy.logdebug("current_speed is None") + self.reset_stuck_values() + return True + elif target_speed is None: + rospy.logdebug("target_speed is None") + self.reset_stuck_values() + return True + + # check if vehicle is not stuck, v > 1 + if self.current_speed.speed >= 1.0: + # reset wait stuck timer + self.wait_stuck_timer = rospy.Time.now() + # check if vehicle is not stuck, v > 1 when told to v > 0 + if target_speed.data >= 1.0: + # reset stuck timer + self.stuck_timer = rospy.Time.now() + + # update the stuck durations + self.stuck_duration = rospy.Time.now() - self.stuck_timer + self.wait_stuck_duration = rospy.Time.now() - self.wait_stuck_timer + + if self.stuck_duration >= TRIGGER_STUCK_DURATION: + rospy.logfatal(f"""Should be Driving but Stuck in one place + for more than {TRIGGER_STUCK_DURATION.secs}\n + -> starting unstuck routine""") + self.init_pos = pos_to_np_array( + self.blackboard.get("/paf/hero/current_pos")) + elif self.wait_stuck_duration >= TRIGGER_WAIT_STUCK_DURATION: + rospy.logfatal(f"""Wait Stuck in one place + for more than {TRIGGER_WAIT_STUCK_DURATION.secs} + \n + -> starting unstuck routine""") + self.init_pos = pos_to_np_array( + self.blackboard.get("/paf/hero/current_pos")) + + return True + + def update(self): + """ + When is this called? + Every time your behaviour is ticked. + + What to do here? + - Triggering, checking, monitoring. Anything...but do not block! + - Set a feedback message + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + + This behaviour doesn't do anything else than just keep running unless + there is a higher priority behaviour + + :return: py_trees.common.Status.RUNNING, keeps the decision tree from + finishing + """ + # def reset_stuck_values(): + # self.unstuck_overtake_count = 0 + # self.stuck_timer = rospy.Time.now() + # self.wait_stuck_timer = rospy.Time.now() + + self.current_pos = pos_to_np_array( + self.blackboard.get("/paf/hero/current_pos")) + self.current_speed = self.blackboard.get("/carla/hero/Speed") + + if self.init_pos is None or self.current_pos is None: + return py_trees.common.Status.FAILURE + # if no stuck detected, return failure + if self.stuck_duration < TRIGGER_STUCK_DURATION and \ + self.wait_stuck_duration < TRIGGER_WAIT_STUCK_DURATION: + # rospy.logfatal("No stuck detected.") + self.pub_unstuck_flag.publish(False) + # unstuck distance -1 is set, to reset the unstuck distance + self.pub_unstuck_distance.publish(-1) + return py_trees.common.Status.FAILURE + + # stuck detected -> unstuck routine + if rospy.Time.now()-self.init_ros_stuck_time < UNSTUCK_DRIVE_DURATION: + self.curr_behavior_pub.publish(bs.us_unstuck.name) + self.pub_unstuck_flag.publish(True) + rospy.logfatal("Unstuck routine running.") + return py_trees.common.Status.RUNNING + else: + # while vehicle is stopping publish us_stop + if abs(self.current_speed.speed) > 0.1: + self.curr_behavior_pub.publish(bs.us_stop.name) + return py_trees.common.Status.RUNNING + # vehicle has stopped: + unstuck_distance = get_distance(self.init_pos, + self.current_pos) + self.pub_unstuck_distance.publish(unstuck_distance) + + # check if vehicle needs to overtake: + # save current pos to last_unstuck_positions + self.last_unstuck_positions = np.roll(self.last_unstuck_positions, + -1, axis=0) + self.last_unstuck_positions[-1] = self.init_pos + + # if last unstuck was too far away, no overtake + # we only want to overtake when we tried to unstuck twice + # this case is the first time ever we tried to unstuck + if np.array_equal(self.last_unstuck_positions[0], + np.array([0, 0])): + self.reset_stuck_values() + rospy.logwarn("Unstuck routine finished.") + return py_trees.common.Status.FAILURE + # rospy.logfatal("Distance to last unstuck position: %s", + # get_distance(self.last_unstuck_positions[0], + # self.last_unstuck_positions[-1])) + # if the distance between the last and the first unstuck position + # is too far, we don't want to overtake, since its the first + # unstuck routine at this position on the map + if get_distance(self.last_unstuck_positions[0], + self.last_unstuck_positions[-1])\ + > UNSTUCK_CLEAR_DISTANCE: + self.reset_stuck_values() + rospy.logwarn("Unstuck routine finished.") + return py_trees.common.Status.FAILURE + + # once we tried the unstuck twice, we try to overtake + if self.current_speed.speed < 1: + # rospy.logwarn("Unstuck DISTANCE %s.", unstuck_distance) + + # publish the over take behavior 3 times to make sure + # it is detected + self.curr_behavior_pub.publish(bs.us_overtake.name) + if self.unstuck_overtake_count > 3: + self.reset_stuck_values() + rospy.logwarn("Unstuck routine finished.") + return py_trees.common.Status.FAILURE + else: + self.unstuck_overtake_count += 1 + return py_trees.common.Status.RUNNING + + def terminate(self, new_status): + """ + When is this called? + Whenever your behaviour switches to a non-running state. + - SUCCESS || FAILURE : your behaviour's work cycle has finished + - INVALID : a higher priority branch has interrupted, or shutting + down + + writes a status message to the console when the behaviour terminates + """ + self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % + (self.name, self.status, new_status)) diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index d26a717d..3c60ecdd 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -52,6 +52,8 @@ def create_node(role_name): 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/oncoming", 'msg': Float32, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/paf/{role_name}/target_velocity", 'msg': Float32, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} ] topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard") diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index e1ac88be..8cefc85b 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -7,7 +7,7 @@ from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo from nav_msgs.msg import Path # from std_msgs.msg import String -from std_msgs.msg import Float32MultiArray, Float32 +from std_msgs.msg import Float32MultiArray, Float32, Bool from collision_check import CollisionCheck import numpy as np @@ -22,6 +22,18 @@ def __init__(self): self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 1) + # Get Unstuck flag and distance + self.unstuck_flag_sub: Subscriber = self.new_subscription( + Bool, + f"/paf/{self.role_name}/unstuck_flag", + self.__get_unstuck_flag, + qos_profile=1) + self.unstuck_distance_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/unstuck_distance", + self.__get_unstuck_distance, + qos_profile=1) + # Get current speed self.velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, @@ -67,6 +79,10 @@ def __init__(self): f"/paf/{self.role_name}/speed_limit", qos_profile=1) + # unstuck attributes + self.__unstuck_flag: bool = False + self.__unstuck_distance: float = -1 + # List of all speed limits, sorted by waypoint index self.__speed_limits_OD: [float] = [] # Current Trajectory @@ -99,6 +115,22 @@ def __collision_callback(self, data: Float32): self.obstacle_speed = data.data[1] self.obstacle_distance = data.data[0] + def __get_unstuck_flag(self, data: Bool): + """_summary_ + + Args: + data (Bool): _description_ + """ + self.__unstuck_flag = data.data + + def __get_unstuck_distance(self, data: Float32): + """_summary_ + + Args: + data (Float32): _description_ + """ + self.__unstuck_distance = data.data + def __get_current_velocity(self, data: CarlaSpeedometer): """_summary_ @@ -148,6 +180,17 @@ def __current_position_callback(self, data: PoseStamped): self.speed_limit = \ self.__speed_limits_OD[self.__current_wp_index] self.speed_limit_publisher.publish(self.speed_limit) + # in case we used the unstuck routine to drive backwards + # we have to follow WPs that are already passed + elif self.__unstuck_flag: + if self.__unstuck_distance is None\ + or self.__unstuck_distance == -1: + return + self.__current_wp_index -= int(self.__unstuck_distance) + self.wp_publisher.publish(self.__current_wp_index) + self.speed_limit = \ + self.__speed_limits_OD[self.__current_wp_index] + self.speed_limit_publisher.publish(self.speed_limit) def run(self): """ diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 10f57135..b54468b1 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -22,6 +22,9 @@ # from scipy.spatial._kdtree import KDTree +UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE = 7.0 + + class MotionPlanning(CompatibleNode): """ This node selects speeds according to the behavior in the Decision Tree @@ -54,6 +57,12 @@ def __init__(self): self.__corners = None self.__in_corner = False self.calculated = False + + # unstuck routine variables + self.unstuck_distance = None + self.unstuck_overtake_flag = False + self.init_overtake_pos = None + # Subscriber self.test_sub = self.new_subscription( Float32, @@ -120,7 +129,14 @@ def __init__(self): self.__set_collision_point, qos_profile=1) + self.unstuck_distance_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/unstuck_distance", + self.__set_unstuck_distance, + qos_profile=1) + # Publisher + self.traj_pub: Publisher = self.new_publisher( msg_type=Path, topic=f"/paf/{self.role_name}/trajectory", @@ -143,6 +159,14 @@ def __init__(self): self.logdebug("MotionPlanning started") self.counter = 0 + def __set_unstuck_distance(self, data: Float32): + """Set unstuck distance + + Args: + data (Float32): Unstuck distance + """ + self.unstuck_distance = data.data + def __set_speed_limit(self, data: Float32): """Set current speed limit @@ -199,17 +223,23 @@ def change_trajectory(self, distance_obj): self.overtake_success_pub.publish(self.__overtake_status) return - def overtake_fallback(self, distance, pose_list): + def overtake_fallback(self, distance, pose_list, unstuck=False): # self.loginfo("Overtake Fallback!") # obstacle_position = approx_obstacle_pos(distance, # self.current_heading, # self.current_pos, # self.current_speed) - selection = pose_list[int(self.current_wp):int(self.current_wp) + + currentwp = self.current_wp + normal_x_offset = 2 + unstuck_x_offset = 3.5 # could need adjustment with better steering + selection = pose_list[int(currentwp):int(currentwp) + int(distance) + 7] waypoints = self.convert_pose_to_array(selection) - offset = np.array([2, 0, 0]) + if unstuck is True: + offset = np.array([unstuck_x_offset, 0, 0]) + else: + offset = np.array([normal_x_offset, 0, 0]) rotation_adjusted = Rotation.from_euler('z', self.current_heading + math.radians(90)) offset_front = rotation_adjusted.apply(offset) @@ -237,8 +267,8 @@ def overtake_fallback(self, distance, pose_list): path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "global" - path.poses = pose_list[:int(self.current_wp)] + \ - result + pose_list[int(self.current_wp + distance + 7):] + path.poses = pose_list[:int(currentwp)] + \ + result + pose_list[int(currentwp + distance + 7):] self.trajectory = path def __set_trajectory(self, data: Path): @@ -406,6 +436,7 @@ def get_speed_by_behavior(self, behavior: str) -> float: speed = 0.0 split = "_" short_behavior = behavior.partition(split)[0] + if short_behavior == "int": speed = self.__get_speed_intersection(behavior) elif short_behavior == "lc": @@ -414,11 +445,52 @@ def get_speed_by_behavior(self, behavior: str) -> float: speed = self.__get_speed_overtake(behavior) elif short_behavior == "parking": speed = bs.parking.speed + elif short_behavior == "us": + speed = self.__get_speed_unstuck(behavior) else: self.__overtake_status = -1 speed = self.__get_speed_cruise() return speed + def __get_speed_unstuck(self, behavior: str) -> float: + global UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE + speed = 0.0 + if behavior == bs.us_unstuck.name: + speed = bs.us_unstuck.speed + elif behavior == bs.us_stop.name: + speed = bs.us_stop.speed + elif behavior == bs.us_overtake.name: + pose_list = self.trajectory.poses + if self.unstuck_distance is None: + self.logfatal("Unstuck distance not set") + return speed + + if self.init_overtake_pos is not None \ + and self.current_pos is not None: + distance = np.linalg.norm( + self.init_overtake_pos[:2] - self.current_pos[:2]) + # self.logfatal(f"Unstuck Distance in mp: {distance}") + # clear distance to last unstuck -> avoid spamming overtake + if distance > UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE: + self.unstuck_overtake_flag = False + self.logwarn("Unstuck Overtake Flag Cleared") + + # to avoid spamming the overtake_fallback + if self.unstuck_overtake_flag is False: + # create overtake trajectory starting 6 meteres before + # the obstacle + # 6 worked well in tests, but can be adjusted + self.overtake_fallback(self.unstuck_distance, pose_list, + unstuck=True) + self.logfatal("Overtake Trajectory while unstuck!") + self.unstuck_overtake_flag = True + self.init_overtake_pos = self.current_pos[:2] + # else: overtake not possible + + speed = bs.us_overtake.speed + + return speed + def __get_speed_intersection(self, behavior: str) -> float: speed = 0.0 if behavior == bs.int_app_init.name: diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index c59675db..0871b552 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -29,6 +29,23 @@ } +def get_distance(pos_1, pos_2): + """Calculate the distance between two positions + + Args: + pos1 (np.array): Position 1 + pos2 (np.array): Position 2 + # the np.array should be in the form of + # np.array([data.pose.position.x, + data.pose.position.y, + data.pose.position.z]) + + Returns: + float: Distance + """ + return np.linalg.norm(pos_1 - pos_2) + + def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float): """Convert world coordinates to (lat,lon,z) coordinates Copied from: diff --git a/doc/05_acting/01_acting_testing.md b/doc/05_acting/01_acting_testing.md index 493e159e..3eeeea5f 100644 --- a/doc/05_acting/01_acting_testing.md +++ b/doc/05_acting/01_acting_testing.md @@ -1,36 +1,58 @@ -# How to test acting components +# Testing of Acting -**Summary:** This page shows ways to verify that acting components work as expected +**Summary:** This page shows ways to verify/test/tune/debug acting components. --- ## Author -Julian Graf +Alexander Hellmann ## Date -09.01.2023 +19.03.2024 +--- -* [How to test acting components](#how-to-test-acting-components) - * [Author](#author) - * [Date](#date) - * [velocity_controller](#velocitycontroller) - * [Dummys](#dummys) +- [Testing of Acting](#testing-of-acting) + - [Author](#author) + - [Date](#date) + - [Acting\_Debug\_Node](#acting_debug_node) + - [velocity\_controller](#velocity_controller) + - [Steering Controllers](#steering-controllers) + - [Dummys](#dummys) +--- + +## Acting_Debug_Node + +To test acting components independently from Perception and Planning, use the Acting_Debug_Node.py class in Acting. + +For this to work properly you have to manually disable all Planning and most of the Perception Components in your testing-branch: + +- Go to the planning.launch file (.../code/planning/launch/planning.launch) and disable every active node by commenting them out. +- Go to the perception.launch file (.../code/perception/launch/perception.launch) and disable every other node EXCEPT the Position_Publisher_Node.py and the kalman_filter.py by commenting them out. + +If you want to test/debug/tune under perfect conditions we also recommend following changes: + +- In the docker-compose.yml file (.../build/docker-compose.yml) you can switch to a developer enviroment by uncommenting the dev.launch (Line 65) and commenting the leaderboard (Line 66). +- In the dev_objects.json file (.../code/agent/config/dev_objects.json) you will find the GPS and IMU sensors. To use ideal sensors, set all noises to 0. +- In the perception.launch file, set the Position_Publisher_Node.py's pos_filter and heading_filter to None to get the ideal sensors' data (position and heading) unfiltered. + ## velocity_controller To test the velocity controller, set the parameter ```enabled``` in the acting launch-file true. Now the velocity_publisher_dummy will publish dummy target speeds. Use ```rqt_plot /carla/hero/velocity_as_float /carla/hero/max_velocity``` to visualize target and current speeds. ![image not found](./../00_assets/testing_velocity_pid.png) +## Steering Controllers + ## Dummys To test the steering controllers and the acc there are two dummys: -* the ```DummyTrajectoryPub``` publishes a simple path to test the steering controllers on, -* the ```AccDistancePublisherDummy``` publishes a dummy distances to an imaginary vehicle in front +- the ```DummyTrajectoryPub``` publishes a simple path to test the steering controllers on, +- the ```AccDistancePublisherDummy``` publishes a dummy distances to an imaginary vehicle in front To activate these dummys, make sure that they are started in the launch file and `````` is set in the launch file. diff --git a/doc/05_acting/04_main_frame_publisher.md b/doc/05_acting/04_main_frame_publisher.md index d6fa2ca7..3a0c3341 100644 --- a/doc/05_acting/04_main_frame_publisher.md +++ b/doc/05_acting/04_main_frame_publisher.md @@ -15,12 +15,12 @@ Julian Graf --- -* [Main frame publisher](#main-frame-publisher) - * [Author](#author) - * [Date](#date) - * [Main frame publisher](#main-frame-publisher-1) - * [How to use](#how-to-use) - * [Known issues](#known-issues) +- [Main frame publisher](#main-frame-publisher) + - [Author](#author) + - [Date](#date) + - [Main frame publisher](#main-frame-publisher-1) + - [How to use](#how-to-use) + - [Known issues](#known-issues) ## Main frame publisher diff --git a/doc/05_acting/02_acc.md b/doc/05_acting/OBSOLETE/02_acc.md similarity index 100% rename from doc/05_acting/02_acc.md rename to doc/05_acting/OBSOLETE/02_acc.md