基本思路:
上一篇已经能实现arduino对ROS节点的订阅与发布,所以只要订阅到ROS的移动控制消息,并把它转换为小车的移动命令即可实现ROS对小车的移动驱动。
ROS小车打造(十一)--arduino订阅/发布Topic_leoFY123的博客-CSDN博客
关键点:
1.ROS的小车移动控制消息cmd_vel是什么样的
2.差速小车如何实现控制
3.cmd_vel与控制如何桥接
1. cmd_vel解析
roscore
新终端
rosrun turtlesim turtle_teleop_key
新终端
rostopic list
rostopic type /turtle1/cmd_vel
rosmsg show geometry_msgs/Twist
以上内容可以看到,机器人的控制有两个大项一个是线速度,一个是角速度
第一项都有一个XYZ,右手坐标系下,车头方向为X+,左为Y+,上为Z+
这里这个坐标系是永远以车为参考对象,不是世界坐标系
那我们就好理解了,如果只有线速度,X+就是正向直线运动,Y+就是左向直线运动,但是可惜小车只能前进后退不能平移,也不能上下,所以只有X有值。
角速度就是绕着某一轴转动,X方向就是打滚,Y方向就是翻跟头 ,Z方向就是转弯,方向同样符合右手定则。我们的小车只能转弯不会翻跟头,所以只有Z向有值。
2.差速小车如何控制
两轮速度一样时,同时为+前进,同时为-后退
左轮比右轮快右转,右轮比左轮快左转。
简单说两轮的共同速度线速度,差值大小反映的是角速度大小
每个轮的快慢由PWM的占空比决定,在这里255(0xFF)是全速,0是停
方向由单独的方向引脚控制
3. 二者如何转换
4.程序编写
arduino端
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h>
#define LEFT_EN A1
#define LEFT_FR A2
#define LEFT_PG 3
#define LEFT_SV 6
#define RIGHT_EN 12
#define RIGHT_FR 8
#define RIGHT_PG 2
#define RIGHT_SV 5
#define ROBOT_WIDTH 300
#define LEFT 0
#define RIGHT 1
unsigned char channel;
unsigned char dir;
unsigned char spd;
unsigned char leftSpd;
unsigned char leftDir;
unsigned char rightSpd;
unsigned char rightDir;
unsigned char recBuf;
double left_vel;
double right_vel;
long lasttim;
long tim;
ros::NodeHandle nh;
void messageCb(const geometry_msgs::Twist& cmd_vel)
{
lasttim=millis();
Serial.println("recevied");
double vel_x=cmd_vel.linear.x;
double vel_th=cmd_vel.angular.z;
left_vel=vel_x-vel_th*ROBOT_WIDTH/2000;
right_vel=vel_x+vel_th*ROBOT_WIDTH/2000;
setSpeed(LEFT,left_vel);
setSpeed(RIGHT,right_vel);
}
void setSpeed(char channel,double vel)
{
vel= vel*255;
if(vel>255)
{
dir=1;
spd=255;
}else if(vel>10)
{
dir=1;
spd=(unsigned char)vel;
}else if(vel>-10)
{
spd=0;
}else if(vel>-255)
{
spd=(unsigned char)(-vel);
dir=0;
}
else
{
dir=0;
spd=255;
}
if(channel== LEFT)
{
digitalWrite(LEFT_FR, dir);
analogWrite (LEFT_SV,spd);
}else if(channel== RIGHT)
{
digitalWrite(RIGHT_FR, !dir);
analogWrite (RIGHT_SV,spd);
}
}
ros::Subscriber<geometry_msgs::Twist> sub("/cmd_vel", messageCb);
void setup()
{
pinMode(LED_BUILTIN, OUTPUT);
pinMode(LEFT_EN, OUTPUT);
pinMode(LEFT_FR, OUTPUT);
pinMode(LEFT_SV, OUTPUT);
pinMode(RIGHT_EN, OUTPUT);
pinMode(RIGHT_FR, OUTPUT);
pinMode(RIGHT_SV, OUTPUT);
Serial.begin(115200);
nh.initNode();
nh.subscribe(sub);
leftSpd=0;
leftDir=0;
rightSpd=0;
rightDir=0;
digitalWrite(LEFT_EN, LOW);
digitalWrite(LEFT_FR, leftDir);
digitalWrite(RIGHT_EN, LOW);
digitalWrite(RIGHT_FR, rightDir);
analogWrite (LEFT_SV,leftSpd);
analogWrite (RIGHT_SV,rightSpd);
}
void loop()
{
// Serial.println(sub.data);
tim = millis();
if((tim-lasttim)>1000)
{
setSpeed(LEFT,0);
setSpeed(RIGHT,0);
}
nh.spinOnce();
delay(1);
}
键盘控制发布cmd_vel python代码mbot_teleop.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
Control mbot!
---------------------------
Moving around:
u i o
j k l
m , .
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly
CTRL-C to quit
"""
moveBindings = {
'i':(1,0),
'o':(1,-1),
'j':(0,1),
'l':(0,-1),
'u':(1,1),
',':(-1,0),
'.':(-1,1),
'm':(-1,-1),
}
speedBindings={
'q':(1.1,1.1),
'z':(.9,.9),
'w':(1.1,1),
'x':(.9,1),
'e':(1,1.1),
'c':(1,.9),
}
def getKey():
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
speed = .2
turn = 1
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('mbot_teleop')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
x = 0
th = 0
status = 0
count = 0
acc = 0.1
target_speed = 0
target_turn = 0
control_speed = 0
control_turn = 0
try:
print msg
print vels(speed,turn)
while(1):
key = getKey()
# 运动控制方向键(1:正方向,-1负方向)
if key in moveBindings.keys():
x = moveBindings[key][0]
th = moveBindings[key][1]
count = 0
# 速度修改键
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0] # 线速度增加0.1倍
turn = turn * speedBindings[key][1] # 角速度增加0.1倍
count = 0
print vels(speed,turn)
if (status == 14):
print msg
status = (status + 1) % 15
# 停止键
elif key == ' ' or key == 'k' :
x = 0
th = 0
control_speed = 0
control_turn = 0
else:
count = count + 1
if count > 4:
x = 0
th = 0
if (key == '\x03'):
break
# 目标速度=速度值*方向值
target_speed = speed * x
target_turn = turn * th
# 速度限位,防止速度增减过快
if target_speed > control_speed:
control_speed = min( target_speed, control_speed + 0.02 )
elif target_speed < control_speed:
control_speed = max( target_speed, control_speed - 0.02 )
else:
control_speed = target_speed
if target_turn > control_turn:
control_turn = min( target_turn, control_turn + 0.1 )
elif target_turn < control_turn:
control_turn = max( target_turn, control_turn - 0.1 )
else:
control_turn = target_turn
# 创建并发布twist消息
twist = Twist()
twist.linear.x = control_speed;
twist.linear.y = 0;
twist.linear.z = 0
twist.angular.x = 0;
twist.angular.y = 0;
twist.angular.z = control_turn
pub.publish(twist)
except:
print e
finally:
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
运行
新终端
roscore
新终端
rosrun rosserial_python serial_node.py /dev/ttyUSB4
新终端
python mbot_teleop.py