fix: obstacle speed > 0 -> Abort Overtake

una-auxme · Jan 29, 2024 · fafa196 · fafa196
1 parent fbb42c4
commit fafa196
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py
@@ -221,6 +221,10 @@ def update(self):
         else:
             distance_lidar = None
 
+        obstacle_msg = self.blackboard.get("/paf/hero/collision")
+        if obstacle_msg is None:
+            return py_trees.common.Status.FAILURE
+
         if distance_lidar is not None:
             collision_distance = distance_lidar.data
             if collision_distance > clear_distance:
@@ -230,6 +234,8 @@ def update(self):
                 rospy.loginfo("Overtake still blocked")
                 self.curr_behavior_pub.publish(bs.ot_wait_stopped.name)
                 return py_trees.commom.Status.RUNNING
+        elif obstacle_msg[1] > convert_to_ms(2):
+            return py_trees.common.Status.FAILURE
         else:
             rospy.loginfo("No Lidar Distance")
             return py_trees.common.Success