基本思路：

上一篇已经能实现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