无人驾驶虚拟仿真（三）--通过ROS系统查看仿真小车视频流

简介：在虚拟仿真平台的基础上实现ROS系统开发，将虚拟仿真平台中小车的视频流通过ROS系统的话题发布出来，然后用rqt_image_view工具播放实时图像。

1、创建工作空间

创建src文件，放置功能包源码：

$ mkdir -p ~/myros/catkin_ws/src

进入src文件夹：

$ cd ~/myros/catkin_ws/src

初始化文件夹：

$ catkin_init_workspace

后续需要编译源代码时，都需要在 catkin_ws 文件见下进行：

$ cd ~/myros/catkin_ws

$ catkin_make

编译完成后，工作空间目录下多出build和devel两个文件夹

每次编译完成后，需要执行source命令，才能将新功能加入到环境变量中：

$ source devel/setup.bash

---

2、安装duckietown_msgs

下载dt-ros-commons：

$ cd ~

$ git clone https://github.com/duckietown/dt-ros-commons.git

进入文件目录：

$ cd ~/dt-ros-commons/

拷贝duckietown_msgs文件夹至工作空间的src目录下：

$ cp -r packages/duckietown_msgs ~/myros/catkin_ws/src

返回工作空间目录：

$ cd ~/myros/catkin_ws/

编译：

$ catkin_make

编译完成后，加载devel/setup.bash到环境变量中：

$ source devel/setup.bash

查看ROS消息列表：

$ rosmsg list

---

3、创建duckiebot节点

进入工作空间源码目录：

$ cd ~/myros/catkin_ws/src/

创建功能包：

$ catkin_create_pkg duckiebot rospy std_msgs sensor_msgs duckietown_msgs

新建功能包源码文件：

$ touch duckiebot/src/duckiebot_node.py

新建配置文件：

$ mkdir -p duckiebot/config/duckiebot_node

$ touch duckiebot/config/duckiebot_node/default.yaml

新建启动文件：

$ mkdir -p duckiebot/launch

$ touch duckiebot/launch/duckiebot.launch

编辑配置文件并保存：

$ gedit duckiebot/config/duckiebot_node/default.yaml

内容如下：

env-name: Duckietown
map-name: small_loop
seed: 1
draw_curve: False
draw_bbox: False
domain_rand: False
frame_skip: 1
distortion: False
camera_rand: False
dynamics_rand: False
anonymous: False

如果要修改地图，可以修改map-name配置项

编辑编译配置文件并保存：

$ gedit duckiebot/CMakeLists.txt

修改为：

编辑启动文件：

$ gedit duckiebot/launch/duckiebot.launch

内容如下：

<launch>
	<arg name="veh" value="duckietown"/>
	<arg name="pkg_name" value="duckiebot"/>
	<arg name="node_name" default="duckiebot_node"/>
	<arg name="param_file_name" default="default" doc="Specify a param file. ex:megaman"/>
	<arg name="required" default="false" />

	<group ns="$(arg veh)">
		<node name="duckiebot_node" pkg="$(arg pkg_name)" type="$(arg node_name).py" respawn="true" 	respawn_delay="10" output="screen" required="$(arg required)">
			<rosparam command="load" file="$(find duckiebot)/config/$(arg node_name)/$(arg param_file_name).yaml"/>
		</node>
	</group>
</launch>

编辑源码文件：

$ gedit duckiebot/src/duckiebot_node.py

主要功能包括读取系统配置文件，启动虚拟仿真环境，发布图像话题，订阅控制和急停topic并实现处理函数。

附duckiebot_node.py源码：

#!/usr/bin/env python3

import rospy
import cv2
import numpy as np
import pyglet
from PIL import Image as PILImage
from pyglet.window import key

from duckietown_msgs.msg import Twist2DStamped, BoolStamped
from sensor_msgs.msg import CompressedImage
from cv_bridge import CvBridge

from gym_duckietown.envs import DuckietownEnv

class Duckiebot():
    #初始化节点
    def __init__(self):
        rospy.init_node('duckiebot_node', anonymous=rospy.get_param("/duckietown/duckiebot_node/anonymous", default=False))
        #环境名称，暂时只有Duckietown一重环境
        env_name = rospy.get_param("/duckietown/duckiebot_node/env-name")
        #地图名称，默认为udem1，需要在配置文件中指定
        map_name = rospy.get_param("/duckietown/duckiebot_node/map-name")
        if env_name and env_name.find("Duckietown") != -1:
            self.env_name = env_name
        else:
            self.env_name = 'Duckietown-udem1-v0'
        if map_name:
            self.map_name = map_name
        else:
            self.map_name = 'udem1'
        self.env = DuckietownEnv(
            seed=rospy.get_param("/duckietown/duckiebot_node/seed"),
            map_name=self.map_name,
            draw_curve=rospy.get_param("/duckietown/duckiebot_node/draw_curve", default=False),
            draw_bbox=rospy.get_param("/duckietown/duckiebot_node/draw_bbox", default=False),
            domain_rand=rospy.get_param("/duckietown/duckiebot_node/domain_rand", default=False),
            frame_skip=rospy.get_param("/duckietown/duckiebot_node/frame_skip", default=1),
            distortion=rospy.get_param("/duckietown/duckiebot_node/distortion", default=False),
            camera_rand=rospy.get_param("/duckietown/duckiebot_node/camera_rand", default=False),
            dynamics_rand=rospy.get_param("/duckietown/duckiebot_node/dynamics_rand", default=False)
        )
        self.env.reset()
        #小车控制参数
        self.action = np.array([0.0, 0.0])
        self.bridge = CvBridge()
        self.estop = False
        #图像发布话题
        self.pub_img = rospy.Publisher('~image/compressed', CompressedImage, queue_size=10)
		#订阅车辆控制指令话题
        self.sub_topic = rospy.Subscriber("~car_cmd",Twist2DStamped,self.car_cmd_cb,queue_size=1)
        #订阅车辆急停指令话题
        self.sub_e_stop = rospy.Subscriber("~emergency_stop",BoolStamped,self.estop_cb,queue_size=1)
    #虚拟环境状态更新函数
    def update(self, dt):
        obs, reward, done, info = self.env.step(self.action)
        image = PILImage.fromarray(obs)
        image = np.array(image)
        image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
        self.pub_img.publish(self.bridge.cv2_to_compressed_imgmsg(image, dst_format='jpeg'))
        if done:
            self.env.reset()
        #self.env.render()
    #车辆控制指令处理函数
    def car_cmd_cb(self, msg):
        if self.estop:
            v = 0.0
            omega = 0.0
        else:
            v = msg.v
            omega = msg.omega

        self.action = np.array([v, omega])
    # 车辆急停指令处理函数
    def estop_cb(self, msg):
        self.estop = msg.data
        if self.estop:
            print("Emergency Stop Activated")
        else:
            print("Emergency Stop Released")
    
    def start(self):
        pyglet.clock.schedule_interval(self.update, 1.0 / self.env.unwrapped.frame_rate)
        pyglet.app.run()
        self.env.close()

if __name__ == '__main__':  
    duckiebot = Duckiebot()
    duckiebot.start()

返回工作空间目录：

$ cd ../

重新编译源码：

$ catkin_make

更新环境变量并启动duckiebot节点：

$ source devel/setup.bash

$ roslaunch duckiebot duckiebot.launch

---

4、通过ros工具查看视频流

打开一个新的终端，设置环境变量

$ source myros/catkin_ws/devel/setup.bash

打开ros图像浏览工具：

$ rqt_image_view

选择duckiebot_node节点发布的图像消息即可浏览到实时视频

当前小车未接收到控制命令，为静止状态，所以图像是静止的，每隔一段时间会重置更新一次。