5156a8d766
- 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)
88 lines
2.7 KiB
Python
Executable File
88 lines
2.7 KiB
Python
Executable File
#!/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()
|