简介：在上一节中，我们完成了障碍物检测模型训练以及python部署，在这一节中，我们将障碍物识别部署到ROS系统中。

新建功能包

$ catkin_create_pkg objects_detect rospy std_msgs sensor_msgs

新建配置文件

$ mkdir -p objects_detect/config/objects_detect_node
$ touch objects_detect/config/objects_detect_node/default.yaml

新建启动脚本文件

 $ mkdir -p objects_detect/launch
 $ touch objects_detect/launch/start.launch

新建模型目录并拷贝训练好的模型到目录下

$ mkdir -p objects_detect/model
$ cp -r ~/PaddleDetection/output_inference/yolov3_mobilenet_v1_duckietown/.* ~/myros/catkin_ws/src/objects_detect/model/

新建paddle发布工具包目录并拷贝工具包到目录下

    $ mkdir -p objects_detect/include/paddle_tools
    $ cp -r ~/PaddleDetection/deploy/python/*.py ~/myros/catkin_ws/src/objects_detect/include/paddle_tools/

新建源码文件

$ touch objects_detect/src/objects_detect_node.py

新建工具包安装文件，并编辑保存

    $ touch obiects_detect/setup.py
    $ gedit objects_detect/setup.py

    from distutils.core import setup
    from catkin_pkg.python_setup import generate_distutils_setup
    d = generate_distutils_setup(
        packages=['paddle_tools'],
        package_dir={'': 'include'}
    )
    setup(**d)

修改编译配置文件

$ gedit objects_detect/CMakeLists.txt

修改为：

去掉注释符号，修改为

编辑启动脚本文件

$ gedit objects_detect/launch/start.launch

<launch>
  <arg name="veh"/>
  <arg name="pkg_name" value="objects_detect"/>
  <arg name="node_name" value="objects_detect_node"/>
  <arg name="param_file_name" default="default" doc="Specify a param file. ex:megaman"/>
  <arg name="required" default="false" />
 
  <group ns="$(arg veh)">
    <remap from="objects_detect_node/image/compressed" to="duckiebot_node/image/compressed"/>
    <node name="$(arg node_name)" pkg="$(arg pkg_name)" type="$(arg node_name).py" respawn="true" respawn_delay="10" output="screen" required="$(arg required)">
      <rosparam command="load" file="$(find objects_detect)/config/$(arg node_name)/$(arg param_file_name).yaml"/>
    </node>
  </group>
</launch>

编辑源码

$ gedit objects_detect/src/objects_detect_node.py

#!/usr/lib/env python3
 
import rospy
import cv2
import numpy as np
import math
 
import os
import yaml
from functools import reduce
from paddle.inference import Config
from paddle.inference import create_predictor
from paddle_tools.preprocess import Resize, NormalizeImage, Permute
from paddle_tools.visualize import visualize_box_mask
 
from cv_bridge import CvBridge
 
from std_msgs.msg import Bool
from sensor_msgs.msg import Image,CompressedImage
 
class ObjectsDetectNode():
    def __init__(self):
        rospy.init_node("objects_detect_node", anonymous=False)
        self.model_dir = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))),"model")
        deploy_file = os.path.join(self.model_dir, 'infer_cfg.yml')
        with open(deploy_file) as f:
            yml_conf = yaml.safe_load(f)
        preprocess_infos = yml_conf['Preprocess']
        self.preprocess_ops = []
        for op_info in preprocess_infos:
            new_op_info = op_info.copy()
            op_type = new_op_info.pop('type')
            self.preprocess_ops.append(eval(op_type)(**new_op_info))
        #读取标签列表
        self.labels = yml_conf['label_list']
        self.threshold = 0.5
        #预测器配置   
        self.config = Config(os.path.join(self.model_dir, 'model.pdmodel'),os.path.join(self.model_dir, 'model.pdiparams'))
        self.config.disable_gpu() #禁用GPU
        self.config.set_cpu_math_library_num_threads(1) #设置线程数
        self.config.disable_glog_info() #禁用识别过程日志输出
        self.config.enable_memory_optim() #启用内存共享功能
        self.config.switch_use_feed_fetch_ops(False) #禁用feed_fetch_ops功能
        self.count = 0
        #加载预测器
        self.predictor = create_predictor(self.config)
        
        self.bridge = CvBridge()
        rospy.Subscriber("~image/compressed", CompressedImage, self.cb_image)
    
    def cb_image(self, msg):
        if self.count<10: #cpu版本处理速度较慢，我们这里每隔10帧识别一张
            self.count += 1
            return
        else:
            self.count = 0   
        image = self.bridge.compressed_imgmsg_to_cv2(msg)
        
        inputs = self.preprocess(image)
        np_boxes = self.preditc(inputs)
        if reduce(lambda x, y: x * y, np_boxes.shape) < 6:
            print('[WARNNING] No object detected.')
        else:
            results = {}
            results['boxes'] = np_boxes
            image = self.draw_box(image, results)
        cv2.imshow("result", image)
        cv2.waitKey(1)
    
    def preprocess(self, image):
        im_info = {'scale_factor': np.array([1., 1.], dtype=np.float32),'im_shape': None,}
        im = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
        im_info['im_shape'] = np.array(im.shape[:2], dtype=np.float32)
        im_info['scale_factor'] = np.array([1., 1.], dtype=np.float32)
        for operator in self.preprocess_ops:
            im, im_info = operator(im, im_info)
        #创建输入数据
        inputs = {}
        inputs['image'] = np.array((im, )).astype('float32')
        inputs['im_shape'] = np.array((im_info['im_shape'], )).astype('float32')
        inputs['scale_factor'] = np.array((im_info['scale_factor'], )).astype('float32')
        return inputs
    
    def preditc(self, inputs):
        np_boxes, np_masks = None, None
        # 获取输入的向量名称
        input_names = self.predictor.get_input_names()
        # 根据向量名称获取输入向量
        # 拷贝数据到输入向量中
        for i in range(len(input_names)):
            input_tensor = self.predictor.get_input_handle(input_names[i])
            input_tensor.copy_from_cpu(inputs[input_names[i]])
        #使用预测器执行前向计算  
        self.predictor.run()
        # 获取输出的向量名称
        output_names = self.predictor.get_output_names()
        # 根据向量名称获取输出向量
        # 将结果从输出向量中拷贝出来
        boxes_tensor = self.predictor.get_output_handle(output_names[0])
        #识别结果，二维数组，每一行第1个元素是结果类别编号，第2个是可信度，后4个是坐标，左上和右下两个点定位一个矩形
        np_boxes = boxes_tensor.copy_to_cpu()
        return np_boxes
        
    def draw_box(self, image, results):
        image = visualize_box_mask(image, results, self.labels, self.threshold)
        image = np.array(image)
        return image
 
if __name__=='__main__':
    node = ObjectsDetectNode()
    rospy.spin()

编译并配置环境变量

$ cd ~/myros/catkin_ws
$ catkin_make
$ source devel/setup.bash

修改多节点启动脚本 

$ gedit start.launch

<launch>
  <arg name="veh" default="duckiebot"/>
  <group>
    <include file="$(find duckiebot)/launch/duckiebot.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find anti_instagram)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find line_detect)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find lane_filter)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find car_control)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find objects_detect)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
  </group>
</launch>

启动程序，运行效果如下图

$ roslaunch start.launch