ros2键盘实现车辆: 简单的油门_刹车_挡位_前后左右移动控制

参考:

1. ROS python 实现键盘控制 底盘移动 https://blog.csdn.net/u011326325/article/details/131609340
2. 游戏手柄控制

1.背景与需求

1.之前实现过 键盘控制 底盘移动的程序, 底盘是线速度控制, 效果还不错.
2.新的底盘 只支持油门控制, 使用线速度控制问题比较多, 和底盘适配不好;

1. 机械开关, 不能频繁切换挡位
2. 停止移动, 需要主动踩刹车, 不然由于车辆惯性会继续向前移动

2.功能需求

键位功能如下

特性介绍

1. 按一下a, 转向-15度; 按一下d, 转向+15度
2. 长按space , 刹车0.0->70.0 %, 每0.1s加10.0%刹车
3. 按c 转向为0
4. 按意挡位切换,油门为0
5. q 退出

3.代码实现

在线代码:
话题定义: https://gitee.com/zero2200/7_ros-robot-example/blob/main/ros2/src/common/msg/CarControl.msg

键盘控制: https://gitee.com/zero2200/7_ros-robot-example/blob/main/ros2/src/12_car_control/12_car_control/keyborad_sim_ctrol_car.py

定义话题
CarControl.msg

float32 acc     # 油门
float32 angle   # 方向盘
float32 brake   # 刹车
uint8 gear      # 档位

键盘控制代码

#!/usr/bin/env python3
# coding:utf-8
""" 
功能:
    实现输入w,s,a,d命令控制车辆:油门,刹车,左转,右转,挡位切换
"""

import sys
import tty
import termios
import select
from enum import IntEnum
import threading

# ROS
import rclpy
from rclpy.node import Node

# Local
from common.msg import CarControl


# import debugpy
# debugpy.listen(6688)
# debugpy.wait_for_client()
# debugpy.breakpoint()


class Gear(IntEnum):
    P = 1  # P档
    R = 2  # 倒档
    N = 3  # 空档
    D = 4  # 前进档


def Ang2Rad(angle) -> float:
    return angle * 0.01745


def Rad2Ang(rad) -> float:
    return rad * 57.2958


Angle_15 = Ang2Rad(15)
Angle_45 = Ang2Rad(45)
Compensation_Rad = 4.0


class Noblock_terminal:
    def __init__(self):
        fd = sys.stdin.fileno()
        self.old_settings = termios.tcgetattr(fd)
        tty.setraw(sys.stdin.fileno(), termios.TCSANOW)

    def __exit__(self):
        if self.old_settings:
            self.stop_no_block()

    def get_char(self):
        ch = sys.stdin.read(1)
        # sys.stdout.write(ch)
        return ch

    def select_cmd(self, timeout=0.2):
        read_list = [sys.stdin]
        cmd = "0"
        read_ret, write_ret, err_ret = select.select(read_list, [], [], timeout)
        if read_ret:
            for fd in read_ret:
                if fd == sys.stdin:
                    cmd = sys.stdin.read(1)
                else:
                    print("unknow fd")
        else:
            # print("read timeout")
            pass

        return cmd

    def stop_no_block(self):
        fd = sys.stdin.fileno()
        termios.tcsetattr(fd, termios.TCSADRAIN, self.old_settings)
        self.old_settings = None


class Sim_Action(Node):
    def __init__(self):
        self.init_ControlCMD()
        super().__init__("sim_control_car")
        self.pub_cmd = self.create_publisher(CarControl, "/vehicle_cmd", 1)

    def thread_init(self, method="planne_move"):
        # thread_inpuc_cmd = threading.Thread(target=self.choose_menu, daemon=True)
        thread_inpuc_cmd = threading.Thread(target=self.plane_move, daemon=True)
        thread_inpuc_cmd.start()

    def init_ControlCMD(self):
        data = CarControl()
        data.acc = 0.0
        data.angle = 0.0
        data.brake = 0.0
        data.gear = Gear.P
        self.control_cmd = data

    def choose_menu(self):
        help_str = """
1 车辆移动
2 车灯,清扫,垃圾倾倒,充电,加水控制,鸣笛
"""
        cmd = input(help_str)
        if cmd == "1":
            self.plane_move()
        elif cmd == "2":
            self.other_ctrol()

    def other_ctrol(self):
        "项目,产品 独有功能, 不公开"
        pass

    def publisher_cmdvel(self, speed, angle, gear, brake):
        msg = self.control_cmd
        msg.acc = speed
        msg.angle = angle
        msg.brake = brake
        msg.gear = gear

        self.pub_cmd.publish(msg)

    def plane_move(self):
        help_str = """
\033[80D     w 加速
\033[80D a 右转+      d 左转+
\033[80D     s 减速

\033[80D    i D档
\033[80D j N档      l P档:油门0,方向0
\033[80D    k R档

\033[80D 空格 刹车+油门0, c 方向回正
\033[80D q, Esc: 退出
\033[80D h 帮助
"""

        noblock_term = Noblock_terminal()
        msg = self.control_cmd
        speed = msg.acc
        angle = msg.angle
        brake = msg.brake
        gear = msg.gear

        while True:
            cmd = noblock_term.select_cmd(timeout=0.1)
            # 前后左右移动
            if cmd == "w":
                speed += 2.0
                if speed > 50.0:
                    speed = 50.0
            elif cmd == "s":
                speed -= 2.0
                if speed < 0.0:
                    speed = 0.0
            elif cmd == "a":
                angle -= Angle_15
            elif cmd == "d":
                angle += Angle_15
            # 挡位
            elif cmd == "i":
                gear = Gear.D
                speed = 0.0
            elif cmd == "k":
                gear = Gear.R
                speed = 0.0
            elif cmd == "j":
                gear = Gear.N
            elif cmd == "l":
                gear = Gear.P
                speed = 0.0
            # 刹车
            elif cmd == " ":
                speed = 0.0
                brake += 10.0
                if brake > 70.0:
                    brake = 70.0
            elif cmd == "c":
                angle = 0.0
            elif cmd == "h":
                print(help_str)
            elif cmd == "q":  # q 按键值
                break
            elif ord(cmd) == 0x1B:  # Esc 按键值
                break
            # 超时
            elif cmd == "0":
                if brake > 0.0:
                    brake -= 10.0
            else:
                print("未知指令")
                print(help_str)

            print(
                f"\033[80D speed:{speed:.2f} angle:{angle:.2f} gear:{gear} brake:{brake:.2f}"
            )

            self.publisher_cmdvel(speed, angle, gear, brake)

        print("exit 键盘控制")
        noblock_term.stop_no_block()
        sys.exit(0)


def main(args=None):
    rclpy.init(args=args)
    sim = Sim_Action()
    sim.thread_init()

    rclpy.spin(sim)
    sim.destory_node()
    rclpy.shutdown()
    sim.move_control()


if __name__ == "__main__":
    main()

实测
运行脚本python程序, 按i, 按w
查看话题输出 ros2 topic echo /vehicle_cmd

acc: 6.0
angle: 0.0
brake: 0.0
gear: 4
/—
acc: 8.0
angle: 0.0
brake: 0.0
gear: 4

对比 参考1 优势

1. 老版本 线速度控制, 速度为0时自动驻车; 新版本 油门控车, 松开w继续油门前行, 按 空格space 刹车; --> fix 机械刹车, 一直踩刹车报故障
2. 老版本 w前进, s后退 ; 新版本,需要i/k切换挡位–> fix 机械挡位频繁切换, 导致异常