简介:在上一节中,我们从原理上完成了交通标志牌的检测,在这一节中,我们将检测功能部署到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