cyy/car_nav_lite
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
cyy
2026-07-03 06:14:36 +00:00
parent 50ff61a16b
commit 85d6984bba
7 changed files with 62 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
scripts/waypoint_nav.py
View File

@@ -45,7 +45,6 @@ class WaypointNav(Node):
self.reached = False
self.fail_gen = 0
self.skip_gen = -1
self.skip_streak = 0 # consecutive skips
self.pose_x = self.pose_y = self.pose_yaw = 0.0
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
@@ -66,11 +65,7 @@ class WaypointNav(Node):
def status_cb(self, msg):
if msg.data == "fail" and self.reached and self.fail_gen != self.skip_gen:
self.skip_streak += 1
if self.skip_streak > 3:
self.get_logger().error("⏭ 3+ consecutive skips — stopping")
return
self.get_logger().warn(f"⏭ WP {self.idx+1} plan failed — skip #{self.skip_streak}")
self.get_logger().warn(f"⏭ WP {self.idx+1} plan failed — skip")
self.skip_gen = self.fail_gen
self.idx += 1
self.reached = False
@@ -112,7 +107,6 @@ class WaypointNav(Node):
f"✓ WP {self.idx+1} reached (dist={dist:.2f}m)")
self.idx += 1
self.reached = False
self.skip_streak = 0
def main():