143 lines
4.8 KiB
Python
Executable File
143 lines
4.8 KiB
Python
Executable File
#!/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.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.get_logger().warn(f"⏭ WP {self.idx+1} plan failed — skip")
|
||
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
|
||
|
||
|
||
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()
|