forked from zbw/yiliao2026
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"
This commit is contained in:
58
tools/udp_to_cmdvel.py
Executable file
58
tools/udp_to_cmdvel.py
Executable file
@@ -0,0 +1,58 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
UDP → /cmd_vel bridge for keyboard control.
|
||||
Receives (float64 lin_x, float64 ang_z) from Windows client.
|
||||
|
||||
Deploy on RDKx5: /home/sunrise/yiliao_ws/udp_to_cmdvel.py
|
||||
Run: python3 udp_to_cmdvel.py
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import Twist
|
||||
import socket
|
||||
import struct
|
||||
import sys
|
||||
|
||||
|
||||
class UDPToCmdVel(Node):
|
||||
def __init__(self, port=9999):
|
||||
super().__init__("udp_to_cmdvel")
|
||||
self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self.sock.bind(("0.0.0.0", port))
|
||||
self.sock.settimeout(0.1)
|
||||
self.get_logger().info("Listening on UDP port %d → /cmd_vel" % port)
|
||||
self.create_timer(0.05, self.loop)
|
||||
|
||||
def loop(self):
|
||||
try:
|
||||
data, addr = self.sock.recvfrom(16)
|
||||
lin, ang = struct.unpack('dd', data)
|
||||
msg = Twist()
|
||||
msg.linear.x = lin
|
||||
msg.angular.z = ang
|
||||
self.pub.publish(msg)
|
||||
except socket.timeout:
|
||||
pass
|
||||
|
||||
def destroy_node(self):
|
||||
self.sock.close()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
port = int(sys.argv[1]) if len(sys.argv) > 1 else 9999
|
||||
node = UDPToCmdVel(port=port)
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user