a

install/gc_bringup/lib/gc_bringup/nav_goal.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+"""
+命令行发送导航目标点到 Nav2
+
+用法:
+    ros2 run gc_bringup nav_goal 1.0 2.0 0.5        # 去 (1.0, 2.0), yaw=0.5
+    ros2 run gc_bringup nav_goal 1.0 2.0             # 去 (1.0, 2.0), yaw=0.0
+    ros2 run gc_bringup nav_goal -2.0 0.0 3.14       # 去 (-2.0, 0.0), 面朝后
+
+    # 或使用 topic pub
+    ros2 topic pub /goal_pose geometry_msgs/PoseStamped "{header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 0.0}, orientation: {z: 0.0, w: 1.0}}}"
+"""
+
+import sys
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped
+import math
+
+
+class GoalSender(Node):
+    def __init__(self):
+        super().__init__('goal_sender')
+        self.publisher = self.create_publisher(PoseStamped, '/goal_pose', 10)
+
+    def send_goal(self, x: float, y: float, yaw: float = 0.0):
+        msg = PoseStamped()
+        msg.header.frame_id = 'map'
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.pose.position.x = x
+        msg.pose.position.y = y
+        msg.pose.position.z = 0.0
+        msg.pose.orientation.z = math.sin(yaw / 2.0)
+        msg.pose.orientation.w = math.cos(yaw / 2.0)
+        self.publisher.publish(msg)
+        self.get_logger().info(f'发送导航目标: x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}')
+
+
+def main():
+    rclpy.init()
+    sender = GoalSender()
+
+    args = sys.argv[1:]
+    if len(args) < 2:
+        print("用法: ros2 run gc_bringup nav_goal <x> <y> [yaw]")
+        print("示例: ros2 run gc_bringup nav_goal 1.0 2.0 0.5")
+        sys.exit(1)
+
+    x = float(args[0])
+    y = float(args[1])
+    yaw = float(args[2]) if len(args) > 2 else 0.0
+
+    sender.send_goal(x, y, yaw)
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()