Diego1# 機器視覺 -AR標籤跟隨

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相機前移動，機器人就會跟着你走。