MPPI + fence-line loc + waypoint nav with fail skip

scripts/waypoint_nav.py

@@ -0,0 +1,148 @@
+#!/usr/bin/env python3
+"""Sequential waypoint navigation for car_nav_lite.
+
+Publishes /goal_pose waypoints one by one. Waits for the car to reach
+each waypoint before publishing the next.
+
+Usage:
+  python3 waypoint_nav.py                           # default waypoints
+  python3 waypoint_nav.py --waypoints waypoints.json  # from JSON file
+  python3 waypoint_nav.py --loop                      # loop back to start
+
+JSON format:
+  [{"x": 1.0, "y": -1.5, "yaw": 0.0}, ...]
+"""
+
+import argparse
+import json
+import math
+import time
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Odometry
+from std_msgs.msg import String
+
+
+# Default waypoints for 5m×5m arena (car spawns near -2.0, -2.3)
+DEFAULT_WAYPOINTS = [
+    {"x": -0.5, "y": -1.5, "yaw": 0.0, "desc": "mid-left"},
+    {"x":  1.0, "y": -1.5, "yaw": 0.0, "desc": "right"},
+    {"x":  1.5, "y":  0.0, "yaw": 0.0, "desc": "top-right"},
+    {"x":  0.0, "y":  1.5, "yaw": 0.0, "desc": "top-center"},
+    {"x": -1.5, "y":  1.0, "yaw": 0.0, "desc": "top-left"},
+    {"x": -1.5, "y": -0.5, "yaw": 0.0, "desc": "left"},
+]
+
+
+class WaypointNav(Node):
+    def __init__(self, waypoints, loop=False, threshold=0.25):
+        super().__init__("waypoint_nav")
+        self.waypoints = waypoints
+        self.loop = loop
+        self.threshold = threshold
+        self.idx = 0
+        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
+        qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE,
+                         durability=DurabilityPolicy.TRANSIENT_LOCAL)
+        self.pub_goal = self.create_publisher(PoseStamped, "/goal_pose", qos)
+        self.sub_odom = self.create_subscription(
+            Odometry, "/odom", self.odom_cb, 10)
+        self.sub_status = self.create_subscription(
+            String, "/nav_status", self.status_cb, 10)
+        self.timer = self.create_timer(0.2, self.check)  # 5Hz
+
+    def odom_cb(self, msg):
+        self.pose_x = msg.pose.pose.position.x
+        self.pose_y = msg.pose.pose.position.y
+        q = msg.pose.pose.orientation
+        self.pose_yaw = math.atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z))
+
+    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.skip_gen = self.fail_gen
+            self.idx += 1
+            self.reached = False
+
+    def send_goal(self, wp):
+        msg = PoseStamped()
+        msg.header.frame_id = "map"
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.pose.position.x = float(wp["x"])
+        msg.pose.position.y = float(wp["y"])
+        yaw_deg = float(wp.get("yaw", 0.0))
+        yaw = math.radians(yaw_deg)  # JSON stores degrees
+        msg.pose.orientation.z = math.sin(yaw / 2.0)
+        msg.pose.orientation.w = math.cos(yaw / 2.0)
+        self.pub_goal.publish(msg)
+        desc = wp.get("desc", "")
+        self.get_logger().info(
+            f"→ WP {self.idx+1}/{len(self.waypoints)}: "
+            f"({wp['x']:.2f},{wp['y']:.2f}) {desc}"
+        )
+
+    def check(self):
+        if self.idx >= len(self.waypoints):
+            if self.loop:
+                self.idx = 0
+                self.reached = False
+            else:
+                return
+
+        wp = self.waypoints[self.idx]
+        if not self.reached:
+            self.fail_gen += 1  # new generation for this wp
+            self.send_goal(wp)
+            self.reached = True
+
+        dist = math.hypot(self.pose_x - wp["x"], self.pose_y - wp["y"])
+        if dist < self.threshold:
+            self.get_logger().info(
+                f"✓ WP {self.idx+1} reached (dist={dist:.2f}m)")
+            self.idx += 1
+            self.reached = False
+            self.skip_streak = 0
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--waypoints", type=str, default=None,
+                        help="JSON file with waypoints")
+    parser.add_argument("--loop", action="store_true",
+                        help="Loop back to first waypoint after last")
+    parser.add_argument("--threshold", type=float, default=0.3,
+                        help="Arrival distance threshold (m)")
+    args = parser.parse_args()
+
+    if args.waypoints:
+        with open(args.waypoints) as f:
+            waypoints = json.load(f)
+        print(f"Loaded {len(waypoints)} waypoints from {args.waypoints}")
+    else:
+        waypoints = DEFAULT_WAYPOINTS
+        print(f"Using {len(waypoints)} default waypoints")
+
+    rclpy.init()
+    node = WaypointNav(waypoints, args.loop, args.threshold)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()