From c5dc10e4da51ad57dda9ce2e92502e55a6d5b3f7 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Sat, 16 Mar 2024 21:52:02 +0100 Subject: [PATCH 01/13] feat(#212): Unstuck Routine functioning --- build/docker-compose.yml | 2 +- code/acting/launch/acting.launch | 5 +- code/acting/src/acting/vehicle_controller.py | 38 ++++- code/acting/src/acting/velocity_controller.py | 45 +++++- code/perception/launch/perception.launch | 8 +- .../src/behavior_agent/behavior_tree.py | 1 + .../behaviours/behavior_speed.py | 4 + .../behavior_agent/behaviours/maneuvers.py | 135 +++++++++++++++++- .../behaviours/topics2blackboard.py | 6 +- .../src/local_planner/motion_planning.py | 15 ++ 10 files changed, 241 insertions(+), 18 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1372dfdb..cb719fb8 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -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..ba711509 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,14 @@ def loop(timer_event=None) -> None: if self.__velocity > 5: steer = self._s_steer else: - steer = self._p_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 +164,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 +242,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..608d3f76 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,41 @@ 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.") + # TODO ROBERT implement backward driving + if self.__target_velocity == -3: + # -3 is the signal for reverse driving + # brake = 0 + reverse = True + # throttle = 1 + # TODO code wiederholung -> cleaner! + # v = self.__target_velocity + # pid_t.setpoint = v + # throttle = pid_t(self.__current_velocity) + # if throttle < 0: + # brake = abs(throttle) + # throttle = 0 + # else: + # brake = 0 + throttle = 1 + brake = 0 + rospy.loginfo("VelocityController: reverse driving") + + else: + # other negative values 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 +151,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/perception/launch/perception.launch b/code/perception/launch/perception.launch index b5e80823..05f60568 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -35,13 +35,13 @@ --> - + - - + diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 5c549f47..e0655e0c 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -78,6 +78,7 @@ def grow_a_tree(role_name): ]), ]), + behaviours.maneuvers.UnstuckRoutine("Unstuck Routine"), behaviours.maneuvers.Cruise("Cruise") ]) ]) diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index 248edd49..58a0ba0b 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -79,3 +79,7 @@ def convert_to_ms(speed): # Cruise cruise = Behavior("Cruise", -1) + +# Unstuck Routine +us_unstuck = Behavior("us_unstuck", -3) +us_stop = Behavior("us_stop", 0) diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 673cb99a..20198b37 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, Int32 import numpy as np from . import behavior_speed as bs # from behavior_agent.msg import BehaviorSpeed @@ -371,3 +371,136 @@ def terminate(self, new_status): """ self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) + + +class UnstuckRoutine(py_trees.behaviour.Behaviour): + + # TODO: Implement this behavior ROBERT + """ + 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 __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) + + 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) + # TODO ROBERT CHECK IF NEEDED + self.stuck_count_pub = rospy.Publisher("/paf/hero/stuck_count", + Int32, 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_time = rospy.Time.now() + + current_speed = self.blackboard.get("/carla/hero/Speed") + target_speed = self.blackboard.get("/paf/hero/target_velocity") + self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") + + if current_speed is None or target_speed is None or \ + self.stuck_count is None: + self.stuck_count_pub.publish(0) + self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") + rospy.loginfo("stuck_count initialized to %s", 0) + if current_speed is None: + rospy.loginfo("current_speed is None") + else: + rospy.loginfo("current_speed: %s", current_speed.speed) + if target_speed is None: + rospy.loginfo("target_speed is None") + else: + rospy.loginfo("target_speed: %s", target_speed.data) + return True + + if current_speed.speed < 1 and target_speed.data >= 1: + rospy.loginfo("stuck_count increased by 1, now: %s", + self.stuck_count.data + 1) + self.stuck_count_pub.publish(self.stuck_count.data + 1) + else: + self.stuck_count_pub.publish(0) + + if self.stuck_count.data >= 10: + rospy.loginfo("Stuck detected -> starting unstuck routine") + + 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 + """ + if self.stuck_count is None: + rospy.loginfo("stuck_count is None") + return py_trees.common.Status.FAILURE + # if no stuck detected, return failure + if self.stuck_count.data < 10: + return py_trees.common.Status.FAILURE + + # stuck detected -> unstuck routine + ros_time = rospy.Time.now() + if ros_time - self.init_ros_time < rospy.Duration(1): + self.curr_behavior_pub.publish(bs.us_unstuck.name) + rospy.loginfo("Unstuck routine running.") + return py_trees.common.Status.RUNNING + else: + self.curr_behavior_pub.publish(bs.us_stop.name) + if self.blackboard.get("/carla/hero/Speed").speed < 1: + rospy.loginfo("Unstuck routine finished.") + self.stuck_count_pub.publish(0) + return py_trees.common.Status.FAILURE + 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..6bd78398 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -4,7 +4,7 @@ import py_trees import py_trees_ros -from std_msgs.msg import Float32, Bool, Float32MultiArray +from std_msgs.msg import Float32, Bool, Float32MultiArray, Int32 from carla_msgs.msg import CarlaSpeedometer from geometry_msgs.msg import PoseStamped @@ -52,6 +52,10 @@ 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}/stuck_count", 'msg': Int32, + '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/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 10f57135..de9ac1cb 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -121,6 +121,7 @@ def __init__(self): qos_profile=1) # Publisher + self.traj_pub: Publisher = self.new_publisher( msg_type=Path, topic=f"/paf/{self.role_name}/trajectory", @@ -367,6 +368,8 @@ def __check_emergency(self, data: Bool): def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) + if behavior == bs.us_unstuck.name or behavior == bs.us_stop.name: + rospy.loginfo(f"Behavior: {behavior}, Speed: {be_speed}") if behavior == bs.parking.name or self.__overtake_status == 1: self.target_speed = be_speed else: @@ -406,6 +409,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 +418,22 @@ 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: + 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 + + return speed + def __get_speed_intersection(self, behavior: str) -> float: speed = 0.0 if behavior == bs.int_app_init.name: From 08c4852c05a97756b4c3b1435d5b2094880ff74e Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 14 Mar 2024 14:11:26 +0100 Subject: [PATCH 02/13] feat: hotfixing steering for leaderboard --- code/acting/launch/acting.launch | 4 ++-- .../src/acting/pure_pursuit_controller.py | 7 +++--- .../{OBSOLETE => }/stanley_controller.py | 9 +++----- code/acting/src/acting/vehicle_controller.py | 22 ++++++++++++++++--- 4 files changed, 28 insertions(+), 14 deletions(-) rename code/acting/src/acting/{OBSOLETE => }/stanley_controller.py (97%) 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): From e7a454132d4cfca3beb5c04d7f2142ace4e602cb Mon Sep 17 00:00:00 2001 From: robertik10 Date: Sat, 16 Mar 2024 21:52:02 +0100 Subject: [PATCH 03/13] feat(#212): Unstuck Routine functioning --- build/docker-compose.yml | 2 +- code/acting/launch/acting.launch | 5 +- code/acting/src/acting/vehicle_controller.py | 38 ++++- code/acting/src/acting/velocity_controller.py | 45 +++++- code/perception/launch/perception.launch | 8 +- .../src/behavior_agent/behavior_tree.py | 1 + .../behaviours/behavior_speed.py | 4 + .../behavior_agent/behaviours/maneuvers.py | 135 +++++++++++++++++- .../behaviours/topics2blackboard.py | 6 +- .../src/local_planner/motion_planning.py | 15 ++ 10 files changed, 241 insertions(+), 18 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1372dfdb..cb719fb8 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -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..ba711509 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,14 @@ def loop(timer_event=None) -> None: if self.__velocity > 5: steer = self._s_steer else: - steer = self._p_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 +164,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 +242,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..608d3f76 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,41 @@ 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.") + # TODO ROBERT implement backward driving + if self.__target_velocity == -3: + # -3 is the signal for reverse driving + # brake = 0 + reverse = True + # throttle = 1 + # TODO code wiederholung -> cleaner! + # v = self.__target_velocity + # pid_t.setpoint = v + # throttle = pid_t(self.__current_velocity) + # if throttle < 0: + # brake = abs(throttle) + # throttle = 0 + # else: + # brake = 0 + throttle = 1 + brake = 0 + rospy.loginfo("VelocityController: reverse driving") + + else: + # other negative values 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 +151,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/perception/launch/perception.launch b/code/perception/launch/perception.launch index b5e80823..05f60568 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -35,13 +35,13 @@ --> - + - - + diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 5c549f47..e0655e0c 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -78,6 +78,7 @@ def grow_a_tree(role_name): ]), ]), + behaviours.maneuvers.UnstuckRoutine("Unstuck Routine"), behaviours.maneuvers.Cruise("Cruise") ]) ]) diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index 248edd49..58a0ba0b 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -79,3 +79,7 @@ def convert_to_ms(speed): # Cruise cruise = Behavior("Cruise", -1) + +# Unstuck Routine +us_unstuck = Behavior("us_unstuck", -3) +us_stop = Behavior("us_stop", 0) diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 673cb99a..20198b37 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, Int32 import numpy as np from . import behavior_speed as bs # from behavior_agent.msg import BehaviorSpeed @@ -371,3 +371,136 @@ def terminate(self, new_status): """ self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) + + +class UnstuckRoutine(py_trees.behaviour.Behaviour): + + # TODO: Implement this behavior ROBERT + """ + 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 __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) + + 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) + # TODO ROBERT CHECK IF NEEDED + self.stuck_count_pub = rospy.Publisher("/paf/hero/stuck_count", + Int32, 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_time = rospy.Time.now() + + current_speed = self.blackboard.get("/carla/hero/Speed") + target_speed = self.blackboard.get("/paf/hero/target_velocity") + self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") + + if current_speed is None or target_speed is None or \ + self.stuck_count is None: + self.stuck_count_pub.publish(0) + self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") + rospy.loginfo("stuck_count initialized to %s", 0) + if current_speed is None: + rospy.loginfo("current_speed is None") + else: + rospy.loginfo("current_speed: %s", current_speed.speed) + if target_speed is None: + rospy.loginfo("target_speed is None") + else: + rospy.loginfo("target_speed: %s", target_speed.data) + return True + + if current_speed.speed < 1 and target_speed.data >= 1: + rospy.loginfo("stuck_count increased by 1, now: %s", + self.stuck_count.data + 1) + self.stuck_count_pub.publish(self.stuck_count.data + 1) + else: + self.stuck_count_pub.publish(0) + + if self.stuck_count.data >= 10: + rospy.loginfo("Stuck detected -> starting unstuck routine") + + 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 + """ + if self.stuck_count is None: + rospy.loginfo("stuck_count is None") + return py_trees.common.Status.FAILURE + # if no stuck detected, return failure + if self.stuck_count.data < 10: + return py_trees.common.Status.FAILURE + + # stuck detected -> unstuck routine + ros_time = rospy.Time.now() + if ros_time - self.init_ros_time < rospy.Duration(1): + self.curr_behavior_pub.publish(bs.us_unstuck.name) + rospy.loginfo("Unstuck routine running.") + return py_trees.common.Status.RUNNING + else: + self.curr_behavior_pub.publish(bs.us_stop.name) + if self.blackboard.get("/carla/hero/Speed").speed < 1: + rospy.loginfo("Unstuck routine finished.") + self.stuck_count_pub.publish(0) + return py_trees.common.Status.FAILURE + 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..6bd78398 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -4,7 +4,7 @@ import py_trees import py_trees_ros -from std_msgs.msg import Float32, Bool, Float32MultiArray +from std_msgs.msg import Float32, Bool, Float32MultiArray, Int32 from carla_msgs.msg import CarlaSpeedometer from geometry_msgs.msg import PoseStamped @@ -52,6 +52,10 @@ 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}/stuck_count", 'msg': Int32, + '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/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 10f57135..de9ac1cb 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -121,6 +121,7 @@ def __init__(self): qos_profile=1) # Publisher + self.traj_pub: Publisher = self.new_publisher( msg_type=Path, topic=f"/paf/{self.role_name}/trajectory", @@ -367,6 +368,8 @@ def __check_emergency(self, data: Bool): def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) + if behavior == bs.us_unstuck.name or behavior == bs.us_stop.name: + rospy.loginfo(f"Behavior: {behavior}, Speed: {be_speed}") if behavior == bs.parking.name or self.__overtake_status == 1: self.target_speed = be_speed else: @@ -406,6 +409,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 +418,22 @@ 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: + 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 + + return speed + def __get_speed_intersection(self, behavior: str) -> float: speed = 0.0 if behavior == bs.int_app_init.name: From adce93e32ec9eedbabf04dc0b05e4e6841ebe0b9 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Sat, 16 Mar 2024 21:52:02 +0100 Subject: [PATCH 04/13] feat(#212): Unstuck Routine functioning --- build/docker-compose.yml | 2 +- code/acting/launch/acting.launch | 5 +- code/acting/src/acting/vehicle_controller.py | 38 ++++- code/acting/src/acting/velocity_controller.py | 45 +++++- code/perception/launch/perception.launch | 8 +- .../src/behavior_agent/behavior_tree.py | 1 + .../behaviours/behavior_speed.py | 4 + .../behavior_agent/behaviours/maneuvers.py | 135 +++++++++++++++++- .../behaviours/topics2blackboard.py | 6 +- .../src/local_planner/motion_planning.py | 15 ++ 10 files changed, 241 insertions(+), 18 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1372dfdb..cb719fb8 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -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..ba711509 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,14 @@ def loop(timer_event=None) -> None: if self.__velocity > 5: steer = self._s_steer else: - steer = self._p_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 +164,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 +242,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..608d3f76 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,41 @@ 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.") + # TODO ROBERT implement backward driving + if self.__target_velocity == -3: + # -3 is the signal for reverse driving + # brake = 0 + reverse = True + # throttle = 1 + # TODO code wiederholung -> cleaner! + # v = self.__target_velocity + # pid_t.setpoint = v + # throttle = pid_t(self.__current_velocity) + # if throttle < 0: + # brake = abs(throttle) + # throttle = 0 + # else: + # brake = 0 + throttle = 1 + brake = 0 + rospy.loginfo("VelocityController: reverse driving") + + else: + # other negative values 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 +151,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/perception/launch/perception.launch b/code/perception/launch/perception.launch index b5e80823..05f60568 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -35,13 +35,13 @@ --> - + - - + diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 5c549f47..e0655e0c 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -78,6 +78,7 @@ def grow_a_tree(role_name): ]), ]), + behaviours.maneuvers.UnstuckRoutine("Unstuck Routine"), behaviours.maneuvers.Cruise("Cruise") ]) ]) diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index 248edd49..58a0ba0b 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -79,3 +79,7 @@ def convert_to_ms(speed): # Cruise cruise = Behavior("Cruise", -1) + +# Unstuck Routine +us_unstuck = Behavior("us_unstuck", -3) +us_stop = Behavior("us_stop", 0) diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 673cb99a..20198b37 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, Int32 import numpy as np from . import behavior_speed as bs # from behavior_agent.msg import BehaviorSpeed @@ -371,3 +371,136 @@ def terminate(self, new_status): """ self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) + + +class UnstuckRoutine(py_trees.behaviour.Behaviour): + + # TODO: Implement this behavior ROBERT + """ + 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 __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) + + 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) + # TODO ROBERT CHECK IF NEEDED + self.stuck_count_pub = rospy.Publisher("/paf/hero/stuck_count", + Int32, 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_time = rospy.Time.now() + + current_speed = self.blackboard.get("/carla/hero/Speed") + target_speed = self.blackboard.get("/paf/hero/target_velocity") + self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") + + if current_speed is None or target_speed is None or \ + self.stuck_count is None: + self.stuck_count_pub.publish(0) + self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") + rospy.loginfo("stuck_count initialized to %s", 0) + if current_speed is None: + rospy.loginfo("current_speed is None") + else: + rospy.loginfo("current_speed: %s", current_speed.speed) + if target_speed is None: + rospy.loginfo("target_speed is None") + else: + rospy.loginfo("target_speed: %s", target_speed.data) + return True + + if current_speed.speed < 1 and target_speed.data >= 1: + rospy.loginfo("stuck_count increased by 1, now: %s", + self.stuck_count.data + 1) + self.stuck_count_pub.publish(self.stuck_count.data + 1) + else: + self.stuck_count_pub.publish(0) + + if self.stuck_count.data >= 10: + rospy.loginfo("Stuck detected -> starting unstuck routine") + + 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 + """ + if self.stuck_count is None: + rospy.loginfo("stuck_count is None") + return py_trees.common.Status.FAILURE + # if no stuck detected, return failure + if self.stuck_count.data < 10: + return py_trees.common.Status.FAILURE + + # stuck detected -> unstuck routine + ros_time = rospy.Time.now() + if ros_time - self.init_ros_time < rospy.Duration(1): + self.curr_behavior_pub.publish(bs.us_unstuck.name) + rospy.loginfo("Unstuck routine running.") + return py_trees.common.Status.RUNNING + else: + self.curr_behavior_pub.publish(bs.us_stop.name) + if self.blackboard.get("/carla/hero/Speed").speed < 1: + rospy.loginfo("Unstuck routine finished.") + self.stuck_count_pub.publish(0) + return py_trees.common.Status.FAILURE + 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..6bd78398 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -4,7 +4,7 @@ import py_trees import py_trees_ros -from std_msgs.msg import Float32, Bool, Float32MultiArray +from std_msgs.msg import Float32, Bool, Float32MultiArray, Int32 from carla_msgs.msg import CarlaSpeedometer from geometry_msgs.msg import PoseStamped @@ -52,6 +52,10 @@ 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}/stuck_count", 'msg': Int32, + '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/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 10f57135..de9ac1cb 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -121,6 +121,7 @@ def __init__(self): qos_profile=1) # Publisher + self.traj_pub: Publisher = self.new_publisher( msg_type=Path, topic=f"/paf/{self.role_name}/trajectory", @@ -367,6 +368,8 @@ def __check_emergency(self, data: Bool): def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) + if behavior == bs.us_unstuck.name or behavior == bs.us_stop.name: + rospy.loginfo(f"Behavior: {behavior}, Speed: {be_speed}") if behavior == bs.parking.name or self.__overtake_status == 1: self.target_speed = be_speed else: @@ -406,6 +409,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 +418,22 @@ 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: + 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 + + return speed + def __get_speed_intersection(self, behavior: str) -> float: speed = 0.0 if behavior == bs.int_app_init.name: From c7e997eec85adfaf38bc751391691dbd1df02235 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Tue, 19 Mar 2024 01:07:20 +0100 Subject: [PATCH 05/13] feat(#212): fully functional unstuck routine --- build/docker-compose.yml | 2 +- code/agent/config/rviz_config.rviz | 34 ++-- code/perception/launch/perception.launch | 8 +- .../src/behavior_agent/behavior_tree.py | 2 +- .../behaviours/behavior_speed.py | 1 + .../behavior_agent/behaviours/maneuvers.py | 189 ++++++++++++++---- .../behaviours/topics2blackboard.py | 2 + .../src/local_planner/motion_planning.py | 51 ++++- code/planning/src/local_planner/utils.py | 17 ++ 9 files changed, 244 insertions(+), 62 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index cb719fb8..0aaabfab 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=Medium -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: diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 2e419cc2..0602e231 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -8,7 +8,7 @@ Panels: - /PointCloud24 - /PointCloud25 Splitter Ratio: 0.5 - Tree Height: 308 + Tree Height: 327 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -68,24 +68,20 @@ Visualization Manager: Path: false PointCloud2: false Value: true + VisonNode Output: true Zoom Factor: 1 - Class: rviz/Image Enabled: true - Image Rendering: background and overlay Image Topic: /paf/hero/Center/segmented_image + Max Value: 1 + Median window: 5 + Min Value: 0 Name: VisonNode Output - Overlay Alpha: 0.5 + Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Visibility: - Grid: true - Imu: true - Path: true - PointCloud2: true - Value: true - Zoom Factor: 1 - Alpha: 1 Class: rviz_plugin_tutorials/Imu Color: 204; 51; 204 @@ -288,7 +284,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 34.785499572753906 + Distance: 356.6126708984375 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -304,19 +300,19 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.19039836525917053 + Pitch: 1.5697963237762451 Target Frame: - Yaw: 4.520427227020264 + Yaw: 5.0054426193237305 Saved: ~ Window Geometry: Camera: collapsed: false Displays: collapsed: false - Height: 1376 + Height: 1403 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000022a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026b000002960000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000304000004e1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001d0000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002110000022a0000001600fffffffb00000020005600690073006f006e004e006f006400650020004f007500740070007500740100000441000000db0000001600ffffff000000010000010f000004e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004e1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005e1000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -325,6 +321,8 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2488 - X: 1992 - Y: 27 \ No newline at end of file + VisonNode Output: + collapsed: false + Width: 2560 + X: 2560 + Y: 0 diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 05f60568..b5e80823 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -35,13 +35,13 @@ --> - + - + + diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index e0655e0c..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"), @@ -78,7 +79,6 @@ def grow_a_tree(role_name): ]), ]), - behaviours.maneuvers.UnstuckRoutine("Unstuck Routine"), behaviours.maneuvers.Cruise("Cruise") ]) ]) diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index 58a0ba0b..e2a02b22 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -83,3 +83,4 @@ def convert_to_ms(speed): # 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 20198b37..5db298a9 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, Int32 +from std_msgs.msg import String, Float32 import numpy as np from . import behavior_speed as bs # from behavior_agent.msg import BehaviorSpeed @@ -373,6 +373,32 @@ def terminate(self, new_status): (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_COUNT = 30 # default 40 (about 5 s) +TRIGGER_WAIT_STUCK_COUNT = 100 # default 100 (about 20 s) +UNSTUCK_DRIVE_DURATION = 1.2 # default 1.2 (s) + + class UnstuckRoutine(py_trees.behaviour.Behaviour): # TODO: Implement this behavior ROBERT @@ -381,6 +407,7 @@ class UnstuckRoutine(py_trees.behaviour.Behaviour): unstuck. The behavior will then try to reverse and steer to the left or right to get out of the stuck situation. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -390,6 +417,14 @@ def __init__(self, name): :param name: name of the behaviour """ super(UnstuckRoutine, self).__init__(name) + self.stuck_count = 0 + self.wait_stuck_count = 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): """ @@ -406,10 +441,16 @@ def setup(self, timeout): self.curr_behavior_pub = rospy.Publisher("/paf/hero/" "curr_behavior", String, queue_size=1) - # TODO ROBERT CHECK IF NEEDED - self.stuck_count_pub = rospy.Publisher("/paf/hero/stuck_count", - Int32, queue_size=1) + # self.stuck_count_pub = rospy.Publisher("/paf/hero/stuck_count", + # Int32, queue_size=1) + # self.wait_stuck_count_pub = rospy.Publisher("/paf/hero/" + # "wait_stuck_count", + # Int32, queue_size=1) + self.pub_unstuck_distance = rospy.Publisher("/paf/hero/" + "unstuck_distance", + Float32, queue_size=1) self.blackboard = py_trees.blackboard.Blackboard() + return True def initialise(self): @@ -424,34 +465,57 @@ def initialise(self): """ self.init_ros_time = rospy.Time.now() - current_speed = self.blackboard.get("/carla/hero/Speed") + self.current_speed = self.blackboard.get("/carla/hero/Speed") target_speed = self.blackboard.get("/paf/hero/target_velocity") - self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") - - if current_speed is None or target_speed is None or \ - self.stuck_count is None: - self.stuck_count_pub.publish(0) - self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") - rospy.loginfo("stuck_count initialized to %s", 0) - if current_speed is None: - rospy.loginfo("current_speed is None") - else: - rospy.loginfo("current_speed: %s", current_speed.speed) - if target_speed is None: - rospy.loginfo("target_speed is None") - else: - rospy.loginfo("target_speed: %s", target_speed.data) + # self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") + # self.wait_stuck_count = self.blackboard.get("/paf/hero/" + # "wait_stuck_count") + + # check for None values and initialize if necessary + if self.current_speed is None or target_speed is None: + # self.stuck_count_pub.publish(0) + # self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") + # rospy.loginfo("stuck_count initialized to %s", 0) + # self.wait_stuck_count_pub.publish(0) + # self.wait_stuck_count = self.blackboard.get("/paf/hero/" + # "wait_stuck_count") + # rospy.loginfo("wait_stuck_count initialized to %s", 0) + if self.current_speed is None: + rospy.logdebug("current_speed is None") + elif target_speed is None: + rospy.logdebug("target_speed is None") return True - if current_speed.speed < 1 and target_speed.data >= 1: + # check if vehicle is stuck, v = 0 when told to v > 0 + if self.current_speed.speed < 1 and target_speed.data >= 1: rospy.loginfo("stuck_count increased by 1, now: %s", - self.stuck_count.data + 1) - self.stuck_count_pub.publish(self.stuck_count.data + 1) + self.stuck_count + 1) + # self.stuck_count_pub.publish(self.stuck_count.data + 1) + self.stuck_count += 1 else: - self.stuck_count_pub.publish(0) - - if self.stuck_count.data >= 10: - rospy.loginfo("Stuck detected -> starting unstuck routine") + # self.stuck_count_pub.publish(0) + self.stuck_count = 0 + # check if vehicle is stuck in wait loop + # for example when waiting for a traffic light seems to take too long + # -> something might have went wrong + if self.current_speed.speed < 1.0: + rospy.loginfo("wait_stuck_count increased by 1, now: %s", + self.wait_stuck_count + 1) + # self.wait_stuck_count_pub.publish(self.wait_stuck_count.data + 1) + self.wait_stuck_count += 1 + else: + # self.wait_stuck_count_pub.publish(0) + self.wait_stuck_count = 0 + + # inform about which kind of stuck situation is detected + if self.stuck_count >= TRIGGER_STUCK_COUNT: + rospy.logwarn("Stuck detected -> starting unstuck routine") + self.init_pos = pos_to_np_array( + self.blackboard.get("/paf/hero/current_pos")) + elif self.wait_stuck_count >= TRIGGER_WAIT_STUCK_COUNT: + rospy.logwarn("Wait stuck detected -> starting unstuck routine") + self.init_pos = pos_to_np_array( + self.blackboard.get("/paf/hero/current_pos")) return True @@ -471,26 +535,79 @@ def update(self): :return: py_trees.common.Status.RUNNING, keeps the decision tree from finishing """ + + 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.stuck_count is None: - rospy.loginfo("stuck_count is None") + # rospy.logwarn("stuck_count is None") return py_trees.common.Status.FAILURE # if no stuck detected, return failure - if self.stuck_count.data < 10: + if self.stuck_count < TRIGGER_STUCK_COUNT and \ + self.wait_stuck_count < TRIGGER_WAIT_STUCK_COUNT: return py_trees.common.Status.FAILURE # stuck detected -> unstuck routine ros_time = rospy.Time.now() - if ros_time - self.init_ros_time < rospy.Duration(1): + if ros_time - self.init_ros_time < \ + rospy.Duration(UNSTUCK_DRIVE_DURATION): + self.curr_behavior_pub.publish(bs.us_unstuck.name) - rospy.loginfo("Unstuck routine running.") + rospy.logwarn("Unstuck routine running.") return py_trees.common.Status.RUNNING else: - self.curr_behavior_pub.publish(bs.us_stop.name) - if self.blackboard.get("/carla/hero/Speed").speed < 1: - rospy.loginfo("Unstuck routine finished.") - self.stuck_count_pub.publish(0) + # while vehicle is stopping publish us_stop + if self.current_speed.speed < -0.1: + self.curr_behavior_pub.publish(bs.us_stop.name) + return py_trees.common.Status.RUNNING + # vehicle has stopped: + # 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 + if np.array_equal(self.last_unstuck_positions[0], + np.array([0, 0])): + self.stuck_count = 0 + self.wait_stuck_count = 0 + self.unstuck_overtake_count = 0 return py_trees.common.Status.FAILURE - return py_trees.common.Status.RUNNING + rospy.logfatal("Distance to last unstuck position: %s", + get_distance(self.last_unstuck_positions[0], + self.last_unstuck_positions[-1])) + if get_distance(self.last_unstuck_positions[0], + self.last_unstuck_positions[-1]) > 1.5: + self.stuck_count = 0 + self.wait_stuck_count = 0 + self.unstuck_overtake_count = 0 + return py_trees.common.Status.FAILURE + + # 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) + # if distance_to_waypoint < 0.5: + + self.pub_unstuck_distance.publish(unstuck_distance) + rospy.logwarn("Unstuck DISTANCE %s.", unstuck_distance) + self.curr_behavior_pub.publish(bs.us_overtake.name) + + # self.stuck_count_pub.publish(0) + # self.wait_stuck_count_pub.publish(0) + if self.unstuck_overtake_count > 3: + self.stuck_count = 0 + self.wait_stuck_count = 0 + self.unstuck_overtake_count = 0 + 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): """ diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index 6bd78398..3b2e2885 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -55,6 +55,8 @@ def create_node(role_name): {'name': f"/paf/{role_name}/stuck_count", 'msg': Int32, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/target_velocity", 'msg': Float32, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/paf/{role_name}/wait_stuck_count", 'msg': Int32, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} ] diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index de9ac1cb..970f0986 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,6 +129,12 @@ 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( @@ -144,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 @@ -368,8 +391,6 @@ def __check_emergency(self, data: Bool): def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) - if behavior == bs.us_unstuck.name or behavior == bs.us_stop.name: - rospy.loginfo(f"Behavior: {behavior}, Speed: {be_speed}") if behavior == bs.parking.name or self.__overtake_status == 1: self.target_speed = be_speed else: @@ -426,11 +447,37 @@ def get_speed_by_behavior(self, behavior: str) -> float: 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}") + if distance > UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE: + self.unstuck_overtake_flag = False + self.logfatal("Unstuck Overtake Flag Cleared") + + # to avoid spamming the overtake_fallback + if self.unstuck_overtake_flag is False: + # create overtake trajectory 6 meteres before the obstacle + self.overtake_fallback(self.unstuck_distance + 6, pose_list) + self.logfatal("Overtake fallback 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 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: From b32b7fa9a3bfa9b0541f8c949bb129bcfbf621c6 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Tue, 19 Mar 2024 15:13:44 +0100 Subject: [PATCH 06/13] feat: first update, testing not finished --- doc/05_acting/01_acting_testing.md | 44 ++++++++++++++++++------ doc/05_acting/04_main_frame_publisher.md | 12 +++---- doc/05_acting/{ => OBSOLETE}/02_acc.md | 0 3 files changed, 39 insertions(+), 17 deletions(-) rename doc/05_acting/{ => OBSOLETE}/02_acc.md (100%) 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 From 602dbf0196a65e60b8d9b88fe795128df10459cb Mon Sep 17 00:00:00 2001 From: robertik10 Date: Tue, 19 Mar 2024 16:51:48 +0100 Subject: [PATCH 07/13] fix: change rviz to how it is on the current main --- code/agent/config/rviz_config.rviz | 34 ++++++++++++++++-------------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 0602e231..2e419cc2 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -8,7 +8,7 @@ Panels: - /PointCloud24 - /PointCloud25 Splitter Ratio: 0.5 - Tree Height: 327 + Tree Height: 308 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -68,20 +68,24 @@ Visualization Manager: Path: false PointCloud2: false Value: true - VisonNode Output: true Zoom Factor: 1 - Class: rviz/Image Enabled: true + Image Rendering: background and overlay Image Topic: /paf/hero/Center/segmented_image - Max Value: 1 - Median window: 5 - Min Value: 0 Name: VisonNode Output - Normalize Range: true + Overlay Alpha: 0.5 Queue Size: 2 Transport Hint: raw Unreliable: false Value: true + Visibility: + Grid: true + Imu: true + Path: true + PointCloud2: true + Value: true + Zoom Factor: 1 - Alpha: 1 Class: rviz_plugin_tutorials/Imu Color: 204; 51; 204 @@ -284,7 +288,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 356.6126708984375 + Distance: 34.785499572753906 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -300,19 +304,19 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 0.19039836525917053 Target Frame: - Yaw: 5.0054426193237305 + Yaw: 4.520427227020264 Saved: ~ Window Geometry: Camera: collapsed: false Displays: collapsed: false - Height: 1403 + Height: 1376 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000304000004e1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001d0000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002110000022a0000001600fffffffb00000020005600690073006f006e004e006f006400650020004f007500740070007500740100000441000000db0000001600ffffff000000010000010f000004e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004e1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005e1000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000022a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026b000002960000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -321,8 +325,6 @@ Window Geometry: collapsed: false Views: collapsed: false - VisonNode Output: - collapsed: false - Width: 2560 - X: 2560 - Y: 0 + Width: 2488 + X: 1992 + Y: 27 \ No newline at end of file From b6df84623b9ec38217d20dd11549baaa97c92c58 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Tue, 19 Mar 2024 17:06:39 +0100 Subject: [PATCH 08/13] fix: last fixes before merge --- build/docker-compose.yml | 2 +- code/acting/src/acting/vehicle_controller.py | 1 + code/acting/src/acting/velocity_controller.py | 15 +------ .../behavior_agent/behaviours/maneuvers.py | 45 +++++-------------- .../behaviours/topics2blackboard.py | 6 +-- .../src/local_planner/motion_planning.py | 7 ++- 6 files changed, 22 insertions(+), 54 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 0aaabfab..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=Medium -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: diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index ba711509..5b50e951 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -143,6 +143,7 @@ def loop(timer_event=None) -> None: if self.__velocity > 5: steer = self._s_steer else: + # 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 diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 608d3f76..a1376e5f 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -110,27 +110,16 @@ def loop(timer_event=None): if self.__target_velocity < 0: # self.logerr("VelocityController doesn't support backward " # "driving yet.") - # TODO ROBERT implement backward driving if self.__target_velocity == -3: # -3 is the signal for reverse driving - # brake = 0 + reverse = True - # throttle = 1 - # TODO code wiederholung -> cleaner! - # v = self.__target_velocity - # pid_t.setpoint = v - # throttle = pid_t(self.__current_velocity) - # if throttle < 0: - # brake = abs(throttle) - # throttle = 0 - # else: - # brake = 0 throttle = 1 brake = 0 rospy.loginfo("VelocityController: reverse driving") else: - # other negative values lead to braking + # other negative values only lead to braking reverse = False brake = 1 throttle = 0 diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 5db298a9..96b94d92 100755 --- a/code/planning/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -394,14 +394,13 @@ def pos_to_np_array(pos): return None -TRIGGER_STUCK_COUNT = 30 # default 40 (about 5 s) -TRIGGER_WAIT_STUCK_COUNT = 100 # default 100 (about 20 s) +TRIGGER_STUCK_COUNT = 30 # default 30 (about 5 s -> behavior tree 5 Hz) +TRIGGER_WAIT_STUCK_COUNT = 100 # default 100 (about 20 s-> behavior tree 5 Hz) UNSTUCK_DRIVE_DURATION = 1.2 # default 1.2 (s) class UnstuckRoutine(py_trees.behaviour.Behaviour): - # TODO: Implement this behavior ROBERT """ 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 @@ -441,11 +440,6 @@ def setup(self, timeout): self.curr_behavior_pub = rospy.Publisher("/paf/hero/" "curr_behavior", String, queue_size=1) - # self.stuck_count_pub = rospy.Publisher("/paf/hero/stuck_count", - # Int32, queue_size=1) - # self.wait_stuck_count_pub = rospy.Publisher("/paf/hero/" - # "wait_stuck_count", - # Int32, queue_size=1) self.pub_unstuck_distance = rospy.Publisher("/paf/hero/" "unstuck_distance", Float32, queue_size=1) @@ -467,19 +461,9 @@ def initialise(self): self.current_speed = self.blackboard.get("/carla/hero/Speed") target_speed = self.blackboard.get("/paf/hero/target_velocity") - # self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") - # self.wait_stuck_count = self.blackboard.get("/paf/hero/" - # "wait_stuck_count") # check for None values and initialize if necessary if self.current_speed is None or target_speed is None: - # self.stuck_count_pub.publish(0) - # self.stuck_count = self.blackboard.get("/paf/hero/stuck_count") - # rospy.loginfo("stuck_count initialized to %s", 0) - # self.wait_stuck_count_pub.publish(0) - # self.wait_stuck_count = self.blackboard.get("/paf/hero/" - # "wait_stuck_count") - # rospy.loginfo("wait_stuck_count initialized to %s", 0) if self.current_speed is None: rospy.logdebug("current_speed is None") elif target_speed is None: @@ -488,32 +472,28 @@ def initialise(self): # check if vehicle is stuck, v = 0 when told to v > 0 if self.current_speed.speed < 1 and target_speed.data >= 1: - rospy.loginfo("stuck_count increased by 1, now: %s", - self.stuck_count + 1) - # self.stuck_count_pub.publish(self.stuck_count.data + 1) + # rospy.loginfo("stuck_count increased by 1, now: %s", + # self.stuck_count + 1) self.stuck_count += 1 else: - # self.stuck_count_pub.publish(0) self.stuck_count = 0 # check if vehicle is stuck in wait loop # for example when waiting for a traffic light seems to take too long # -> something might have went wrong if self.current_speed.speed < 1.0: - rospy.loginfo("wait_stuck_count increased by 1, now: %s", - self.wait_stuck_count + 1) - # self.wait_stuck_count_pub.publish(self.wait_stuck_count.data + 1) + # rospy.loginfo("wait_stuck_count increased by 1, now: %s", + # self.wait_stuck_count + 1) self.wait_stuck_count += 1 else: - # self.wait_stuck_count_pub.publish(0) self.wait_stuck_count = 0 # inform about which kind of stuck situation is detected if self.stuck_count >= TRIGGER_STUCK_COUNT: - rospy.logwarn("Stuck detected -> starting unstuck routine") + rospy.logfatal("Stuck detected -> starting unstuck routine") self.init_pos = pos_to_np_array( self.blackboard.get("/paf/hero/current_pos")) elif self.wait_stuck_count >= TRIGGER_WAIT_STUCK_COUNT: - rospy.logwarn("Wait stuck detected -> starting unstuck routine") + rospy.logfatal("Wait stuck detected -> starting unstuck routine") self.init_pos = pos_to_np_array( self.blackboard.get("/paf/hero/current_pos")) @@ -591,14 +571,13 @@ def update(self): if self.current_speed.speed < 1: unstuck_distance = get_distance(self.init_pos, self.current_pos) - # if distance_to_waypoint < 0.5: self.pub_unstuck_distance.publish(unstuck_distance) - rospy.logwarn("Unstuck DISTANCE %s.", unstuck_distance) - self.curr_behavior_pub.publish(bs.us_overtake.name) + # rospy.logwarn("Unstuck DISTANCE %s.", unstuck_distance) - # self.stuck_count_pub.publish(0) - # self.wait_stuck_count_pub.publish(0) + # 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.stuck_count = 0 self.wait_stuck_count = 0 diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index 3b2e2885..942f0409 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -4,7 +4,7 @@ import py_trees import py_trees_ros -from std_msgs.msg import Float32, Bool, Float32MultiArray, Int32 +from std_msgs.msg import Float32, Bool, Float32MultiArray from carla_msgs.msg import CarlaSpeedometer from geometry_msgs.msg import PoseStamped @@ -52,12 +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}/stuck_count", 'msg': Int32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/target_velocity", 'msg': Float32, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/wait_stuck_count", 'msg': Int32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} ] topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard") diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 970f0986..cd28d3c1 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -463,14 +463,17 @@ def __get_speed_unstuck(self, behavior: str) -> float: 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}") + # 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.logfatal("Unstuck Overtake Flag Cleared") # to avoid spamming the overtake_fallback if self.unstuck_overtake_flag is False: - # create overtake trajectory 6 meteres before the obstacle + # create overtake trajectory starting 6 meteres before + # the obstacle + # 6 worked well in tests, but can be adjusted self.overtake_fallback(self.unstuck_distance + 6, pose_list) self.logfatal("Overtake fallback while unstuck!") self.unstuck_overtake_flag = True From 28c56756fdffc05697d8bcecd6507e55782aa892 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Tue, 19 Mar 2024 20:40:51 +0100 Subject: [PATCH 09/13] fix: improved programming of the unstuck routine -> timer --- .../behavior_agent/behaviours/maneuvers.py | 128 ++++++++++-------- .../src/local_planner/motion_planning.py | 29 +++- 2 files changed, 93 insertions(+), 64 deletions(-) diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 96b94d92..233fb506 100755 --- a/code/planning/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -394,9 +394,10 @@ def pos_to_np_array(pos): return None -TRIGGER_STUCK_COUNT = 30 # default 30 (about 5 s -> behavior tree 5 Hz) -TRIGGER_WAIT_STUCK_COUNT = 100 # default 100 (about 20 s-> behavior tree 5 Hz) -UNSTUCK_DRIVE_DURATION = 1.2 # default 1.2 (s) +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): @@ -416,8 +417,11 @@ def __init__(self, name): :param name: name of the behaviour """ super(UnstuckRoutine, self).__init__(name) - self.stuck_count = 0 - self.wait_stuck_count = 0 + 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 @@ -425,6 +429,11 @@ def __init__(self, name): dummy_pos = np.array([0, 0]) self.last_unstuck_positions = np.array([dummy_pos, dummy_pos]) + def reset_stuck_values(self): + self.unstuck_overtake_count = 0 + self.stuck_timer = rospy.Time.now() + self.wait_stuck_timer = rospy.Time.now() + def setup(self, timeout): """ Delayed one-time initialisation that would otherwise interfere with @@ -457,43 +466,46 @@ def initialise(self): Any initialisation you need before putting your behaviour to work. :return: True """ - self.init_ros_time = rospy.Time.now() + + 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 or target_speed is None: - if self.current_speed is None: - rospy.logdebug("current_speed is None") - elif target_speed is None: - rospy.logdebug("target_speed is None") + 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 stuck, v = 0 when told to v > 0 - if self.current_speed.speed < 1 and target_speed.data >= 1: - # rospy.loginfo("stuck_count increased by 1, now: %s", - # self.stuck_count + 1) - self.stuck_count += 1 - else: - self.stuck_count = 0 - # check if vehicle is stuck in wait loop - # for example when waiting for a traffic light seems to take too long - # -> something might have went wrong - if self.current_speed.speed < 1.0: - # rospy.loginfo("wait_stuck_count increased by 1, now: %s", - # self.wait_stuck_count + 1) - self.wait_stuck_count += 1 - else: - self.wait_stuck_count = 0 - - # inform about which kind of stuck situation is detected - if self.stuck_count >= TRIGGER_STUCK_COUNT: - rospy.logfatal("Stuck detected -> starting unstuck routine") + # 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_count >= TRIGGER_WAIT_STUCK_COUNT: - rospy.logfatal("Wait stuck detected -> starting unstuck routine") + 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")) @@ -515,35 +527,36 @@ def update(self): :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.stuck_count is None: - # rospy.logwarn("stuck_count is None") + 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_count < TRIGGER_STUCK_COUNT and \ - self.wait_stuck_count < TRIGGER_WAIT_STUCK_COUNT: + if self.stuck_duration < TRIGGER_STUCK_DURATION and \ + self.wait_stuck_duration < TRIGGER_WAIT_STUCK_DURATION: + # rospy.logfatal("No stuck detected.") return py_trees.common.Status.FAILURE # stuck detected -> unstuck routine - ros_time = rospy.Time.now() - if ros_time - self.init_ros_time < \ - rospy.Duration(UNSTUCK_DRIVE_DURATION): - + if rospy.Time.now()-self.init_ros_stuck_time < UNSTUCK_DRIVE_DURATION: self.curr_behavior_pub.publish(bs.us_unstuck.name) - rospy.logwarn("Unstuck routine running.") + rospy.logfatal("Unstuck routine running.") return py_trees.common.Status.RUNNING else: # while vehicle is stopping publish us_stop - if self.current_speed.speed < -0.1: + 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: - # check if vehicle needs to overtake: + # 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) @@ -551,20 +564,23 @@ def update(self): # 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.stuck_count = 0 - self.wait_stuck_count = 0 - self.unstuck_overtake_count = 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])) + # 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]) > 1.5: - self.stuck_count = 0 - self.wait_stuck_count = 0 - self.unstuck_overtake_count = 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 @@ -579,9 +595,7 @@ def update(self): # it is detected self.curr_behavior_pub.publish(bs.us_overtake.name) if self.unstuck_overtake_count > 3: - self.stuck_count = 0 - self.wait_stuck_count = 0 - self.unstuck_overtake_count = 0 + self.reset_stuck_values() rospy.logwarn("Unstuck routine finished.") return py_trees.common.Status.FAILURE else: diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index cd28d3c1..0622c5c7 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -223,13 +223,27 @@ 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 + + # if overtake is called by the unstuck routine + # -> reset the current wp to the distance driven backwards + # distance between each waypoint is set to 1 + # -> distance is the amount of waypoints we drove + # backwards during unstuck! + if unstuck is True: + if distance > currentwp: + currentwp = 0 + else: + currentwp = currentwp - int(distance + 1) + + # else: overtake starts from current wp + selection = pose_list[int(currentwp):int(currentwp) + int(distance) + 7] waypoints = self.convert_pose_to_array(selection) @@ -261,8 +275,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): @@ -467,15 +481,16 @@ def __get_speed_unstuck(self, behavior: str) -> float: # clear distance to last unstuck -> avoid spamming overtake if distance > UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE: self.unstuck_overtake_flag = False - self.logfatal("Unstuck Overtake Flag Cleared") + 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 + 6, pose_list) - self.logfatal("Overtake fallback while unstuck!") + 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 From 4ef7e7686790ebc7628eb2d531b15f7b5afd4656 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Wed, 20 Mar 2024 16:12:20 +0100 Subject: [PATCH 10/13] fix: cleaned up unstuck Routine --- code/planning/src/behavior_agent/behaviours/maneuvers.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 233fb506..098ca5a0 100755 --- a/code/planning/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -407,6 +407,10 @@ class UnstuckRoutine(py_trees.behaviour.Behaviour): 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): """ @@ -429,11 +433,6 @@ def __init__(self, name): dummy_pos = np.array([0, 0]) self.last_unstuck_positions = np.array([dummy_pos, dummy_pos]) - def reset_stuck_values(self): - self.unstuck_overtake_count = 0 - self.stuck_timer = rospy.Time.now() - self.wait_stuck_timer = rospy.Time.now() - def setup(self, timeout): """ Delayed one-time initialisation that would otherwise interfere with From 6d0bcc74677480d2484ddb70b014db624c12ac8b Mon Sep 17 00:00:00 2001 From: robertik10 Date: Wed, 20 Mar 2024 17:10:23 +0100 Subject: [PATCH 11/13] fix: changed the x_offset when overtaking for easier overtake --- code/planning/src/local_planner/motion_planning.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 0622c5c7..b992ab02 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -230,6 +230,8 @@ def overtake_fallback(self, distance, pose_list, unstuck=False): # self.current_pos, # self.current_speed) currentwp = self.current_wp + normal_x_offset = 2 + unstuck_x_offset = 4 # could need adjustment with better steering # if overtake is called by the unstuck routine # -> reset the current wp to the distance driven backwards @@ -247,7 +249,10 @@ def overtake_fallback(self, distance, pose_list, unstuck=False): 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) From c889ff28fd55b1063afc1393c3c4f325178a0ea3 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Wed, 20 Mar 2024 18:58:38 +0100 Subject: [PATCH 12/13] fix: wp generation for unstuck routine --- .../behavior_agent/behaviours/maneuvers.py | 16 ++++--- .../behaviours/topics2blackboard.py | 2 +- code/planning/src/local_planner/ACC.py | 45 ++++++++++++++++++- .../src/local_planner/motion_planning.py | 5 ++- 4 files changed, 59 insertions(+), 9 deletions(-) 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) + From 53f604828151237a33f3bf6215ee7015afe08b18 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Wed, 20 Mar 2024 19:07:36 +0100 Subject: [PATCH 13/13] fix: removed wp reset from motion_planning --- code/planning/src/local_planner/motion_planning.py | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 591d73d0..b54468b1 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -232,20 +232,6 @@ def overtake_fallback(self, distance, pose_list, unstuck=False): currentwp = self.current_wp normal_x_offset = 2 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 - # distance between each waypoint is set to 1 - # -> distance is the amount of waypoints we drove - # backwards during unstuck! - if unstuck is True: - if distance > currentwp: - currentwp = 0 - else: - distance = distance + 2 - # currentwp = currentwp - int(distance) - - # else: overtake starts from current wp selection = pose_list[int(currentwp):int(currentwp) + int(distance) + 7] waypoints = self.convert_pose_to_array(selection)