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.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 优势
- 老版本 线速度控制, 速度为0时自动驻车; 新版本 油门控车, 松开w继续油门前行, 按 空格space 刹车; --> fix 机械刹车, 一直踩刹车报故障
- 老版本 w前进, s后退 ; 新版本,需要i/k切换挡位–> fix 机械挡位频繁切换, 导致异常
原文地址：https://blog.csdn.net/u011326325/article/details/144091641
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！