无人驾驶虚拟仿真（十四）--图像处理之交通标志牌识别2

        简介：在上一节中，我们从原理上完成了交通标志牌的检测，在这一节中，我们将检测功能部署到ROS系统中。

新建功能包

$ catkin_create_pkg tags_detect rospy std_msgs sensor_msgs

新建配置文件

$ mkdir -p tags_detect/config/tags_detect_node
$ touch tags_detect/config/tags_detect_node/default.yaml

新建启动脚本文件

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

新建源码文件

$ touch tags_detect/src/tags_detect_node.py
$ gedit tags_detect/CMakeLists.txt

修改为：

编辑启动脚本文件

$ gedit tags_detect/launch/start.launch

<launch>
  <arg name="veh"/>
  <arg name="pkg_name" value="tags_detect"/>
  <arg name="node_name" value="tags_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="tags_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 tags_detect)/config/$(arg node_name)/$(arg param_file_name).yaml"/>
    </node>
  </group>
</launch>

编辑源码

$ gedit tags_detect/src/tags_detect_node.py

#!/usr/lib/env python3

import rospy
import cv2
import numpy as np
from dt_apriltags import Detector
from cv_bridge import CvBridge

from std_msgs.msg import Bool
from sensor_msgs.msg import CompressedImage

class TagsDetectNode():
    def __init__(self):
        rospy.init_node("tags_detect_node", anonymous=False)
        self.at_detector = Detector(families='tag36h11',
                       nthreads=1,
                       quad_decimate=1.0,
                       quad_sigma=0.0,
                       refine_edges=1,
                       decode_sharpening=0.25,
                       debug=0)
        self.matrix = np.array([[397.06923648, 0, 319.53462827], [0, 409.40521919, 246.53066753], [0, 0, 1]]) 
        self.bridge = CvBridge()
        rospy.Subscriber("~image/compressed", CompressedImage, self.cb_image)
    
    def cb_image(self, msg):
        image = self.bridge.compressed_imgmsg_to_cv2(msg)
        tags = self.detect(image)
        box_image = self.draw_box(image, tags)
        cv2.imshow("box_image", box_image)
        cv2.waitKey(1)
    
    def detect(self, image):
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        tags = self.at_detector.detect(gray, True, (self.matrix[0][0],self.matrix[1][1],self.matrix[0][2],self.matrix[1][2]), 0.06)
        return tags
    
    def draw_box(self, image, tags):
        index = 0
        for tag in tags:
            cv2.line(image,tag.corners[0].astype(int),tag.corners[1].astype(int),(0,255,0))
            cv2.line(image,tag.corners[1].astype(int),tag.corners[2].astype(int),(0,255,0))
            cv2.line(image,tag.corners[2].astype(int),tag.corners[3].astype(int),(0,255,0))
            cv2.line(image,tag.corners[3].astype(int),tag.corners[0].astype(int),(0,255,0))
            cv2.putText(image, str(tag.tag_id)+","+str(tag.pose_t[2][0]), (20,20+(30*index)), cv2.FONT_HERSHEY_COMPLEX, 1.0, (0, 0, 255), 1)
            index += 1
        return image
        
if __name__=='__main__':
    node = TagsDetectNode()
    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 tags_detect)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
  </group>
</launch>

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

$ roslaunch start.launch