物体跟踪器object_tracker.py解读

物体跟踪器object_tracker.py

1.先安装rbx1这个包；
2.代码文档位置：rbx1_apps/nodes
3.代码解读：源码+注释

import rospy
from sensor_msgs.msg import RegionOfInterest, CameraInfo
from geometry_msgs.msg import Twist
import thread

class ObjectTracker():
def init(self):
rospy.init_node(“object_tracker”)

    # 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_rotation_speed = rospy.get_param("~max_rotation_speed", 2.0)
    
    # The minimum rotation speed in radians per second  
    # 机器人偏离选定区域时，机器人旋转的最小角速度
    # 设置一个最小速度，保证机器人不会因为自重和摩擦的影响而过慢
    self.min_rotation_speed = rospy.get_param("~min_rotation_speed", 0.5)
    
    # Sensitivity to target displacements.  Setting this too high
    # can lead to oscillations of the robot.
    # 增益参数用于决定目标点与视图中心点不同步时，系统做出的响应速度
    self.gain = rospy.get_param("~gain", 2.0)
    
    # The x threshold (% of image width) indicates how far off-center
    # the ROI needs to be in the x-direction before we react 
    # 设置一个阈值，其值按图像宽度的百分比表示，0.1=10%，阈值用来衡量追踪目标移动速度的大小，达到这个阈值机器人才做出响应。（省电）
    self.x_threshold = rospy.get_param("~x_threshold", 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()
    
    # Get a lock for updating the self.move_cmd values
    # 被分配到ROS订阅者的回调函数运行在与主程序不同的线程中，当修改他们的速度，保证脚本线程的安全
    self.lock = thread.allocate_lock()
    
    # We will get the image width and height from the camera_info topic
    self.image_width = 0
    self.image_height = 0
    
    # Set flag to indicate when the ROI stops updating
    # 目标跟丢机器人停车
    self.target_visible = False
    
    # Wait for the camera_info topic to become available
    rospy.loginfo("Waiting for camera_info topic...")
    rospy.wait_for_message('camera_info', CameraInfo)
    
    # Subscribe the camera_info topic to get the image width and height
    rospy.Subscriber('camera_info', CameraInfo, self.get_camera_info, queue_size=1)

    # Wait until we actually have the camera data
    while self.image_width == 0 or self.image_height == 0:
        rospy.sleep(1)
                
    # Subscribe to the ROI topic and set the callback to update the robot's motion
    # 订阅/roi话题并设置了set_cmd_vel()为回调函数，它会在目标位置变化时设置Twist命令并发送给机器人
    rospy.Subscriber('roi', RegionOfInterest, self.set_cmd_vel, queue_size=1)
    
    # Wait until we have an ROI to follow
    rospy.loginfo("Waiting for messages on /roi...")
    rospy.wait_for_message('roi', RegionOfInterest)
    
    rospy.loginfo("ROI messages detected. Starting tracker...")
    
    # Begin the tracking loop
    # 这是主控值循环，首先有一个锁来保护两个全局变量self.move_cmd和self.target_visible,
    # 因为这两个变量都可以被回调函数set_cmd_vel()所修改
    while not rospy.is_shutdown():
        # Acquire a lock while we're setting the robot speeds
        self.lock.acquire()
        
        try:
            # If the target is not visible, stop the robot
            if not self.target_visible:
                self.move_cmd = Twist()
            else:
                # Reset the flag to False by default
                self.target_visible = False
                
            # Send the Twist command to the robot
            self.cmd_vel_pub.publish(self.move_cmd)
            
        finally:
            # Release the lock
            self.lock.release()
            
        # Sleep for 1/self.rate seconds
        r.sleep()
    # 有新消息发布到/roi话题时，回调函数set_cmd_vel（）就会被触发。
    # 这时首先检测感兴趣区域（ROI）的宽度和高度是否为0.如果是，目标丢失，返回到主循环，机器人停止。
    # 如果不是，target_visible = true,机器人做出反应。
def set_cmd_vel(self, msg):
    # Acquire a lock while we're setting the robot speeds
    self.lock.acquire()
    
    try:
        # If the ROI has a width or height of 0, we have lost the target
        if msg.width == 0 or msg.height == 0:
            self.target_visible = False
            return
        
        # If the ROI stops updating this next statement will not happen
        self.target_visible = True

        # Compute the displacement of the ROI from the center of the image
        # 
        target_offset_x = msg.x_offset + msg.width / 2 - self.image_width / 2

        try:
            percent_offset_x = float(target_offset_x) / (float(self.image_width) / 2.0)
        except:
            percent_offset_x = 0

        # Rotate the robot only if the displacement of the target exceeds the threshold
        if abs(percent_offset_x) > self.x_threshold:
            # Set the rotation speed proportional to the displacement of the target
            try:
                speed = self.gain * percent_offset_x
                if speed < 0:
                    direction = -1
                else:
                    direction = 1
                self.move_cmd.angular.z = -direction * max(self.min_rotation_speed,
                                            min(self.max_rotation_speed, abs(speed)))
            except:
                self.move_cmd = Twist()
        else:
            # Otherwise stop the robot
            self.move_cmd = Twist()

    finally:
        # Release the lock
        self.lock.release()

def get_camera_info(self, msg):
    self.image_width = msg.width
    self.image_height = msg.height

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

if name == ‘main’:
try:
ObjectTracker()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo(“Object tracking node terminated.”)