开源|智能车目标识别系统连载教程—RT-Thread连接ROS摄像头小车控制(6)

__Shirley 2021-08-10 14:03:32 阅读数:898

本文一共[544]字,预计阅读时长:1分钟~
ios github 目标识别 开发者大会 #include

 

引言

这篇文档会介绍如何用 RT-Thread 和 ROS 连接实现一个带摄像头的远程控制小车。 watermark,size_16,text_QDUxQ1RP5Y2a5a6i,color_FFFFFF,t_100,g_se,x_10,y_10,shadow_90,type_ZmFuZ3poZW5naGVpdGk= 不过其实 RT-Thread 部分的代码已经在这篇文档里面介绍了: RT-Thread 连接 ROS 小车控制 ,在这个基础上,我们只需要修改 ROS 的代码就可以了。 这里先把整个系统框图画出来,这样如果想要自己做一辆这样的小车也可以动手试一试: watermark,size_16,text_QDUxQ1RP5Y2a5a6i,color_FFFFFF,t_100,g_se,x_10,y_10,shadow_90,type_ZmFuZ3poZW5naGVpdGk=

实物图看起来就是这样:

watermark,size_16,text_QDUxQ1RP5Y2a5a6i,color_FFFFFF,t_100,g_se,x_10,y_10,shadow_90,type_ZmFuZ3poZW5naGVpdGk=

2.ROS平滑运动

 

2.1 ROS 工作环境

 

下面的代码都是在安装了 ROS 的电脑上操作的

 

ROS 的安装之前已经介绍过了,这里就不重复了,我们先新建一个工作区间:
1$ mkdir telebot_ws && cd telebot_ws
2$ catkin_init_workspace
  • 1.
  • 2.

 

我们再新建一个 ROS 软件包:

1$ cd src
2$ catkin_create_pkg telebot rospy

  • 1.
  • 2.

 

这样我们就可以开始 ROS 的开发了,在 telebot_ws 目录下:
1$ catkin_make
2$ source devel/setup.bash

  • 1.
  • 2.

 

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)

  • 1.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 7.
  • 8.
  • 9.
  • 10.
  • 11.
  • 12.
  • 13.
  • 14.
  • 15.
  • 16.
  • 17.
  • 18.
  • 19.
  • 20.
  • 21.
  • 22.
  • 23.
  • 24.
  • 25.
  • 26.
  • 27.

上面的代码不到 20 行,我也添加了一些注释,就不详细介绍了,我们为这个文件添加可执行权限:

1$ chmod u+x key_publisher.py

  • 1.
就可以启动节点了:
1$ rosrun telebot key_publisher.py

  • 1.
这样我们就可以看到有一个 /keys 的话题会不断输出键盘按键:
1$ rostopic echo /keys
2data: "w"
3---
4data: "a"
5---
6data: "s"
7---

  • 1.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 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.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 7.
  • 8.
  • 9.
  • 10.
  • 11.
  • 12.
  • 13.
  • 14.
  • 15.
  • 16.
  • 17.
  • 18.
  • 19.
  • 20.
  • 21.
  • 22.
  • 23.
  • 24.
  • 25.
  • 26.
  • 27.
  • 28.
  • 29.
  • 30.
  • 31.
  • 32.
  • 33.
  • 34.
  • 35.
  • 36.
  • 37.
  • 38.
  • 39.
  • 40.
  • 41.
  • 42.
  • 43.
  • 44.
  • 45.
  • 46.
  • 47.
  • 48.
  • 49.
  • 50.
  • 51.
  • 52.
  • 53.
  • 54.
  • 55.
  • 56.
  • 57.
  • 58.
  • 59.
  • 60.
  • 61.
  • 62.
  • 63.
  • 64.
  • 65.
  • 66.
  • 67.
  • 68.
  • 69.
  • 70.
  • 71.
  • 72.
  • 73.
  • 74.
  • 75.
  • 76.
  • 77.
  • 78.
  • 79.
  • 80.
  • 81.

同样的,我们为这个文件添加可执行权限:

1$ chmod u+x keys_to_twist_with_ramps.py

  • 1.

 

就可以启动节点了:
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

  • 1.

上面传入的参数是我们希望的小车运动速度和加速度比例,这样我们就可以看到有一个 /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---

  • 1.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 7.
  • 8.
  • 9.
  • 10.

 

现在小车已经可以按照我们的指令慢慢启动,停下,转弯了,下一步就是给它加上远程摄像头。

3.ROS 摄像头

在和小车的摄像头连接之前,有一点非常重要,之前的操作都是在电脑上执行的,接下来我们要把自己的 ARM 开发板作为 ROS 主节点,所以需要设置环境变量,需要把下面的 IP 地址替换为小车上 ARM 板的实际 ip 地址
1$ export ROS_MASTER_URI=http://your.armbian_ros.ip.address:11311

  • 1.
下面的代码都是在安装了 ROS 的小车上 ARM 开发板上操作的   我们先新建一个工作区间:
1$ mkdir telebot_ws && cd telebot_ws
2$ catkin_init_workspace

  • 1.
  • 2.
我们再新建一个 ROS 软件包:
1$ cd src
2$ catkin_create_pkg telebot_image roscpp

  • 1.
  • 2.

这样我们就可以开始 ROS 的开发了,在 telebot_ws 目录下:

1$ catkin_make
2$ source devel/setup.bash

  • 1.
  • 2.

 

3.1 发布摄像头消息

其实发布摄像头消息的代码也就 30 行左右,我们在 telebot_ws/src/telebot_image/src 目录下新建 my_publisher.cpp

 1#include <ros/ros.h>
2#include <image_transport/image_transport.h>
3#include <opencv2/opencv.hpp>
4#include <cv_bridge/cv_bridge.h>
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}

  • 1.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 7.
  • 8.
  • 9.
  • 10.
  • 11.
  • 12.
  • 13.
  • 14.
  • 15.
  • 16.
  • 17.
  • 18.
  • 19.
  • 20.
  • 21.
  • 22.
  • 23.
  • 24.
  • 25.
  • 26.
  • 27.
  • 28.
  • 29.
  • 30.
  • 31.
  • 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})

  • 1.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 7.
  • 8.
  • 9.
  • 10.
  • 11.
  • 12.
  • 13.
  • 14.
  • 15.
  • 16.
  • 17.
  • 18.
  • 19.
  • 20.
  • 21.
  • 22.
  • 23.
  • 24.

我们在 telebot_ws 目录下编译项目:

1$ catkin_make

  • 1.

这样就可以发布摄像头消息了,图像消息发布在 camera/image:

1$rosrun telebot_image my_publisher
  • 1.

3.2 订阅摄像头消息

watermark,size_16,text_QDUxQ1RP5Y2a5a6i,color_FFFFFF,t_100,g_se,x_10,y_10,shadow_90,type_ZmFuZ3poZW5naGVpdGk=

要订阅并看到摄像头消息其实非常简单:
1$ rosrun image_view image_view image:=/camera/image

  • 1.
就可以再电脑上看到小车摄像头的图片了。 watermark,size_16,text_QDUxQ1RP5Y2a5a6i,color_FFFFFF,t_100,g_se,x_10,y_10,shadow_90,type_ZmFuZ3poZW5naGVpdGk=

4.结语

如果已经有一辆能够用 ROS 控制的小车,其实只需要写第 3 部分图像发布 30 行左右的代码,就可以用 OpenCV 库获取摄像头信息,然后用 ROS 库发布出去了。

 

当然,如果只是为了在电脑上看到小车的摄像头,其实还有其他很多甚至不用写代码的办法,用 ROS 发布图像数据的好处在于我们可以对获取到的图像进行处理,例如 目标检测 ,这会在之后的文档里进一步介绍。

5.参考文献

  • ROS 机器人编程实践:https://github.com/osrf/rosbook

  • rosserial 软件包:https://github.com/wuhanstudio/rt-rosserial

 

watermark,size_16,text_QDUxQ1RP5Y2a5a6i,color_FFFFFF,t_100,g_se,x_10,y_10,shadow_90,type_ZmFuZ3poZW5naGVpdGk=

watermark,size_14,text_QDUxQ1RP5Y2a5a6i,color_FFFFFF,t_100,g_se,x_10,y_10,shadow_20,type_ZmFuZ3poZW5naGVpdGk=

版权声明:本文为[__Shirley]所创,转载请带上原文链接,感谢。 https://blog.51cto.com/u_15288275/3338206