开源|智能车目标识别系统连载教程—RT-Thread连接ROS摄像头小车控制（6）
上期回顾：
引言
这篇文档会介绍如何用 RT-Thread 和 ROS 连接实现一个带摄像头的远程控制小车。
这里先把整个系统框图画出来，这样如果想要自己做一辆这样的小车也可以动手试一试：
实物图看起来就是这样：
2.ROS平滑运动
2.1 ROS 工作环境
下面的代码都是在安装了 ROS 的电脑上操作的
ROS 的安装之前已经介绍过了，这里就不重复了，我们先新建一个工作区间：
1$ mkdir telebot_ws && cd telebot_ws
2$ catkin_init_workspace
我们再新建一个 ROS 软件包：
1$ cd src
2$ catkin_create_pkg telebot rospy
这样我们就可以开始 ROS 的开发了，在 telebot_ws 目录下：
1$ catkin_make
2$ source devel/setup.bash
2.2 按键触发
我们先建立一个节点用来监听键盘的按键，并且将收到的按键发布到 /keys 这个话题 (ROS 节点间的通信就是靠发布和订阅话题实现的)，我们在
telebot_ws/src/telebot/src
目录下新建一个文件
key_publisher.py
1#!/usr/bin/python
2
3# 导入软件包
4import sys, select, tty, termios
5import rospy
6from std_msgs.msg import String
7
8if __name__ == '__main__':
9 # 初始化节点
10 key_pub = rospy.Publisher('keys', String, queue_size=1)
11 rospy.init_node("keyboard_driver")
12 rate = rospy.Rate(100)
13
14 # 设置终端输入模式
15 old_attr = termios.tcgetattr(sys.stdin)
16 tty.setcbreak(sys.stdin.fileno())
17 print("Publishing keystrokes. Press Ctrl-C to exit ...")
18
19 # 循环监听键盘事件
20 while not rospy.is_shutdown():
21 if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]:
22 # 发布监听到的按键
23 key_pub.publish(sys.stdin.read(1))
24 rate.sleep()
25
26 # 恢复终端设置
27 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_attr)
上面的代码不到 20 行，我也添加了一些注释，就不详细介绍了，我们为这个文件添加可执行权限：
1$ chmod u+x key_publisher.py
就可以启动节点了：
1$ rosrun telebot key_publisher.py
这样我们就可以看到有一个 /keys 的话题会不断输出键盘按键：
1$ rostopic echo /keys
2data: "w"
3---
4data: "a"
5---
6data: "s"
7---
2.3 按键解析
现在已经有一个节点在发布我们的按键消息了，接下来就是把按键消息转换为小车的运动指令了，也就是发布到 /cmd_vel，我们在 telebot_ws/src/telebot/src 目录下新建一个文件 keys_to_twist_with_ramps.py：
1#!/usr/bin/python
2
3# 导入软件包
4import rospy
5import math
6from std_msgs.msg import String
7from geometry_msgs.msg import Twist
8
9# 键盘和速度映设 w a s d
10key_mapping = { 'w': [ 0, 1], 'x': [ 0, -1],
11 'a': [ -1, 0], 'd': [1, 0],
12 's': [ 0, 0] }
13g_twist_pub = None
14g_target_twist = None
15g_last_twist = None
16g_last_send_time = None
17g_vel_scales = [0.1, 0.1] # default to very slow
18g_vel_ramps = [1, 1] # units: meters per second^2
19
20# 防止速度突变
21def ramped_vel(v_prev, v_target, t_prev, t_now, ramp_rate):
22 # compute maximum velocity step
23 step = ramp_rate * (t_now - t_prev).to_sec()
24 sign = 1.0 if (v_target > v_prev) else -1.0
25 error = math.fabs(v_target - v_prev)
26 if error < step: # we can get there within this timestep. we're done.
27 return v_target
28 else:
29 return v_prev + sign * step # take a step towards the target
30
31def ramped_twist(prev, target, t_prev, t_now, ramps):
32 tw = Twist()
33 tw.angular.z = ramped_vel(prev.angular.z, target.angular.z, t_prev,
34 t_now, ramps[0])
35 tw.linear.x = ramped_vel(prev.linear.x, target.linear.x, t_prev,
36 t_now, ramps[1])
37 return tw
38
39# 发布控制指令到 /cmd_vel
40def send_twist():
41 global g_last_twist_send_time, g_target_twist, g_last_twist,\
42 g_vel_scales, g_vel_ramps, g_twist_pub
43 t_now = rospy.Time.now()
44 g_last_twist = ramped_twist(g_last_twist, g_target_twist,
45 g_last_twist_send_time, t_now, g_vel_ramps)
46 g_last_twist_send_time = t_now
47 g_twist_pub.publish(g_last_twist)
48
49# 订阅 /keys 的回调函数
50def keys_cb(msg):
51 global g_target_twist, g_last_twist, g_vel_scales
52 if len(msg.data) == 0 or not key_mapping.has_key(msg.data[0]):
53 return # unknown key.
54 vels = key_mapping[msg.data[0]]
55 g_target_twist.angular.z = vels[0] * g_vel_scales[0]
56 g_target_twist.linear.x = vels[1] * g_vel_scales[1]
57
58# 获取传递进来的速度加速度比例
59def fetch_param(name, default):
60 if rospy.has_param(name):
61 return rospy.get_param(name)
62 else:
63 print "parameter [%s] not defined. Defaulting to %.3f" % (name, default)
64 return default
65
66if __name__ == '__main__':
67 rospy.init_node('keys_to_twist')
68 g_last_twist_send_time = rospy.Time.now()
69 g_twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
70 rospy.Subscriber('keys', String, keys_cb)
71 g_target_twist = Twist() # initializes to zero
72 g_last_twist = Twist()
73 g_vel_scales[0] = fetch_param('~angular_scale', 0.1)
74 g_vel_scales[1] = fetch_param('~linear_scale', 0.1)
75 g_vel_ramps[0] = fetch_param('~angular_accel', 1.0)
76 g_vel_ramps[1] = fetch_param('~linear_accel', 1.0)
77
78 rate = rospy.Rate(20)
79 while not rospy.is_shutdown():
80 send_twist()
81 rate.sleep()
同样的，我们为这个文件添加可执行权限：
1$ chmod u+x keys_to_twist_with_ramps.py
就可以启动节点了：
1$ rosrun telebot keys_to_twist_with_ramps.py _linear_scale:=1.0 _angular_scale:=0.8 _linear_accel:=1.0 _angular_accel:=0.8
上面传入的参数是我们希望的小车运动速度和加速度比例，这样我们就可以看到有一个 /cmd_vel 的话题会输出期望的小车速度：
1$ rostopic echo /cmd_vel
2linear:
3 x: 1.0
4 y: 0.0
5 z: 0.0
6angular:
7 x: 0.0
8 y: 0.0
9 z: 0.0
10---
现在小车已经可以按照我们的指令慢慢启动，停下，转弯了，下一步就是给它加上远程摄像头。
3.ROS 摄像头
在和小车的摄像头连接之前，有一点非常重要，之前的操作都是在电脑上执行的，接下来我们要把自己的 ARM 开发板作为 ROS 主节点，所以需要设置环境变量，需要把下面的 IP 地址替换为小车上 ARM 板的实际 ip 地址：
1$ export ROS_MASTER_URI=http://your.armbian_ros.ip.address:11311
下面的代码都是在安装了 ROS 的小车上 ARM 开发板上操作的
我们先新建一个工作区间：
1$ mkdir telebot_ws && cd telebot_ws
2$ catkin_init_workspace
我们再新建一个 ROS 软件包：
1$ cd src
2$ catkin_create_pkg telebot_image roscpp
这样我们就可以开始 ROS 的开发了，在 telebot_ws 目录下：
1$ catkin_make
2$ source devel/setup.bash
3.1 发布摄像头消息
其实发布摄像头消息的代码也就 30 行左右，我们在 telebot_ws/src/telebot_image/src 目录下新建 my_publisher.cpp
1#include
2#include
3#include
4#include
5
6int main(int argc, char** argv)
7{
8 ros::init(argc, argv, "video_transp");
9 ros::NodeHandle nh;
10 image_transport::ImageTransport it(nh);
11 image_transport::Publisher pub = it.advertise("camera/image", 1);
12
13 cv::VideoCapture cap(0);
14 cv::Mat frame;
15 sensor_msgs::ImagePtr frame_msg;
16
17 ros::Rate rate(10);
18
19 while(ros::ok())
20 {
21 cap >> frame;
22 if (!frame.empty())
23 {
24 frame_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
25 pub.publish(frame_msg);
26 cv::waitKey(1);
27 }
28 ros::spinOnce();
29 rate.sleep();
30 }
31 return 0;
32}
在编译之前
需要先安装好 OpenCV 的开发环境
，因为我们是用 OpenCV 的库函数获取到摄像头数据，然后用 ROS 的库函数发布出去的，这是 telebot_ws/src/telebot_image 目录下的 CMakeLists.txt
1cmake_minimum_required(VERSION 2.8.3)
2project(telebot_image)
3
4find_package(catkin REQUIRED COMPONENTS
5 cv_bridge
6 image_transport
7)
8
9catkin_package(
10# INCLUDE_DIRS include
11# LIBRARIES my_image_transport
12# CATKIN_DEPENDS cv_bridge image_transport
13# DEPENDS system_lib
14)
15
16include_directories(
17 ${catkin_INCLUDE_DIRS}
18)
19
20find_package(OpenCV)
21include_directories(include ${OpenCV_INCLUDE_DIRS})
22#build my_publisher and my_subscriber
23add_executable(my_publisher src/my_publisher.cpp)
24target_link_libraries(my_publisher ${catkin_LIBRARIES} ${OpenCV_LIBS})
我们在 telebot_ws 目录下编译项目：
1$ catkin_make
这样就可以发布摄像头消息了，图像消息发布在 camera/image：
1$rosrun telebot_image my_publisher3.2 订阅摄像头消息
要订阅并看到摄像头消息其实非常简单：
1$ rosrun image_view image_view image:=/camera/image
就可以再电脑上看到小车摄像头的图片了。
4.结语
如果已经有一辆能够用 ROS 控制的小车，其实只需要写第 3 部分图像发布 30 行左右的代码，就可以用 OpenCV 库获取摄像头信息，然后用 ROS 库发布出去了。
当然，如果只是为了在电脑上看到小车的摄像头，其实还有其他很多甚至不用写代码的办法，用 ROS 发布图像数据的好处在于我们可以对获取到的图像进行处理，例如
目标检测
，这会在之后的文档里进一步介绍。
5.参考文献
ROS 机器人编程实践：https://github.com/osrf/rosbook
rosserial 软件包：https://github.com/wuhanstudio/rt-rosserial
RT-Thread线上/下活动
1、【RT-Thread开发者大会报名】2019年RT-Thread开发者大会将登入成都、上海、深圳与开发者们见面，还有RT-Thread在中高端智能领域的应用、一站式RTT开发工具、打造IoT极速开发模式等干货演讲，期待您的参与！本次大会也设立了codelab动手实验室活动，开发者可在现场体验RT-Thread给开发带来的便捷！
立即报名
2、RT-Thread能力认证考前线上培训，将于11月25日全线截止报名，如果您有晋升、求职、寻找更好机会的需要，有深入学习和掌握RT-Thread的需求，请尽快垂询/报考！学生优惠价：168/人
学生专属报名通道
能力认证官网链接：https://www.rt-thread.org/page/rac.html（在外部浏览器打开）
立即报名（非学生）
#题外话# 喜欢RT-Thread不要忘了在GitHub上留下你的
STAR哦，你的star对我们来说非常重要！链接地址：https://github.com/RT-Thread/rt-thread
你可以添加微信17775983565为好友，注明：公司+姓名，拉进 RT-Thread 官方微信交流群
RT-Thread
长按二维码，关注我们
点击“阅读原文”报名线下培训/开发者大会