基本思路:
上一篇已经能实现arduino对ROS节点的订阅与发布,所以只要订阅到ROS的移动控制消息,并把它转换为小车的移动命令即可实现ROS对小车的移动驱动。
ROS小车打造(十一)–arduino订阅/发布Topic_leoFY123的博客-CSDN博客
关键点:
1.ROS的小车移动控制消息cmd_vel是什么样的
2.差速小车如何实现控制
3.cmd_vel与控制如何桥接
- 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
#include
#include
#include
#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 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
`python
!/usr/bin/env python
–– coding: utf-8 ––
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = “””
Control mbot!
Original: https://blog.csdn.net/leoFY123/article/details/122718382
Author: leoFY123
Title: ROS小车打造(12)--Arduino订阅cmd_vel实现差速控制
原创文章受到原创版权保护。转载请注明出处:https://www.johngo689.com/597409/
转载文章受原作者版权保护。转载请注明原作者出处!