香橙派与松灵机器人SCOUT2.0底盘CAN通信控制测试

参考资料

用户手册 https://new.agilex.ai/raw/upload/20230718/SCOUT 2.0用户手册20230718_74677.pdf
如何通过开源SDK控制松灵机器人SCOUT底盘？ https://blog.csdn.net/AgileX/article/details/106730024
SCOUT底盘测试 通过CAN总线和RS232控制松灵地盘 https://blog.csdn.net/qsl56789/article/details/135264569

测试环境

硬件与系统

香橙派5max
ubuntu 22.04
python 3.10

环境安装

安装CAN 工具（提供 cansend/candump）

sudo apt update
sudo apt install -y can-utils

硬件连接使用官方配备的usb转can线，使用以下命令发现并检测是否连接usb-can adapter（常见 can0）

ip link | grep can

测试代码

import os
import sys
import time
from threading import Thread, Lock

from pynput import keyboard  # 用于按键按下/松开监听


class Scout_base:
    Light_Off = "00"
    Light_On = "01"
    Light_Flash = "02"

    def __init__(self, canName, bitrate=500000):
        self.__canName = canName
        self.__bitrate = str(bitrate)
        self.__light_count = 0
        self.__v_linear = 0.0
        self.__v_angle = 0.0
        self.__lock = Lock()

        os.system(f"sudo ip link set {self.__canName} down 2>/dev/null || true")

        os.system("sudo rmmod gs_usb 2>/dev/null || true")
        os.system("sudo modprobe can && sudo modprobe can_raw && sudo modprobe gs_usb 2>/dev/null")
        os.system(
            f"sudo ip link set {self.__canName} up type can bitrate {self.__bitrate} "
            f"loopback off restart-ms 0 2>/dev/null"
        )
        os.system(f"sudo ip link set {self.__canName} txqueuelen 2000 2>/dev/null")
        os.system(f"sudo ip link set {self.__canName} type can presume-ack on one-shot on 2>/dev/null")
        os.system(
            "sudo sysctl -w net.core.wmem_max=2097152 >/dev/null && "
            "sudo sysctl -w net.core.wmem_default=1048576 >/dev/null"
        )
        time.sleep(0.5)

        try:
            self.setMode()
            time.sleep(1)
            self.scoutProcess()
            print(f"端口{self.__canName} 初始化成功")
        except Exception as e:
            print(f"CAN总线初始化失败：{e}")
            self.stopCar()
            os.system(f"sudo ip link set {self.__canName} down")
            sys.exit(1)

    def setMode(self):
        os.system(f"cansend {self.__canName} 421#0100000000000000 2>/dev/null")

    def setCarMotion(self, v_linear, v_angle):
        motionCmd = f"cansend {self.__canName} 111#"
        linear_val = int(v_linear * 1000.0) & 0xFFFF
        speed = f"{linear_val:04x}"
        angle_val = int(v_angle * 1000.0) & 0xFFFF
        gyr = f"{angle_val:04x}"
        motionCmd = motionCmd + speed + gyr + "00000000"
        os.system(motionCmd + " 2>/dev/null")

        os.system(f"echo 1 | sudo tee /sys/class/net/{self.__canName}/queues/tx-0/flush > /dev/null 2>&1")

    def stopCar(self):
        self.setVelocity(0.0, 0.0)

    def setVelocity(self, v_linear, v_angle):
        self.__lock.acquire()
        self.__v_linear = v_linear
        self.__v_angle = v_angle
        self.__lock.release()

    def task_runCmd(self):
        while True:
            self.__lock.acquire()
            v_linear = self.__v_linear
            v_angle = self.__v_angle
            self.__lock.release()

            self.setCarMotion(v_linear, v_angle)
            time.sleep(0.1)

    def scoutProcess(self):
        thread_runCmd = Thread(target=self.task_runCmd, name="MotionSendThread")
        thread_runCmd.daemon = True
        thread_runCmd.start()


if __name__ == "__main__":
    try:
        car = Scout_base("can0", bitrate=500000)

        LIN_SPEED = 0.1
        ANG_SPEED = 0.1

        pressed = set()

        def update_velocity():
            if ("w" in pressed) and ("s" not in pressed):
                v_linear = LIN_SPEED
            elif ("s" in pressed) and ("w" not in pressed):
                v_linear = -LIN_SPEED
            else:
                v_linear = 0.0

            if ("a" in pressed) and ("d" not in pressed):
                v_angle = ANG_SPEED
            elif ("d" in pressed) and ("a" not in pressed):
                v_angle = -ANG_SPEED
            else:
                v_angle = 0.0

            car.setVelocity(v_linear, v_angle)

        def on_press(key):
            try:
                k = key.char.lower()
            except Exception:
                return

            if k == "q":
                car.stopCar()
                os.system(f"sudo ifconfig {car._Scout_base__canName} down")
                print("\n停止小车并关闭CAN总线，程序退出")
                return False

            if k in ("w", "a", "s", "d"):
                pressed.add(k)
                update_velocity()

        def on_release(key):
            try:
                k = key.char.lower()
            except Exception:
                return

            if k in pressed:
                pressed.remove(k)
                update_velocity()

        print("W+A=前进左转 | W+D=前进右转 | S+A=后退左转 | S+D=后退右转 | 松开按键即停止 | 按 Q 退出 ")
        with keyboard.Listener(on_press=on_press, on_release=on_release) as listener:
            listener.join()
    except KeyboardInterrupt:
        car.stopCar()
        os.system(f"sudo ifconfig {car._Scout_base__canName} down")
        print("\n程序被手动中断，已停止小车并关闭CAN总线")
    except Exception as e:
        print(f"程序运行出错：{e}")
        if 'car' in locals():
            car.stopCar()
            os.system(f"sudo ifconfig {car._Scout_base__canName} down")