Diego1# 机器视觉 -AR标签跟随

更多创客作品，请关注笔者网站园丁鸟，搜集全球极具创意，且有价值的创客作品
ROS机器人知识请关注，diegorobot
业余时间完成的一款在线统计过程分析工具SPC，及SPC知识分享网站qdo

---

ROS里面有一个非常好用的AR标签包，可以产生AR标签，识别AR标签。我们可以基于此功能实现很多好玩的AR应用，这篇文章中我们将介绍如何使用这个包，及基于此包我们实现AR标签的跟随。
1.安装ar_track_alvar
ar_track_alvar可以通过apt_get来安装，非常方便：

sudo apt-get install ros-kinetic-ar-track-alvar

安装完成后暨在/opt/ros/kinetic/share/目录下有ar_track_alvar的目录，而且在launch子目录下已经有了launch文件。

如果你ROS一开始就是安装的完整版本的话，这个包就已经安装好了，不需要单独安装。
2.生成ar tag标签
ar_track_alvar提供的ar标签的生成功能，我们首先进入存放标签文件的目录

cd ~/diego1/src/diego_ar_tags/data

接下来执行执行生成标签的命令，就会在此文件夹下生成对应的标签，一条命令只生成一个标签

rosrun ar_track_alvar createMarker 0
rosrun ar_track_alvar createMarker 1
rosrun ar_track_alvar createMarker 2
rosrun ar_track_alvar createMarker 3
rosrun ar_track_alvar createMarker 4

这时候我们查看目录就会看到生成的ar文件，是png格式的图片

双击就可以打开文件

可以把标签答应出来，在跟随的时候使用。
3.ar标签跟随
3.1 源代码
下面是ar_follower.py的源文件，是从rbx2的项目中提取出来的，可以直接使用

#!/usr/bin/env python

"""
    ar_follower.py - Version 1.0 2013-08-25
    
    Follow an AR tag published on the /ar_pose_marker topic.  The /ar_pose_marker topic
    is published by the ar_track_alvar package
    
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2013 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
"""

import rospy
from ar_track_alvar_msgs.msg import AlvarMarkers
from geometry_msgs.msg import Twist
from math import copysign

class ARFollower():
    def __init__(self):
        rospy.init_node("ar_follower")
                        
        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        # How often should we update the robot's motion?
        self.rate = rospy.get_param("~rate", 10)
        r = rospy.Rate(self.rate) 
        
        # The maximum rotation speed in radians per second
        self.max_angular_speed = rospy.get_param("~max_angular_speed", 2.0)
        
        # The minimum rotation speed in radians per second
        self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.5)
        
        # The maximum distance a target can be from the robot for us to track
        self.max_x = rospy.get_param("~max_x", 20.0)
        
        # The goal distance (in meters) to keep between the robot and the marker
        self.goal_x = rospy.get_param("~goal_x", 0.6)
        
        # How far away from the goal distance (in meters) before the robot reacts
        self.x_threshold = rospy.get_param("~x_threshold", 0.05)
        
        # How far away from being centered (y displacement) on the AR marker
        # before the robot reacts (units are meters)
        self.y_threshold = rospy.get_param("~y_threshold", 0.05)
        
        # How much do we weight the goal distance (x) when making a movement
        self.x_scale = rospy.get_param("~x_scale", 0.5)

        # How much do we weight y-displacement when making a movement        
        self.y_scale = rospy.get_param("~y_scale", 1.0)
        
        # The max linear speed in meters per second
        self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3)
        
        # The minimum linear speed in meters per second
        self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.1)

        # Publisher to control the robot's movement
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        
        # Intialize the movement command
        self.move_cmd = Twist()
        
        # Set flag to indicate when the AR marker is visible
        self.target_visible = False
        
        # Wait for the ar_pose_marker topic to become available
        rospy.loginfo("Waiting for ar_pose_marker topic...")
        rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
        
        # Subscribe to the ar_pose_marker topic to get the image width and height
        rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.set_cmd_vel)
        
        rospy.loginfo("Marker messages detected. Starting follower...")
        
        # Begin the cmd_vel publishing loop
        while not rospy.is_shutdown():
            # Send the Twist command to the robot
            self.cmd_vel_pub.publish(self.move_cmd)
            
            # Sleep for 1/self.rate seconds
            r.sleep()

    def set_cmd_vel(self, msg):
        # Pick off the first marker (in case there is more than one)
        try:
            marker = msg.markers[0]
            if not self.target_visible:
                rospy.loginfo("FOLLOWER is Tracking Target!")
            self.target_visible = True
        except:
            # If target is loar, stop the robot by slowing it incrementally
            self.move_cmd.linear.x /= 1.5
            self.move_cmd.angular.z /= 1.5
            
            if self.target_visible:
                rospy.loginfo("FOLLOWER LOST Target!")
            self.target_visible = False
            
            return
                
        # Get the displacement of the marker relative to the base
        target_offset_y = marker.pose.pose.position.y
        rospy.loginfo("target_offset_y"+str(target_offset_y))
        
        # Get the distance of the marker from the base
        target_offset_x = marker.pose.pose.position.x
        rospy.loginfo("target_offset_x"+str(target_offset_x))
        
        # Rotate the robot only if the displacement of the target exceeds the threshold
        if abs(target_offset_y) > self.y_threshold:
            # Set the rotation speed proportional to the displacement of the target
            speed = target_offset_y * self.y_scale
            self.move_cmd.angular.z = copysign(max(self.min_angular_speed,
                                        min(self.max_angular_speed, abs(speed))), speed)
        else:
            self.move_cmd.angular.z = 0.0
 
        # Now get the linear speed
        if abs(target_offset_x - self.goal_x) > self.x_threshold:
            speed = (target_offset_x - self.goal_x) * self.x_scale
            if speed < 0:
                speed *= 1.5
            self.move_cmd.linear.x = copysign(min(self.max_linear_speed, max(self.min_linear_speed, abs(speed))), speed)
        else:
            self.move_cmd.linear.x = 0.0

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)     

if __name__ == '__main__':
    try:
        ARFollower()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("AR follower node terminated.")

3.2代码解释
这段代码的原理其实非常简单，就是调用ar_track_alvar发布的/ar_pose_marker主题，然后将该主题的信息转换为cmd_vel主题的控制信息来控制机器人运动。我们先来看下/ar_pose_maker主题

上图执行rostopic echo /ar_pose_marker命令的截图，从中可以看出此topic中包含着一个Pose类型的消息，我们只需要处理position部分的x，y，z的值就可以了，其中x就是机器人到ar 标签的距离，y值就是相对于与摄像头中心位置的偏移，理解了ar_pose_marker我们就很容易理解代码的原理，这里就不多做叙述了。
3.3 launch文件
ar_large_markers_xition.launch文件启动ar_track_alvar node

<launch>
     <arg name="marker_size" default="12.5" /> 
     <arg name="max_new_marker_error" default="0.08" />
     <arg name="max_track_error" default="0.4" />
     <arg name="cam_image_topic" default="/camera/depth_registered/points" />
     <arg name="cam_info_topic" default="/camera/rgb/camera_info" /> 
     <arg name="output_frame" default="/base_link" />
     <arg name="debug" default="false" />
     <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
     <arg unless="$(arg debug)" name="launch_prefix" value="" />
     <node pkg="tf" type="static_transform_publisher" name="base_link_2_camera_link" args="0.0 0.0 0.2 0 0 0 /base_link /camera_link 40"/> 
     <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" launch-prefix="$(arg launch_prefix)" /> <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" launch-prefix="$(arg launch_prefix)" />
</launch>

diego_ar_follower.launch 文件启动 diego_ar_follower node，跟随的参数可以在这个文件中设置。

<launch>

     <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
         <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
     </node>

     <node pkg="diego_ar_tags" name="ar_follower" type="ar_follower.py" clear_params="true" output="screen">
         <rosparam>
             rate: 10
             max_x: 20.0
             goal_x: 0.7
             x_threshold: 0.1
             y_threshold: 0.05
             y_scale: 2.0
             x_scale: 1.0
             max_angular_speed: 1.0
             min_angular_speed: 0.2
             max_linear_speed: 0.2
             min_linear_speed: 0.05
         </rosparam>

    </node>
</launch>

4.启动节点

启动摄像头，在diego中使用xtion深度相机，所以在这里我们首先启动openni

roslaunch diego_vision openni_node.launch

启动ar_track_alvar节点

roslaunch diego_ar_tags ar_large_markers_xition.launch

启动diego_ar_follower节点

roslaunch diego_ar_tags diego_ar_follower.launch

现在只需要拿着我们事先打印好的ar tag在Xtion相机前移动，机器人就会跟着你走。