简介：ROS2功能的学习我们还是在基于OpenAI的gym虚拟仿真环境中来完成，gym虚拟仿真环境安装请参考另一篇教程，这里不再重复说明，接下来我们开始创建一个ROS2的功能节点，并发布虚拟仿真环境小车摄像头的图像，然后利用rqt工具查看图像。

1、创建工作空间

与ROS1类似，ROS2也有工作空间的概念，所谓工作空间，实际就是一个目录结构按约定搭建的文件夹（我们这里的工作空间是指ros2_ws），ROS2的工作空间只需要提前创建src目录（功能包目录）即可，不需要初始化：

$ mkdir -p ~/ros2_ws/src

后续所有的功能包都在src目录下创建，编译要回到ros2_ws目录下，编译后,在工作空间内生成build、install、log目录。

2、创建功能包

进入功能包目录src

$ cd ros2_ws/src/

新建duckiebot功能包

$ ros2 pkg create duckiebot --build-type ament_python --node-name duckiebot_node --dependencies rclpy std_msgs sensor_msgs

创建完成后，目录结构如下图：

duckiebot_node.py就是自动生成的源码文件，内容如下：

def main():
    print('Hi from duckiebot.')


if __name__ == '__main__':
    main()

3、编译并运行

ROS2编译需要用colcon工具，默认是没有安装的，需要手动安装：

$ sudo apt-get install python3-colcon-common-extensions -y

返回工作空间

$ cd .. 或者 $ cd ~/ros2_ws

编译所有功能包：

$ colcon build

为了提高工作效率，也可以指定要编译的功能包：

$ colcon build --packages-select duckiebot

编译后工作空间的目录结构：

设置环境变量：

$ source install/setup.bash

运行节点：

$ ros2 run duckiebot duckiebot_node

屏幕上输出：

Hi from duckiebot.

4、配置文件

上文提到在创建功能包时一些配置项可以手动配置，主要的配置项都在功能包内的setup.py和package.xml文件内：

setup.py内主要是指定源码文件及启动函数：

package.xml指定了编译方式和依赖项：

5、修改源码，本地显示虚拟仿真图像

修改duckiebot_node.py，加入虚拟仿真环境启动和图像显示功能：

#!/usr/bin/env python3

import rclpy #ROS2的python实现库
from rclpy.node import Node  #节点基类，所有节点都要继承Node类

import gym  #虚拟仿真环境基础实现库
from pyglet.window import key  #python动画GUI库
from gym_duckietown.envs import DuckietownEnv  #环境模型

import cv2
import numpy as np

class DuckiebotNode(Node):

    def __init__(self, name):
        super().__init__(name)
        #初始化虚拟仿真环境参数，如果要切换地图，修改map_name的值
        self.env = DuckietownEnv(seed=1,map_name="udem1",draw_curve=False,draw_bbox=False,domain_rand=False,frame_skip=1,distortion=False,camera_rand=False,dynamics_rand=False)
        #初始化虚拟仿真环境
        self.env.reset()
        #定义一个定时器，通过定时器回调函数控制图像更新
        self.timer = self.create_timer(0.05, self.timer_callback)
    #定时器回调函数
    def timer_callback(self):
        #创建一个随机的action，让小车随机运动，
        #action是一个二元数组，两个元素分别代表小车的线速度和角速度
        #也可以自行指定action的值,例如：action = np.array([1,0])
        action = self.env.action_space.sample()
        #执行给定的动作，并返回当前状态
        #obs 是当前状态的图像数据
        #reward 是模拟器对小车当前状态计算出的奖励分数，可用来判断车辆状态好坏（强化学习中可以使用）
        #done 小车运行状态，出轨（行驶到禁止区域）、车祸（撞击到障碍物）、完成行驶任务（默认是1500次为一轮）返回true
        #info 小车当前状态的其他参数，可能会用到的是omega_r和omega_l，模拟小车的左右轮角速度
        obs, reward, done, info = self.env.step(action)
        #obs是rgb编码，opencv默认是bgr编码，转化颜色编码方式
        image = cv2.cvtColor(obs, cv2.COLOR_RGB2BGR)
        #显示图像
        cv2.imshow("image",image)
        cv2.waitKey(1)
        #出轨、车祸、完成任务时重置仿真环境
        if done:
            self.env.reset()   

def main(args=None):
    rclpy.init(args=args) #初始化客户端
    node = DuckiebotNode(name="duckiebot_node") #创建节点实例
    rclpy.spin(node=node)  #运行节点实例
    rclpy.shutdown() #关闭节点

重新编译（每次修改源码文件，都需要重新编译）

$ colcon build --packages-select duckiebot

运行节点（在没有重启终端或者改变节点数量的情况下，可以不执行source命令）

$ ros2 run duckiebot duckiebot_node

正常情况下，会弹出图像窗口，显示一个随机运动的小车视角画面：

6、发布图像，并通过工具查看

上文我们在本地显示了仿真图像，接下来我们把仿真图像通过一个话题发布出去，并使用ROS的GUI工具查看图像：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node

import gym
from pyglet.window import key
from gym_duckietown.envs import DuckietownEnv

import cv2

from sensor_msgs.msg import Image  #发布图像使用Image消息类型
from cv_bridge import CvBridge #opencv和ros图像数据转换工具

class DuckiebotNode(Node):

    def __init__(self, name):
        super().__init__(name)
        self.env = DuckietownEnv(seed=1,map_name="udem1",draw_curve=False,draw_bbox=False,domain_rand=False,frame_skip=1,distortion=False,camera_rand=False,dynamics_rand=False)
        self.env.reset()
        #定义图像发布接口
        self.pub_img = self.create_publisher(Image,"duckiebot_node/image",10)
        self.timer = self.create_timer(0.05, self.timer_callback)
        #创建图像转换工具
        self.bridge = CvBridge()

    def timer_callback(self):
        action = self.env.action_space.sample()
        obs, reward, done, info = self.env.step(action)
        #发布图像数据，obs是rgb编码，转化时指定编码，解码时就有据可查
        self.pub_img.publish(self.bridge.cv2_to_imgmsg(obs, 'rgb8'))
        if done:
            self.env.reset()

def main(args=None):
    rclpy.init(args=args)
    node = DuckiebotNode(name="duckiebot_node")
    rclpy.spin(node=node)
    rclpy.shutdown()

重新编译并运行节点

$ colcon build --packages-select duckiebot
$ ros2 run duckiebot duckiebot_node

新开终端，运行rqt命令，打开GUI工具

$ rqt

在窗口中通过Plugins—>Visualization—>Image View打开图像显示工具，并选择我们发布的主题/duckiebot_node/image，就可以看到实时的图像了。