目录
- 写在前面的话
- 遇到的问题
-
- 不能通过launch方式启动
- [安装 gazebo_ros_control](#安装 gazebo_ros_control)
- 同相模式需要加入y方向线速度
- 原地旋转模型需要角速度即可
- 发布的角速度需要取反
- 线速度不能过小（反相模式）
- 步骤
- 完整代码
- 控制演示
- 视频演示
写在前面的话
之前的一篇的文章（底盘四轮转向运动学解析（含代码））设置了四轮转向控制器，通过线速度和角速度计算四个轮子的转速和方向。那下一步需要考虑使用键盘去控制车子运动，参考代码（teleop_twist_keyboard_trio/teleop_keyboard.py）。
遇到的问题
不能通过launch方式启动
会出现如下报错：（意思是系统版本不适配，其实改为run启动方式即可）
bash
orig_attr = termios.tcgetattr(self.fd)
termios.error: (25, 'Inappropriate ioctl for device')安装 gazebo_ros_control
不能缺少 gazebo-ros2-control，否则控制指令无法生效
bash
sudo apt install ros-humble-gazebo-ros-pkgs
sudo apt install ros-humble-gazebo-msgs
sudo apt install ros-humble-gazebo-plugins
sudo apt install ros-humble-gazebo-ros2-control
sudo apt install ros-humble-teleop-twist-keyboard 
同相模式需要加入y方向线速度
没y方向线速度无法实现同相模式行驶
python
# in-phase
elif(mode_selection == 2):
V = math.hypot(vel_msg.linear.x, vel_msg.linear.y)
sign = np.sign(vel_msg.linear.x)
if(vel_msg.linear.x != 0):
ang = vel_msg.linear.y / vel_msg.linear.x
else:
ang = 0
self.pos[0] = math.atan(ang)
self.pos[1] = math.atan(ang)
self.pos[2] = self.pos[0]
self.pos[3] = self.pos[1]
self.vel[:] = sign*V 原地旋转模型需要角速度即可
python
# pivot turn
elif(mode_selection == 3):
self.pos[0] = -math.atan(self.wheel_base/self.steering_track)
self.pos[1] = math.atan(self.wheel_base/self.steering_track)
self.pos[2] = math.atan(self.wheel_base/self.steering_track)
self.pos[3] = -math.atan(self.wheel_base/self.steering_track)
self.vel[0] = -vel_msg.angular.z
self.vel[1] = vel_msg.angular.z
self.vel[2] = self.vel[0]
self.vel[3] = self.vel[1]发布的角速度需要取反
根据实验发现需要角速度取反才可以符合行驶要求
线速度不能过小（反相模式）
由于线速度过小，导致左右前轮转向相反，不符合反相模式。
步骤
完整代码
构建python包
bash
ros2 pkg create car_keyboard_controller --build-type ament_python --dependencies rclpy运行代码
bash
ros2 run car_keyboard_controller car_keyboard_controller car_keyboard_twist.py
python
#!/usr/bin/python3
import math
import threading
import rclpy
import numpy as np
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
import sys, select, termios, tty
import pty
'''
wheel_separation:
Shortest distance between the left and right wheels. If this parameter is wrong,
the robot will not behave correctly in curves.
wheel_base:
Distance between front and rear wheels, in meters.
wheel_radius:
Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations.
If this parameter is wrong the robot will move faster or slower then expected.
'''
class Commander(Node):
def __init__(self):
super().__init__('commander')
self.wheel_seperation = 1.2
self.wheel_base = 1
self.wheel_radius = 0.3
self.wheel_steering_y_offset = 0.03
self.steering_track = self.wheel_seperation - 2*self.wheel_steering_y_offset #1.14
self.pos = np.array([0,0,0,0], float)
self.vel = np.array([0,0,0,0], float) #left_front, right_front, left_rear, right_rear
# remappings = [('/forward_velocity_controller/commands', '/cmd_vel'),
# ('/forward_position_controller/commands', '/cmd_ang')], ####
self.pub_pos = self.create_publisher(Float64MultiArray, '/forward_position_controller/commands', 10)
self.pub_vel = self.create_publisher(Float64MultiArray, '/forward_velocity_controller/commands', 10)
def pub_pos_vel(self, vel_msg, mode_selection):
# opposite phase
if(mode_selection == 1):
vel_steerring_offset = vel_msg.angular.z * self.wheel_steering_y_offset
sign = np.sign(vel_msg.linear.x)
self.vel[0] = sign*math.hypot(vel_msg.linear.x - vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2) - vel_steerring_offset
self.vel[1] = sign*math.hypot(vel_msg.linear.x + vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2) + vel_steerring_offset
self.vel[2] = sign*math.hypot(vel_msg.linear.x - vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2) - vel_steerring_offset
self.vel[3] = sign*math.hypot(vel_msg.linear.x + vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2) + vel_steerring_offset
self.pos[0] = math.atan(vel_msg.angular.z*self.wheel_base/(2*vel_msg.linear.x + vel_msg.angular.z*self.steering_track+0.001))
self.pos[1] = math.atan(vel_msg.angular.z*self.wheel_base/(2*vel_msg.linear.x - vel_msg.angular.z*self.steering_track+0.001))
self.pos[2] = -self.pos[0]
self.pos[3] = -self.pos[1]
# in-phase
elif(mode_selection == 2):
V = math.hypot(vel_msg.linear.x, vel_msg.linear.y)
sign = np.sign(vel_msg.linear.x)
if(vel_msg.linear.x != 0):
ang = vel_msg.linear.y / vel_msg.linear.x
else:
ang = 0
self.pos[0] = math.atan(ang)
self.pos[1] = math.atan(ang)
self.pos[2] = self.pos[0]
self.pos[3] = self.pos[1]
self.vel[:] = sign*V
# pivot turn
elif(mode_selection == 3):
self.pos[0] = -math.atan(self.wheel_base/self.steering_track)
self.pos[1] = math.atan(self.wheel_base/self.steering_track)
self.pos[2] = math.atan(self.wheel_base/self.steering_track)
self.pos[3] = -math.atan(self.wheel_base/self.steering_track)
self.vel[0] = -vel_msg.angular.z
self.vel[1] = vel_msg.angular.z
self.vel[2] = self.vel[0]
self.vel[3] = self.vel[1]
else:
self.pos[:] = 0
self.vel[:] = 0
# fixed by xcg: change the sign of the position
self.pos = -self.pos
pos_array = Float64MultiArray(data=self.pos)
vel_array = Float64MultiArray(data=self.vel)
self.pub_pos.publish(pos_array)
self.pub_vel.publish(vel_array)
self.pos[:] = 0
self.vel[:] = 0
class Keyboard_subscriber(Node):
def __init__(self):
super().__init__('keyboard_subscriber')
self.msg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
注意：r是停止，角速度不能设置太大会导致转弯不稳车身摆动,
要想实现反相效果需要线速度大于角速度，否则车轮转角会不正确。
Moving around:
q w e (opposite-phase)
a s d f g h j (in-phase)
z c
anything else : stop
f/g : run to left_front/right_front (in-phase)
h/j : run to left_back/right_back (in-phase)
,/. : increase/decrease max speed by 10%
v/b : increase/decrease only linear speed by 10%
n/m : increase/decrease only angular speed by 10%
mode_selection
0 or other_number: 静止(stop 没速度控制)
1: 低速小角度转弯(oppostise-phase)
2: 高速变道(in-phase)需要y轴速度
3: 原地转弯(pviot-turn)
CTRL-C to quit
"""
self.moveBindings = {
'w':(1,0,0,0),
's':(-1,0,0,0),
'q':(1,0,0,1),
'e':(1,0,0,-1),
'a':(0,0,0,1),
'd':(0,0,0,-1),
'f':(1,1,0,0),
'g':(1,-1,0,0),
'h':(-1,1,0,0),
'j':(-1,-1,0,0),
'z':(-1,0,0,1),
'c':(-1,0,0,-1),
'r':(0,0,0,0),
# 'W':(1,0,0,0),
# 'S':(-1,0,0,0),
# 'Q':(1,0,0,1),
# 'E':(1,0,0,-1),
# 'A':(0,0,0,1),
# 'D':(0,0,0,-1),
# 'Z':(-1,0,0,1),
# 'C':(-1,0,0,-1),
# 'R':(0,0,0,0),
}
self.speedBindings={
',':(1.1,1.1),
'.':(0.9,0.9),
'v':(1.1,1),
'b':(0.9,1),
'n':(1,1.1),
'm':(1,.9),
}
self.modeBindings={
'1':'opposite-phase',# 低速转弯
'2':'in-phase',
'3':'pivot-turn',#原地旋转
}
# pid = pty.fork()
# if not pid:
self.old_settings = termios.tcgetattr(sys.stdin)
print(self.old_settings)
self.commander = Commander()
self.vel_msg = Twist() # robot velocity
self.mode_selection = 1 # 1:opposite phase, 2:in-phase, 3:pivot turn 4: none
self.speed = 0.5
self.turn = 0.5 #30度左右
self.x = 0.0
self.y = 0.0
self.th = 0.0
self.status = 0.0
def getKey(self):
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
return key
def listener_keyboard(self):
try:
print(self.msg)
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")
while True:
key = self.getKey()#获取按键
# print(f'Press {key}')
if key in self.moveBindings.keys():
self.x = float(self.moveBindings[key][0])
self.y = float(self.moveBindings[key][1])
self.th = float(self.moveBindings[key][3])
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.x * self.speed}) \tturn({self.th * self.turn})")
elif key in self.speedBindings.keys():
self.speed = self.speed * self.speedBindings[key][0]
self.turn = self.turn * self.speedBindings[key][1]
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.x * self.speed}) \tturn({self.th * self.turn})")
elif key in self.modeBindings.keys():
if self.modeBindings[key] == 'opposite-phase':
self.mode_selection = 1
elif self.modeBindings[key] == 'in-phase':
self.mode_selection = 2
elif self.modeBindings[key] == 'pivot-turn':
self.mode_selection = 3
else:
self.mode_selection = 0
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")
else:
self.x = 0.0
self.y = 0.0
self.th = 0.0
if (self.status == 14):
print(self.msg)
self.status = (self.status + 1) % 15
if (key == '\x03'):
break
self.vel_msg.linear.x = self.x * self.speed
self.vel_msg.linear.y = self.y * self.speed
self.vel_msg.angular.x = 0.0
self.vel_msg.angular.y = 0.0
self.vel_msg.angular.z = self.th * self.turn
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")
self.commander.pub_pos_vel(self.vel_msg, self.mode_selection)
except Exception as e:
print('sss',e)
finally:
self.vel_msg.linear.x = 0.0;
self.vel_msg.linear.y = 0.0;
self.vel_msg.linear.z = 0.0
self.vel_msg.angular.x = 0.0;
self.vel_msg.angular.y = 0.0;
self.vel_msgangular.z = 0.0
self.commander.pub_pos_vel(self.vel_msg, self.mode_selection)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
def main(args=None):
rclpy.init(args=sys.argv)
keyboard_subscriber = Keyboard_subscriber()
keyboard_subscriber.listener_keyboard()
rclpy.spin(keyboard_subscriber)
rclpy.shutdown()
if __name__ == '__main__':
main()setup.py
加入
car_keyboard_controller = car_keyboard_controller.car_keyboard_twist:main
python
from setuptools import find_packages, setup
package_name = 'car_keyboard_controller'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='gg',
maintainer_email='gg@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'car_keyboard_controller = car_keyboard_controller.car_keyboard_twist:main'
],
},
)
控制演示
视频演示
gazebo