ROS小车打造（12）－－Arduino订阅cmd_vel实现差速控制

基本思路：

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