diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 098ca5a0..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, Float32 +from std_msgs.msg import String, Float32, Bool import numpy as np from . import behavior_speed as bs # from behavior_agent.msg import BehaviorSpeed @@ -451,6 +451,9 @@ def setup(self, timeout): 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 @@ -541,11 +544,15 @@ def update(self): 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: @@ -554,6 +561,9 @@ def update(self): 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 @@ -584,10 +594,6 @@ def update(self): # once we tried the unstuck twice, we try to overtake if self.current_speed.speed < 1: - unstuck_distance = get_distance(self.init_pos, - self.current_pos) - - self.pub_unstuck_distance.publish(unstuck_distance) # rospy.logwarn("Unstuck DISTANCE %s.", unstuck_distance) # publish the over take behavior 3 times to make sure diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index 942f0409..3c60ecdd 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -53,7 +53,7 @@ def create_node(role_name): {'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}, + '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 b992ab02..591d73d0 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -231,7 +231,7 @@ def overtake_fallback(self, distance, pose_list, unstuck=False): # self.current_speed) currentwp = self.current_wp normal_x_offset = 2 - unstuck_x_offset = 4 # could need adjustment with better steering + unstuck_x_offset = 3.5 # could need adjustment with better steering # if overtake is called by the unstuck routine # -> reset the current wp to the distance driven backwards @@ -242,7 +242,8 @@ def overtake_fallback(self, distance, pose_list, unstuck=False): if distance > currentwp: currentwp = 0 else: - currentwp = currentwp - int(distance + 1) + distance = distance + 2 + # currentwp = currentwp - int(distance) # else: overtake starts from current wp selection = pose_list[int(currentwp):int(currentwp) +