语音控制车辆运动功能集成（详细教程）-CSDN博客

本文链接：https://blog.csdn.net/suoxd123/article/details/148147034

1. 代码

import numpy as np
import json
import time
import zmq
from src.iot.thing import Thing, Parameter, ValueType

class Lekiwi(Thing):
    def __init__(self):
        super().__init__("Lekiwi", "lekiwi移动机器人")

        self.current_car_status = "静止"
        self.current_arm_status= "静止中"
        self.is_moving_status = False

        Lekiwi_ip: str = "192.168.18.140"
        lekiwi_port: int = 5555 
        video_port: int = 5556
        connection_string = f"tcp://{Lekiwi_ip}:{lekiwi_port}"
        video_connection = f"tcp://{Lekiwi_ip}:{video_port}"
        context = zmq.Context()

        self.cmd_socket = context.socket(zmq.PUSH)
        self.cmd_socket.connect(connection_string)
        self.video_socket = context.socket(zmq.PULL)
        self.video_socket.connect(video_connection) 
        time.sleep(0.5)

        self._register_properties()
        self._register_methods()

        print(f"移动机器人设备初始化完成")



    def _register_properties(self):
        """注册属性"""
        self.add_property("current_car_status", "当前车辆状态", lambda: self.current_car_status)
        self.add_property("current_arm_status", "当前机械臂状态", lambda: self.current_arm_status)
        self.add_property("is_moving_status", "是否静止", lambda: self.is_moving_status)
    def _register_methods(self):
        """注册方法"""
        self.add_method(
            "moveForward",
            "向前移动",
            [],
            lambda params: self._move(0)
        )
        self.add_method(
            "moveBackward",
            "向后移动",
            [],
            lambda params: self._move(180)
        )
        self.add_method(
            "moveLeft",
            "向左移动",
            [],
            lambda params: self._move(270)
        )
        self.add_method(
            "moveRight",
            "向右移动",
            [],
            lambda params: self._move(90)
        )
        
        self.add_method(
            "turnClockwise",
            "原地正转",
            [],
            lambda params: self._turn(True)
        )
        
        self.add_method(
            "turnAnticlockwise",
            "原地反转",
            [],
            lambda params: self._turn(False)
        )

        self.add_method(
            "move",
            "向定方向移动(0-360)",
            [Parameter("direction", "向指定方向平移", ValueType.NUMBER, True)],
            lambda params: self._move(params["direction"].get_value())
        )

    def _move(self, direction: int):
        print(f"车辆准备向{direction}方向运动")
        for i in range(20):
            self._start_working(direction,  True)
            time.sleep(0.1)
        self.stop()
        return {"status": "success", "message": "移动完成"}

    def _turn(self, direction: bool):
        if direction:
            str_direction = "顺时针"
        else:
            str_direction = "逆时针"    
        print(f"车辆准备向{str_direction}原地旋转")
        for i in range(20):
            self._start_working(direction, False)
            time.sleep(0.1)
        self.stop()
        return {"status": "success", "message": "原地旋转完成"}

    def _start_working(self, direction: int, moveflag: bool):
        y_cmd = 0.0  # m/s 前后方向
        x_cmd = 0.0  # m/s 左右方向
        theta_cmd = 0.0  # deg/s 旋转
        xy_speed = 0.2  # m/s 移动速度
        theta_speed = 60.0 # deg/s 旋转速度
        
        self.is_moving_status = True
        if moveflag:            
            self.current_car_status = "正在移动"
            x_cmd = xy_speed*np.sin(np.radians(direction))
            y_cmd = xy_speed*np.cos(np.radians(direction))
        else:
            self.current_car_status = "正在旋转"
            if direction :
                theta_cmd -= theta_speed
            else:
                theta_cmd += theta_speed

        wheel_commands = self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd)
        message = {"raw_velocity": wheel_commands}
        print(f"车辆命令: {message}")
        self.cmd_socket.send_string(json.dumps(message))
        self.video_socket.recv_string()

    def stop(self):
        self.is_moving_status = False
        self.current_car_status = "静止"
        wheel_commands = self.body_to_wheel_raw(0, 0, 0)
        self.cmd_socket.send_string(json.dumps({"raw_velocity": wheel_commands}))
        print("车辆停止")
        self.video_socket.recv_string()

    def body_to_wheel_raw(self, x_cmd: float, y_cmd: float, theta_cmd: float) -> dict:               
        wheel_radius: float = 0.05  # meters，轮子半径
        base_radius: float = 0.125  # meters，轮子到车辆中心的距离
        max_raw: int = 3000 # 轮子最大命令
        
        theta_rad = theta_cmd * (np.pi / 180.0)
        velocity_vector = np.array([x_cmd, y_cmd, theta_rad])
        #       Y
        #       ^
        #       |
        #X <----0
        angles = np.radians(np.array([300, 180, 60]))
        m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
        wheel_linear_speeds = m.dot(velocity_vector)
        wheel_angular_speeds = wheel_linear_speeds / wheel_radius
        wheel_deg = wheel_angular_speeds * (180.0 / np.pi)

        steps_per_deg = 4096.0 / 360.0
        raw_floats = [abs(degps) * steps_per_deg for degps in wheel_deg]
        max_raw_computed = max(raw_floats)
        if max_raw_computed > max_raw:
            scale = max_raw / max_raw_computed
            wheel_deg = wheel_deg * scale
        wheel_raw = [self.deg_to_raw(deg) for deg in wheel_deg]

        return {"left_wheel": wheel_raw[0], "back_wheel": wheel_raw[1], "right_wheel": wheel_raw[2]}
    
    def deg_to_raw(self, deg: float) -> int:
        steps_per_deg = 4096.0 / 360.0
        speed_in_steps = abs(deg) * steps_per_deg
        speed_int = int(round(speed_in_steps))
        if speed_int > 0x7FFF:
            speed_int = 0x7FFF
        if deg < 0:
            return speed_int | 0x8000
        else:
            return speed_int & 0x7FFF