Please enter the commit message for your changes. Lines starting

with '#' will be ignored, and an empty message aborts the commit. On branch master Your branch is ahead of 'origin/master' by 2 commits. (use "git push" to publish your local commits) Changes to be committed: modified: .gitignore modified: README.md new file: bashes/auto-wifi-connect.service new file: bashes/auto-wifi-connect.sh deleted: keyboard_control.py new file: my_model/image.png new file: path_follower_demo.py new file: scripts/PIDtracking.py new file: scripts/__pycache__/publish_sine_path.cpython-310.pyc new file: scripts/publish_sine_path.py modified: src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml modified: src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak new file: src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2 modified: src/origincar_base/config/ekf.yaml new file: src/origincar_base/config/ekf.yaml.bak modified: src/origincar_base/launch/base_serial.launch.py new file: src/origincar_base/launch/base_serial.launch.py.bak modified: src/origincar_base/launch/origincar_bringup.launch.py new file: src/past_control/CMakeLists.txt new file: src/past_control/config/past_control.yaml new file: src/past_control/include/past_control/tools.h new file: src/past_control/launch/past_control.launch.py new file: src/past_control/msg/Obstacle.msg new file: src/past_control/msg/ObstacleArray.msg new file: src/past_control/package.xml new file: src/past_control/src/lane_follower_node.cpp new file: src/past_control/src/obstacle_detector_node.cpp new file: src/past_control/src/racing_orchestrator.cpp new file: src/planner/CMakeLists.txt new file: src/planner/config/planner.yaml new file: src/planner/launch/planner.launch.py new file: src/planner/package.xml new file: src/planner/src/planner_version.cpp modified: src/qr_detection/src/qr_dete_depth.cpp new file: src/racing_control/CMakeLists.txt new file: src/racing_control/include/racing_control/racing_control.hpp new file: src/racing_control/package.xml new file: src/racing_control/src/racing_control.cpp modified: src/vlm_detect/setup.py new file: src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc new file: src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc new file: src/vlm_detect/vlm_detect/test_publisher.py new file: src/vlm_detect/vlm_detect/tts_node.py modified: src/vlm_detect/vlm_detect/vlm_node.py new file: tools/measure_turning_radius.py new file: tools/set_volume.py new file: tools/udp_to_cmdvel.py new file: tools/windows_keyboard_control.py new file: vlm_server.py new file: "\350\260\203\350\257\225\350\256\260\345\275\225.Assets/1.png" renamed: "\350\260\203\350\257\225\350\256\260\345\275\225.log" -> "\350\260\203\350\257\225\350\256\260\345\275\225.md"
2026-06-22 17:16:45 +08:00
parent a9bfeef59b
commit 779b32362a
51 changed files with 3974 additions and 398 deletions
--- a/scripts/publish_sine_path.py
+++ b/scripts/publish_sine_path.py
@@ -0,0 +1,123 @@
+#!/usr/bin/env python3
+"""
+publish_sine_path.py — 基于机器人当前位姿发布一次正弦路径到 /plan
+
+用法:
+  python3 publish_sine_path.py [amplitude] [wavelength] [length]
+  默认: amplitude=0.8m  wavelength=3.0m  length=8.0m
+
+路径在机器人前方展开:
+  local_x 沿机器人朝向正前方
+  local_y = A * sin(2pi * local_x / wavelength)
+  再旋转变换到 odom 坐标系
+
+依赖: /odom 话题
+"""
+
+import sys
+import math
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Path, Odometry
+from geometry_msgs.msg import PoseStamped
+
+
+class SinePathPublisher(Node):
+    def __init__(self, amplitude, wavelength, length):
+        super().__init__("sine_path_publisher")
+
+        self.A = amplitude
+        self.wavelength = wavelength
+        self.length = length
+        self.step = 0.05
+
+        self.robot_x = 0.0
+        self.robot_y = 0.0
+        self.robot_yaw = 0.0
+        self.has_odom = False
+
+        self.pub = self.create_publisher(Path, "/plan", 10)
+        self.odom_sub = self.create_subscription(
+            Odometry, "/odom", self.odom_cb, 10)
+
+    def odom_cb(self, msg):
+        self.robot_x = msg.pose.pose.position.x
+        self.robot_y = msg.pose.pose.position.y
+        qx = msg.pose.pose.orientation.x
+        qy = msg.pose.pose.orientation.y
+        qz = msg.pose.pose.orientation.z
+        qw = msg.pose.pose.orientation.w
+        self.robot_yaw = math.atan2(
+            2.0 * (qw * qz + qx * qy),
+            1.0 - 2.0 * (qy * qy + qz * qz))
+        self.has_odom = True
+
+    def build_and_publish(self):
+        cos_yaw = math.cos(self.robot_yaw)
+        sin_yaw = math.sin(self.robot_yaw)
+
+        path = Path()
+        path.header.frame_id = "odom"
+        path.header.stamp = self.get_clock().now().to_msg()
+
+        lx = 0.0
+        while lx <= self.length:
+            ly = self.A * math.sin(2.0 * math.pi * lx / self.wavelength)
+
+            dy_dx = self.A * (2.0 * math.pi / self.wavelength) * \
+                math.cos(2.0 * math.pi * lx / self.wavelength)
+            local_yaw = math.atan2(dy_dx, 1.0)
+
+            wx = self.robot_x + lx * cos_yaw - ly * sin_yaw
+            wy = self.robot_y + lx * sin_yaw + ly * cos_yaw
+            world_yaw = self.robot_yaw + local_yaw
+
+            pose = PoseStamped()
+            pose.header.frame_id = "odom"
+            pose.pose.position.x = wx
+            pose.pose.position.y = wy
+            pose.pose.position.z = 0.0
+            pose.pose.orientation.z = math.sin(world_yaw / 2.0)
+            pose.pose.orientation.w = math.cos(world_yaw / 2.0)
+            path.poses.append(pose)
+
+            lx += self.step
+
+        self.pub.publish(path)
+        self.get_logger().info(
+            f"Published sine path: {len(path.poses)} waypoints "
+            f"from ({self.robot_x:.2f}, {self.robot_y:.2f}, {math.degrees(self.robot_yaw):.0f}°)"
+        )
+
+
+def main():
+    rclpy.init()
+
+    A = float(sys.argv[1]) if len(sys.argv) > 1 else 0.8
+    wavelength = float(sys.argv[2]) if len(sys.argv) > 2 else 3.0
+    length = float(sys.argv[3]) if len(sys.argv) > 3 else 8.0
+
+    node = SinePathPublisher(A, wavelength, length)
+
+    # 等待 odom (最多 3 秒)
+    timeout = node.get_clock().now() + rclpy.duration.Duration(seconds=3)
+    while not node.has_odom and node.get_clock().now() < timeout:
+        rclpy.spin_once(node, timeout_sec=0.05)
+
+    if not node.has_odom:
+        node.get_logger().error("No /odom received within 3s — aborting")
+        node.destroy_node()
+        rclpy.shutdown()
+        sys.exit(1)
+
+    # 额外 spin 一下让其他订阅就绪
+    rclpy.spin_once(node, timeout_sec=0.5)
+
+    node.build_and_publish()
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()