MPPI planner + fence-line localization + scan obstacles

- Lightweight MPPI (200 trajectories, 15 steps, endpoint path_align)
- adaptive exploration (more random when stuck)
- fence-line Hough detection + geometric correction
- scan hit points marked at 255 immediately
- forward half-step pre-check for obstacle avoidance
- Ackermann constraint |wz| <= |vx|/min_turn_r
- XORshift RNG (stdlib broken on ARM)

scripts/test_goal.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+"""Send a goal pose to car_nav_lite via /goal_pose topic.
+
+Usage:
+  python3 test_goal.py                           # default goal
+  python3 test_goal.py --x 1.0 --y -1.5          # custom goal
+  python3 test_goal.py --auto                    # auto-cycle multiple goals
+"""
+
+import time
+import argparse
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped
+
+
+GOALS = [
+    # (x, y) — world coords in map frame (origin at center, 5m×5m)
+    # Car spawns near (-2.0, -2.3) facing +x
+
+    # Goal A: straight ahead ~3m, slight right
+    # Should trigger multi-pass inflation (cone at -1.58,-2.31 blocks straight path)
+    (0.98, -1.49),
+
+    # Goal B: far corner
+    (2.0, 2.0),
+
+    # Goal C: left side
+    (-1.5, 0.5),
+]
+
+
+class GoalSender(Node):
+    def __init__(self):
+        super().__init__("goal_sender")
+        self.pub = self.create_publisher(PoseStamped, "/goal_pose", 10)
+        self.idx = 0
+
+    def send(self, x, y, yaw=0.0):
+        msg = PoseStamped()
+        msg.header.frame_id = "map"
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.pose.position.x = float(x)
+        msg.pose.position.y = float(y)
+        msg.pose.position.z = 0.0
+        # yaw encoded as quaternion (scalar-last: qx,qy,qz,qw)
+        msg.pose.orientation.z = float(__import__('math').sin(yaw / 2.0))
+        msg.pose.orientation.w = float(__import__('math').cos(yaw / 2.0))
+        self.pub.publish(msg)
+        self.get_logger().info(f"Sent goal: ({x:.2f}, {y:.2f}, yaw={yaw:.2f})")
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--x", type=float, default=None)
+    parser.add_argument("--y", type=float, default=None)
+    parser.add_argument("--yaw", type=float, default=0.0)
+    parser.add_argument("--auto", action="store_true",
+                        help="Auto-cycle through test goals")
+    parser.add_argument("--interval", type=float, default=30.0,
+                        help="Seconds between auto goals")
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = GoalSender()
+
+    if args.auto:
+        node.get_logger().info(f"Auto mode: {len(GOALS)} goals, {args.interval}s interval")
+        while rclpy.ok():
+            x, y = GOALS[node.idx % len(GOALS)]
+            node.send(x, y, 0.0)
+            node.idx += 1
+            time.sleep(args.interval)
+    elif args.x is not None and args.y is not None:
+        node.send(args.x, args.y, args.yaw)
+    else:
+        # Default: use first goal
+        x, y = GOALS[0]
+        node.get_logger().info(f"No args — sending default goal ({x:.2f}, {y:.2f})")
+        node.send(x, y, 0.0)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()