ros2键盘实现车辆: 简单的油门_刹车_挡位

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

参考:

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

1.背景与需求

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

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

2.功能需求

键位功能如下
在这里插入图片描述

特性介绍

按一下a, 转向-15度; 按一下d, 转向+15度
长按space , 刹车0.0->70.0 %, 每0.1s加10.0%刹车
按c 转向为0
按意挡位切换,油门为0
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.01745def Rad2Ang(rad) -> float:return rad * 57.2958Angle_15 = Ang2Rad(15)
Angle_45 = Ang2Rad(45)
Compensation_Rad = 4.0class 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 chdef 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")passreturn cmddef stop_no_block(self):fd = sys.stdin.fileno()termios.tcsetattr(fd, termios.TCSADRAIN, self.old_settings)self.old_settings = Noneclass 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.0data.angle = 0.0data.brake = 0.0data.gear = Gear.Pself.control_cmd = datadef 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):"项目,产品 独有功能, 不公开"passdef publisher_cmdvel(self, speed, angle, gear, brake):msg = self.control_cmdmsg.acc = speedmsg.angle = anglemsg.brake = brakemsg.gear = gearself.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_cmdspeed = msg.accangle = msg.anglebrake = msg.brakegear = msg.gearwhile True:cmd = noblock_term.select_cmd(timeout=0.1)# 前后左右移动if cmd == "w":speed += 2.0if speed > 50.0:speed = 50.0elif cmd == "s":speed -= 2.0if speed < 0.0:speed = 0.0elif cmd == "a":angle -= Angle_15elif cmd == "d":angle += Angle_15# 挡位elif cmd == "i":gear = Gear.Dspeed = 0.0elif cmd == "k":gear = Gear.Rspeed = 0.0elif cmd == "j":gear = Gear.Nelif cmd == "l":gear = Gear.Pspeed = 0.0# 刹车elif cmd == " ":speed = 0.0brake += 10.0if brake > 70.0:brake = 70.0elif cmd == "c":angle = 0.0elif cmd == "h":print(help_str)elif cmd == "q":  # q 按键值breakelif ord(cmd) == 0x1B:  # Esc 按键值break# 超时elif cmd == "0":if brake > 0.0:brake -= 10.0else: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()