ROS机器人是上个月就到了的，但之前一直在处于无从下手的边缘，因为里面涉及到众多尚不熟悉的领域。目前已经把turtlebot3_teleop_key的源码初步研究下已了解差不多。希望下周可以把建图的那一块搞出突破口。
ROS主要可供参考的网址：
1. ROBOTIS Turtlebot3
2. ROS Tutorials
还有一个中文网址：
创客智造—Turtlebot3
中文网址仅供英文不好的童鞋参考，但里面可能会有一些问题（至少我发现了一个问题），尽量还是以英文官网为主，否则出了错误又要满世界搜索。
ROS是一个开源的操作系统，从底层的硬件到上层的包管理都提供了支持与服务，这也意味着所有的东西都要做到基本了解，关键部分需要熟悉与掌握才能做到二次开发。
作为突破口，我先从看起来比较简单的turtlebot3_teleop_key部分下手，该代码的路径位于
~/catkin_ws/src/turtlebot3/turtlebot3_teleop/notes/
以下为带上我写注释的源码，如有错误，敬请指出。
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
if os.name == 'nt':
import msvcrt
else:
import tty, termios
# burger型小车
BURGER_MAX_LIN_VEL = 0.22 # 直线运动最大速度
BURGER_MAX_ANG_VEL = 2.84 # 旋转运动最大角速度
# waffle和waffle_pi型小车
WAFFLE_MAX_LIN_VEL = 0.26 # 直线运动最大速度
WAFFLE_MAX_ANG_VEL = 1.82 # 旋转运动最大角速度
LIN_VEL_STEP_SIZE = 0.01 # 直线运动的匀速步进范围
ANG_VEL_STEP_SIZE = 0.1 # 旋转运动的角速步进范围
msg = """
Control Your TurtleBot3!
---------------------------
Moving around:
w
a s d
x
w/x : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)
space key, s : force stop
CTRL-C to quit
"""
e = """
Communications Failed
"""
#读取键盘数据
def getKey():
if os.name == 'nt':
return msvcrt.getch()
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
def vels(target_linear_vel, target_angular_vel): # 显示输出状态信息
return "currently:\tlinear vel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)
def makeSimpleProfile(output, input, slop): # 保证每次赋给小车使能的速度值是渐进的，而不是跳跃式的
if input > output:
output = min( input, output + slop ) # 渐进的幅值为二分之一步长
elif input < output:
output = max( input, output - slop )
else:
output = input
return output
def constrain(input, low, high): #当前值，最小值，最大值
if input < low:
input = low
elif input > high:
input = high
else:
input = input
return input
def checkLinearLimitVelocity(vel):
if turtlebot3_model == "burger":
vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL) #保证值在设置约束的范围内
elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL)
else:
vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
return vel
def checkAngularLimitVelocity(vel):
if turtlebot3_model == "burger":
vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL)
else:
vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
return vel
if __name__=="__main__":
if os.name != 'nt':
settings = termios.tcgetattr(sys.stdin)
# ROS初始化
rospy.init_node('turtlebot3_teleop')
# 创建一个publisher，并发布一个名为cmd_vel的topic，消息类型为Twist，队列(缓冲区)长度为10
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# 选择小车
turtlebot3_model = rospy.get_param("model", "burger")
status = 0
target_linear_vel = 0.0 # 直线前进速度
target_angular_vel = 0.0 # 旋转角速度
control_linear_vel = 0.0 # 直线控制速度
control_angular_vel = 0.0 # 角度控制速度
try:
print(msg)
while(1):
key = getKey()# 读取键值
if key == 'w' :
target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == 'x' :
target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == 'a' :
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == 'd' :
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == ' ' or key == 's' :
target_linear_vel = 0.0
control_linear_vel = 0.0
target_angular_vel = 0.0
control_angular_vel = 0.0
print(vels(target_linear_vel, target_angular_vel))
else:
if (key == '\x03'): # Ctrl + C
break
if status == 20 :
print(msg)
status = 0
twist = Twist()# 通过参数控制电机
# 根据当前的速度值对设置的速度参数进行调整
control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
# 直线控制
twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
# 角度控制
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
# 发布消息
pub.publish(twist)
except:
print(e)
finally:# 停止运转
twist = Twist()
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
pub.publish(twist)
if os.name != 'nt':
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
通过观看代码可以基本了解到，整个发布的过程为先初始化，然后循环读取键值来修改参数，参数主要分为直线运动速度(target_linear_vel)和角速度(target_angular_vel)，这两个参数由终端读取键值并直接设置到该参数上，但作用于小车上的值并非这两个参数，而是另外定义了两个控制的值：control_linear_vel和control_angular_vel。中间额外定义的两个值主要为小车的过渡设置，以防止参数骤增或骤减导致小车出现故障，个人猜测是延长电机使用寿命，且更安全。具体的设置的方式见makeSimpleProfile函数。同时，对控制参数设置了一个最大值。本来我以为是参数设置过大会造成小车电机转速过快而不安全，但后面更改了最大值发现小车电机运转效果与修改前相当，那么默认的值估计便已经是最大的值了。
根据对以上源码的理解，我修改了代码增加了方向键的控制并修改了按键驱动逻辑。因为才刚刚接触python，正好拿来练练手。值得一提的是，因为python是解释器，调试起来感觉比C语言之类的编译器要快得多，也是一种全新的调试感受。
下面附上运行效果和源码，我就不详细写注释了，上面那个代码看懂了这个应该也能看懂。主要需要注意的是方向键的读取逻辑，因为任意方向键按下去一次就会接受三个字节的数据，并且不同的方向键前两个字节都一样，所以读取完第一个字节后需要再读取两次字节，并在最后一个字节确认是方向键中的哪一个键。
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
if os.name == 'nt':
import msvcrt
else:
import tty, termios
BURGER_MAX_LIN_VEL = 0.42
BURGER_MAX_ANG_VEL = 4.84
WAFFLE_MAX_LIN_VEL = 0.26
WAFFLE_MAX_ANG_VEL = 1.82
LIN_VEL_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.1
msg = """
Control Your TurtleBot3!
---------------------------
Moving around:
w ^
a s d < v >
w/s : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)
space key, Enter(space) : force stop
CTRL-C to quit
"""
e = """
Communications Failed
"""
def getKey():
if os.name == 'nt':
return msvcrt.getch()
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
def vels(target_linear_vel, target_angular_vel):
return "currently:\tlinear vel %s \tangular vel %s " % (target_linear_vel,target_angular_vel)
def makeSimpleProfile(output, input, slop):
if input > output:
output = min( input, output + slop )
elif input < output:
output = max( input, output - slop )
else:
output = input
return output
def constrain(input, low, high):
if input < low:
input = low
elif input > high:
input = high
elif input > 0.0001:
input = input
elif input < -0.0001:
input = input
else :
input = 0.0
return input
def checkLinearLimitVelocity(vel):
if turtlebot3_model == "burger":
vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL)
else:
vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
return vel
def checkAngularLimitVelocity(vel):
if turtlebot3_model == "burger":
vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL)
else:
vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
return vel
if __name__=="__main__":
if os.name != 'nt':
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('turtlebot3_teleop')
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
turtlebot3_model = rospy.get_param("model", "burger")
status = 0
target_linear_vel = 0.0
target_angular_vel = 0.0
control_linear_vel = 0.0
control_angular_vel = 0.0
first_linear_vel = 0.1
first_angular_vel = 1.0
run = False;
runnum = 0;
try:
print(msg)
while(1):
key = getKey()
# if key != '':
# print('key = %s', key)
if key == 'w' :
first_linear_vel = checkLinearLimitVelocity(first_linear_vel + LIN_VEL_STEP_SIZE)
status = status + 1
print(vels(first_linear_vel,first_angular_vel))
elif key == 's' :
first_linear_vel = checkLinearLimitVelocity(first_linear_vel - LIN_VEL_STEP_SIZE)
status = status + 1
print(vels(first_linear_vel,first_angular_vel))
elif key == 'a' :
first_angular_vel = checkAngularLimitVelocity(first_angular_vel + ANG_VEL_STEP_SIZE)
status = status + 1
print(vels(first_linear_vel,first_angular_vel))
elif key == 'd' :
first_angular_vel = checkAngularLimitVelocity(first_angular_vel - ANG_VEL_STEP_SIZE)
status = status + 1
print(vels(first_linear_vel,first_angular_vel))
elif key == ' ' or key == '\r' :
target_linear_vel = 0.0
control_linear_vel = 0.0
target_angular_vel = 0.0
control_angular_vel = 0.0
status = status + 1
print(vels(target_linear_vel, target_angular_vel))
elif key != '':
if (key == '\x03'): # ctrl+c
break
elif (key == '\x1b'): # direction
key = sys.stdin.read(1)
if (key == '[') :
key = sys.stdin.read(1)
if (key == 'A') :
if run == False:
run = True
target_linear_vel = first_linear_vel
status = status + 1
runnum = 0;
print(vels(target_linear_vel,target_angular_vel) + '\tup')
# print('upOK')
elif (key == 'B') :
if run == False:
run = True
target_linear_vel = -first_linear_vel
status = status + 1
runnum = 0;
print(vels(target_linear_vel,target_angular_vel) + '\tdown')
# print('downOK')
elif (key == 'D') :
if run == False:
run = True
target_angular_vel = first_angular_vel
status = status + 1
runnum = 0;
print(vels(target_linear_vel,target_angular_vel) + '\tleft')
# print('leftOK')
elif (key == 'C') :
if run == False:
run = True
target_angular_vel = -first_angular_vel
status = status + 1
runnum = 0;
print(vels(target_linear_vel,target_angular_vel) + '\tright')
# print('rightOK')
elif run == True:
if runnum < 4:
runnum = runnum + 1
else :
target_linear_vel = 0.0
control_linear_vel = 0.0
target_angular_vel = 0.0
control_angular_vel = 0.0
run = False
status = status + 1
print(vels(target_linear_vel, target_angular_vel))
if status >= 20 :
print(msg)
status = 0
twist = Twist()
control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
pub.publish(twist)
except:
print(e)
finally:
twist = Twist()
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
pub.publish(twist)
if os.name != 'nt':
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
昨天刚看了古月老师的21讲视频，参考了第10讲的发布者Publisher的编程实现。看完后今天便想尝试一下将原来的python语言写的turtlebot3_teleop_key部分用C/C++语言重写一遍。拿来练手，也加深一下Publisher实现过程的印象。经过反复的调试，下面附上运行图和代码为了区分开来，文件名被我定义为turtlebot3_teleop_ckey.cpp。代码看起来和python差不多，也是有一点点的区别的。同时并不像python解释器那样写好便可解释执行查看运行效果，需要catkin_make编译后才可以执行。同时在首次编译之前还需要添加修改CMakeLists.txt和package.xml两个文件中的内容，具体可以看古月老师讲但的视频：链接，或者看官方教程：链接。
值得一提的是，虽然用C/C++语言调试过程麻烦一些，但是应用执行的效率比python快很多，用python写的代码在执行后的初始化需要等待一两秒，而用C/C++语言开发的代码从开始启动不到一秒便已经初始化完成。
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <stdio.h>
#include <termio.h>
#include <string>
#define BURGER_MAX_LIN_VEL 0.22
#define BURGER_MAX_ANG_VEL 2.84
#define WAFFLE_MAX_LIN_VEL 0.26
#define WAFFLE_MAX_ANG_VEL 1.82
#define LIN_VEL_STEP_SIZE 0.01
#define ANG_VEL_STEP_SIZE 0.1
using namespace std;
int getch();
float constrain(float input, float low, float high);
float checkLinearLimitVelocity(float vel);
float checkAngularLimitVelocity(float vel);
float makeSimpleProfile(float output, float input, float slop);
char msg[] = "\nControl Your TurtleBot3!\n\
---------------------------\n\
Moving around:\n\
w\n\
a s d\n\
x\n\
\n\
w/x : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26)\n\
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)\n\
\n\
space key, s : force stop\n\
\n\
CTRL-C to quit\n\n";
string turtlebot3_model;
int main(int argc, char **argv)
{
ros::init(argc, argv, "turtlebot3_teleop");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
turtlebot3_model = "burger";
int status = 0;
float target_linear_vel = 0.0;
float target_angular_vel = 0.0;
float control_linear_vel = 0.0;
float control_angular_vel = 0.0;
printf("%s", msg);
bool kda = true;
while(kda)
{
int key = getch();
//printf("key = %d\n", key);
switch (key)
{
case 119: //w
{
target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE);
status++;
ROS_INFO("currently:\tlinear vel %.2f\t angular vel %.2f", target_linear_vel, target_angular_vel);
break;
}
case 120: //x
{
target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE);
status++;
ROS_INFO("currently:\tlinear vel %.2f\t angular vel %.2f", target_linear_vel, target_angular_vel);
break;
}
case 97: //a
{
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE);
status++;
ROS_INFO("currently:\tlinear vel %.2f\t angular vel %.2f", target_linear_vel, target_angular_vel);
break;
}
case 100: //d
{
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE);
status++;
ROS_INFO("currently:\tlinear vel %.2f\t angular vel %.2f", target_linear_vel, target_angular_vel);
break;
}
case 115: //s
{
target_linear_vel = 0.0;
target_angular_vel = 0.0;
control_linear_vel = 0.0;
control_angular_vel = 0.0;
status++;
ROS_INFO("currently:\tlinear vel %.2f\t angular vel %.2f", target_linear_vel, target_angular_vel);
break;
}
case 13: break; //Enter
case 32: //space
{
target_linear_vel = 0.0;
target_angular_vel = 0.0;
control_linear_vel = 0.0;
control_angular_vel = 0.0;
status++;
ROS_INFO("currently:\tlinear vel %.2f\t angular vel %.2f", target_linear_vel, target_angular_vel);
break;
}
case 27: //Esc
{
key = getch();
if (key == 91)
{
key = getch();
if (key == 65) //^
{
printf("^\n");
}
else if (key == 66) //v
{
printf("v\n");
}
else if (key == 68) //<
{
printf("<\n");
}
else if (key == 67) //>
{
printf(">\n");
}
}
else
{
printf("Esc\n");
}
break;
}
case 3: //Ctrl + c
{
target_linear_vel = 0.0;
target_angular_vel = 0.0;
control_linear_vel = 0.0;
control_angular_vel = 0.0;
kda = false;
break;
}
}
if (status >= 20)
{
printf("%s", msg);
status = 0;
}
geometry_msgs::Twist twist;
control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, LIN_VEL_STEP_SIZE/2.0);
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, ANG_VEL_STEP_SIZE/2.0);
twist.linear.x = control_linear_vel;
twist.linear.y = 0.0;
twist.linear.z = 0.0;
twist.angular.x = 0.0;
twist.angular.y = 0.0;
twist.angular.z = control_angular_vel;
pub.publish(twist);
}
return 0;
}
float constrain(float input, float low, float high)
{
if (input < low)
{
input = low;
}
else if (input > high)
{
input = high;
}
else
{
input = input;
}
return input;
}
float makeSimpleProfile(float output, float input, float slop)
{
if (input > output)
{
if (input < output + slop)
{
output = input;
}
else
{
output = output + slop;
}
}
else if (input < output)
{
if (input > output - slop)
{
output = input;
}
else
{
output = output - slop;
}
}
else
{
output = input;
}
return output;
}
float checkLinearLimitVelocity(float vel)
{
if (turtlebot3_model == "burger")
{
vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL);
}
else if (turtlebot3_model == "waffle" || turtlebot3_model == "waffle_pi")
{
vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL);
}
else
{
vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL);
}
return vel;
}
float checkAngularLimitVelocity(float vel)
{
if (turtlebot3_model == "burger")
{
vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL);
}
else if (turtlebot3_model == "waffle" || turtlebot3_model == "waffle_pi")
{
vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL);
}
else
{
vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL);
}
return vel;
}
int getch()
{
struct termios tm, tm_old;
int fd = 0, ch;
if (tcgetattr(fd, &tm) < 0) {//保存现在的终端设置
return -1;
}
tm_old = tm;
cfmakeraw(&tm);//更改终端设置为原始模式，该模式下所有的输入数据以字节为单位被处理
if (tcsetattr(fd, TCSANOW, &tm) < 0) {//设置上更改之后的设置
return -1;
}
ch = getchar();
if (tcsetattr(fd, TCSANOW, &tm_old) < 0) {//更改设置为最初的样子
return -1;
}
return ch;
}