机器人视觉

视觉控制结合视觉处理和运动控制
?关注两个应用 ：?

?? ? 目标跟踪object tracking ?和?
?? ? 人体跟踪（跟随） person following

坐标系:

?? ?相机坐标系
?? ?右手坐标系
?? ?相机正前方为 z轴正方向
?? ?水平方向为 x轴
?? ?垂直方向为 y轴
1. 目标跟踪object tracking
上面 使用 opencv 跟踪 面部 关键点 和 颜色?
跟踪的结果为 目标在 图像中的区域 ROI region of interest 感兴趣区域
被发布在 ROS话题 /roi 上，如果相机安装在一个移动地盘上

我们可以使用 ROI 的 x_offset 水平坐标偏置(相对于整个图像)
我们可以旋转 移动底盘保证 ROI 的 x_offset位于图像的水平正中央
(偏左 向左旋转，逆时针； 偏右 向右旋转，顺时针)
如果相机加入垂直方向舵机 还可以 旋转舵机 使得 ROI 的 y_offset
位于 图像的垂直 正中央

还可以使用 roi区域对应的深度信息 控制 移动底盘 前进后者后退

深度较大 前进
深度较小 后退

保持深度值在一个固定的值
关键程序: rbx1_apps/nodes/object_tracker.py

1） 先启动一个深度摄像头或者 普通相机 深度摄像头 1 Microsoft Kinect: $ roslaunch freenect_launch freenect-registered-xyzrgb.launch

深度摄像头 2华硕 Asus Xtion, Xtion Pro, or Primesense 1.08/1.09 cameras:
$ roslaunch openni2_launch openni2.launch depth_registration:=true

webcam ?:
$ roslaunch rbx1_vision usb_cam.launch video_device:=/dev/video0
2）启动脸部追踪节点 roslaunch rbx1_vision face_tracker2.launch
运行了节点 /rbx1_vision/node/face_tracker2.py

3）启动目标追踪节点 roslaunch rbx1_apps object_tracker.launch
运行了节点 /rbx1_apps/nodes/object_tracker.py

4）使用 rqt_plot 显示 脸部 位置 和 移动速度信息 rqt_plot /cmd_vel/angular/z

5）仿真环境下测试跟踪效果

$ roslaunch rbx1_bringup fake_turtlebot.launch
$ rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz
移动脸部 ， turtlebot会旋转
6）代码分析 /rbx1_apps/nodes/object_tracker.py

#!/usr/bin/env python 
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
        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
        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)
                    
        # 订阅ROI话题 Subscribe to the ROI topic and set the callback to update the robot's motion
	# 回调函数为 set_cmd_ve()
        rospy.Subscriber('roi', RegionOfInterest, self.set_cmd_vel, queue_size=1)
        
        # 等待ROI信息 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...")
        
        # 开始跟踪循环====
        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话题  的回调函数=================
    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
	    # roi 中心 和 图像 中心的 水平方向偏移量=======================
            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.")
2.目标跟随, 在目标跟踪上 引入深度信息 可旋转 前进 后退
可以使用 face tracker, CamShift or LK tracker 节点 发现目标

在 rbx1_apps/nodes/object_follower.py 

/roi 话题 获取 目标 水平 和 垂直方向 的位置  让相机正对着 目标中央

/camera/depth_registered/image_rect 深度图 话题 
    消息类型 sensor_msgs/Image  深度单位 mm毫米 除以1000 得到米m
    回调函数 使用 cv_bridge 将深度图 转换成 数组 可以计算 ROI区域的均值
还可以使用 pcl 点运数据


计算 roi 区域 的平均深度 来反应 目标物体 在 相机前方的距离 
通过使移动底盘 前进、后退 保持一个给定的距离  （注意相机 和 底盘的安装 位置）
6）代码分析

/rbx1_apps/nodes/object_follower.py

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image, RegionOfInterest, CameraInfo
from geometry_msgs.msg import Twist
from math import copysign, isnan
from cv2 import cv as cv
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import thread

class ObjectFollower():
    def __init__(self):
	# 节点初始化
        rospy.init_node("object_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)
        
        # 感兴趣区域 Scale the ROI by this factor to avoid background distance values around the edges
        self.scale_roi = rospy.get_param("~scale_roi", 0.9)
        
        # 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.02) 
        
        # 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)
        
        # The x threshold (% of image width) indicates how far off-center
        # the ROI needs to be in the x-direction before we react
        self.x_threshold = rospy.get_param("~x_threshold", 0.1)
        
        # How far away from the goal distance (in meters) before the robot reacts
        self.z_threshold = rospy.get_param("~z_threshold", 0.05)
        
        # The maximum distance a target can be from the robot for us to track
        self.max_z = rospy.get_param("~max_z", 1.6)

        # The minimum distance to respond to
        self.min_z = rospy.get_param("~min_z", 0.5)
        
        # The goal distance (in meters) to keep between the robot and the person
        self.goal_z = rospy.get_param("~goal_z", 0.75)

        # How much do we weight the goal distance (z) when making a movement
        self.z_scale = rospy.get_param("~z_scale", 0.5)

        # How much do we weight (left/right) of the person when making a movement        
        self.x_scale = rospy.get_param("~x_scale", 2.0)
        
        # Slow down factor when stopping
        self.slow_down_factor = rospy.get_param("~slow_down_factor", 0.8)
        
        # Initialize the global ROI
        self.roi = RegionOfInterest()

        # 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
        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
        
        # We need cv_bridge to convert the ROS depth image to an OpenCV array
        self.cv_bridge = CvBridge()
        self.depth_array = None
        
        # 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 to 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)
            
        # Wait for the depth image to become available
        rospy.loginfo("Waiting for depth_image topic...")
        
        rospy.wait_for_message('depth_image', Image)
                    
        # Subscribe to the depth image
	# 订阅深度图 话题 回调函数  convert_depth_image() 深度数据/1000 转换成 数组
	# 话题名  在 launch 里 需要remap 重映射  
        self.depth_subscriber = rospy.Subscriber("depth_image", Image, self.convert_depth_image, queue_size=1)
        
        # Subscribe to the ROI topic and set the callback to update the robot's motion
        rospy.Subscriber('roi', RegionOfInterest, self.set_cmd_vel, queue_size=1)
        
        # Wait until we have an ROI to follow
        rospy.loginfo("Waiting for an ROI to track...")
        
        rospy.wait_for_message('roi', RegionOfInterest)
        
        rospy.loginfo("ROI messages detected. Starting follower...")
        
        # Begin the tracking loop
        while not rospy.is_shutdown():
            # Acquire a lock while we're setting the robot speeds
            self.lock.acquire()
            
            try:
                if not self.target_visible:
                    # If the target is not visible, stop the robot smoothly
                    self.move_cmd.linear.x *= self.slow_down_factor
                    self.move_cmd.angular.z *= self.slow_down_factor
                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消息 和 深度数据  转换到 速度指令                     
    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
            else:
                self.target_visible = True
    
            self.roi = msg
                        
            # Compute the displacement of the ROI from the center of the image
	    # 原来 图像中心点 self.image_width / 2 水平方向
            # 目标区域 中心点 = 起点 + roi宽度  = msg.x_offset + msg.width / 2
	    # 两者之差 为 目标 偏离相机中心为插值 
            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
                speed = percent_offset_x * self.x_scale
                self.move_cmd.angular.z = -copysign(max(self.min_rotation_speed,
                                            min(self.max_rotation_speed, abs(speed))), speed)
            else:
                self.move_cmd.angular.z = 0
             
	    # 计算目标 深度信息   
            # Now compute the depth component
            
            # Initialize a few depth variables
            n_z = sum_z = mean_z = 0
             
            # Shrink the ROI slightly to avoid the target boundaries
	    # 缩小 roi范围 精准  滤出边缘背景点 因子 0.9  roi是长方形 物体形状不规则
            scaled_width = int(self.roi.width * self.scale_roi)
            scaled_height = int(self.roi.height * self.scale_roi)
            # ROI像素坐标范围
            # Get the min/max x and y values from the scaled ROI
            min_x = int(self.roi.x_offset + self.roi.width * (1.0 - self.scale_roi) / 2.0)
            max_x = min_x + scaled_width
            min_y = int(self.roi.y_offset + self.roi.height * (1.0 - self.scale_roi) / 2.0)
            max_y = min_y + scaled_height
            # 得到 ROI区域深度信息 的均值
            # Get the average depth value over the ROI
            for x in range(min_x, max_x):
                for y in range(min_y, max_y):
                    try:
                        # Get a depth value in meters
                        z = self.depth_array[y, x] #对应坐标的深度值
                        
                        # Check for NaN values returned by the camera driver
                        if isnan(z):# 验证是否存在
                            continue
                                                   
                    except:
                        # It seems to work best if we convert exceptions to 0
                        z = 0
                        
                    # A hack to convert millimeters to meters for the freenect driver
		    # 毫米转换到 米m
                    if z > 100:
                        z /= 1000.0
                        
                    # Check for values outside max range
                    if z > self.max_z: #滤除 异常值
                        continue
                    
                    # Increment the sum and count
                    sum_z = sum_z + z # 深度值 纸盒
                    n_z += 1# 深度值 点数计数 用来计算均值 
                    
            # Stop the robot's forward/backward motion by default
            linear_x = 0 # 先停止前进后退 
            
            # If we have depth values...
            if n_z:#有记录到 有效的点
                mean_z = float(sum_z) / n_z #深度均值
                                                                                                
                # Don't let the mean fall below the minimum reliable range
                mean_z = max(self.min_z, mean_z)# 最小距离限制
                                                        
                # Check the mean against the minimum range
                if mean_z > self.min_z:
                    # Check the max range and goal threshold
		    # 均值深度 在最小值和最大值之间 并且 均值深度值和 目标深度差值超过阈值  
                    # 向前向后移动
                    if mean_z < self.max_z and (abs(mean_z - self.goal_z) > self.z_threshold):
                        speed = (mean_z - self.goal_z) * self.z_scale
                        linear_x = copysign(min(self.max_linear_speed, max(self.min_linear_speed, abs(speed))), speed)
    
            if linear_x == 0:
                # Stop the robot smoothly
                self.move_cmd.linear.x *= self.slow_down_factor
            else:
                self.move_cmd.linear.x = linear_x
        
        finally:
            # Release the lock
            self.lock.release()#释放进程锁

    # 深度图 话题 订阅 的回调函数  用cv_bridge 转换 深度图 到 深度数据数组
    def convert_depth_image(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        try:
            # Convert the depth image using the default passthrough encoding
  	    # 转换	     
            depth_image = self.cv_bridge.imgmsg_to_cv2(ros_image, "passthrough")
        except CvBridgeError, e:
            print e

        # Convert the depth image to a Numpy array
        self.depth_array = np.array(depth_image, dtype=np.float32)

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

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        # Unregister the subscriber to stop cmd_vel publishing
        self.depth_subscriber.unregister()
        rospy.sleep(1)
        # Send an emtpy Twist message to stop the robot
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)      

if __name__ == '__main__':
    try:
        ObjectFollower()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("Object follower node terminated.")
运行
1）传感器

深度摄像头 1 Microsoft Kinect:
$ roslaunch freenect_launch freenect-registered-xyzrgb.launch

深度摄像头 2华硕 Asus Xtion, Xtion Pro, or Primesense 1.08/1.09 cameras:
$ roslaunch openni2_launch openni2.launch depth_registration:=true

双面摄像头获取深度
可编写
2）启动脸部追踪节点

roslaunch rbx1_vision face_tracker2.launch
运行了节点 /rbx1_vision/node/face_tracker2.py 或者 颜色 追踪 roslaunch rbx1_vision camshift.launch 鼠标框选 有颜色的物体 或者 特征点 光流法追踪 roslaunch rbx1_vision lk_tracker.launch 鼠标框选 任意有结构纹理角点线的物体

3）启动目标跟随节点

roslaunch rbx1_apps object_follower.launch     
运行了节点  /rbx1_apps/nodes/object_flower.py
4）使用 rqt_plot 显示 脸部 位置 和 移动速度信息

rqt_plot /cmd_vel/angular/z
5）仿真环境下测试跟踪效果

$ roslaunch rbx1_bringup fake_turtlebot.launch
$ rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz
移动脸部 / 有颜色物体 ， turtlebot会旋转

走进 机器人退后  离开机器人跟随

深度距离object_follower.launch默认 0.7 米 可修改
3. 人体跟随 使用 PCL点运数据
ROS sensor_msgs 包 定义了一个类  PointCloud2 消息类型
和一个 point_cloud2.py 模块 由 点云数据 获取 深度 

我们不知道 人体的确切形状 但是可是 找一个像人一样的 团块 距离相机 坐标系 一定距离的 空间范围内的点 
避免认错 家具的一部分 椅子腿 

对于深度相机 z轴 代表了深度信息 里人体的远近 
 1】 太远 前进  太近 后退
 2】 靠左 左转  靠右 右转
/rbx1_apps/nodes/follower.py

代码分析

#!/usr/bin/env python

import rospy
from roslib import message
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import Twist
from math import copysign

class Follower():
    def __init__(self):
        # 节点初始化
        rospy.init_node("follower")
        
        # 节点关闭 清理内存 Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        # The dimensions (in meters) of the box in which we will search
        # for the person (blob). These are given in camera coordinates
        # where x is left/right,y is up/down and z is depth (forward/backward)
        # 近似 人体的 参数
	# 人体点在空间 相对于 移动底盘的 空间坐标点集 范围  所有的点云在这个范围内的 认为是人体的点 
        self.min_x = rospy.get_param("~min_x", -0.2)
        self.max_x = rospy.get_param("~max_x", 0.2)
        self.min_y = rospy.get_param("~min_y", -0.3)
        self.max_y = rospy.get_param("~max_y", 0.5)
        self.max_z = rospy.get_param("~max_z", 1.2)
        
        # The goal distance (in meters) to keep between the robot and the person
        self.goal_z = rospy.get_param("~goal_z", 0.6)
        
        # How far away from the goal distance (in meters) before the robot reacts
        self.z_threshold = rospy.get_param("~z_threshold", 0.05)
        
        # How far away from being centered (x displacement) on the person
        # before the robot reacts
        self.x_threshold = rospy.get_param("~x_threshold", 0.05)
        
        # 深度差值 转出 前进后退运动速度 权重 How much do we weight the goal distance (z) when making a movement
        self.z_scale = rospy.get_param("~z_scale", 1.0)

        # 水平差值 转成旋转运动速度 权重  How much do we weight left/right displacement of the person when making a movement        
        self.x_scale = rospy.get_param("~x_scale", 2.5)
        
        # 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.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)
        
        # Slow down factor when stopping
        self.slow_down_factor = rospy.get_param("~slow_down_factor", 0.8)
        
        # Initialize the movement command
        self.move_cmd = Twist()

        # 发布 控制质量 消息话题 Publisher to control the robot's movement
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

        # 订阅点云数据 回调函数 set_cmd_vel()  Subscribe to the point cloud
        self.depth_subscriber = rospy.Subscriber('point_cloud', PointCloud2, self.set_cmd_vel, queue_size=1)

        rospy.loginfo("Subscribing to point cloud...")
        
        # Wait for the pointcloud topic to become available
        rospy.wait_for_message('point_cloud', PointCloud2)

        rospy.loginfo("Ready to follow!")
        
    def set_cmd_vel(self, msg):
        # Initialize the centroid coordinates point count
        x = y = z = n = 0

        # Read in the x, y, z coordinates of all points in the cloud
        for point in point_cloud2.read_points(msg, skip_nans=True):#遍历 所有的三维 点云数据
            pt_x = point[0]
            pt_y = point[1]
            pt_z = point[2]
            
            # Keep only those points within our designated boundaries and sum them up
	    # 在相对相机 特定范围内的点 认为是 人体 点
            if -pt_y > self.min_y and -pt_y < self.max_y and pt_x < self.max_x and pt_x > self.min_x and pt_z < self.max_z:
                x += pt_x # 所有人体点 哥哥坐标值 求和取均值
                y += pt_y
                z += pt_z
                n += 1
        
       # If we have points, compute the centroid coordinates
       # 计算 点云坐标 均值
        if n:
            x /= n 
            y /= n 
            z /= n
            
            # Check our movement thresholds
            # 深度距离差值 超过最小值阈值 进行 前进/后退 移动
            if (abs(z - self.goal_z) > self.z_threshold):
                # Compute the angular component of the movement
                linear_speed = (z - self.goal_z) * self.z_scale # 乘上 权重系数
                
                # Make sure we meet our min/max specifications
                self.move_cmd.linear.x = copysign(max(self.min_linear_speed, 
                                        min(self.max_linear_speed, abs(linear_speed))), linear_speed)
            else:
                self.move_cmd.linear.x *= self.slow_down_factor # 线速度 缓慢  减速  
                
            if (abs(x) > self.x_threshold): # x值为水平方向 x=0 为相机光心 坐标值 过大 就偏转了 
                # Compute the linear component of the movement
                angular_speed = -x * self.x_scale
                
                # Make sure we meet our min/max specifications
                # 最小最大角速度 限制
                self.move_cmd.angular.z = copysign(max(self.min_angular_speed, 
                                        min(self.max_angular_speed, abs(angular_speed))), angular_speed)
            else:
                # Stop the rotation smoothly
                self.move_cmd.angular.z *= self.slow_down_factor # 角速度 缓慢减速
                
        else: #未找到 人体点 
            # Stop the robot smoothly 缓慢减速
            self.move_cmd.linear.x *= self.slow_down_factor
            self.move_cmd.angular.z *= self.slow_down_factor
            
        # Publish the movement command
        self.cmd_vel_pub.publish(self.move_cmd)

        
    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        
        # Unregister the subscriber to stop cmd_vel publishing
        self.depth_subscriber.unregister()
        rospy.sleep(1)
        
        # Send an emtpy Twist message to stop the robot
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)        
                   
if __name__ == '__main__':
    try:
        Follower()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("Follower node terminated.")

实验
1）传感器

深度摄像头 1 Microsoft Kinect:
$ roslaunch freenect_launch freenect-registered-xyzrgb.launch

深度摄像头 2华硕 Asus Xtion, Xtion Pro, or Primesense 1.08/1.09 cameras:
$ roslaunch openni2_launch openni2.launch depth_registration:=true

双面摄像头获取深度
可编写
2）启动 人体跟随节点

roslaunch rbx1_apps ?follower.py
5）仿真环境下测试 人体 跟踪效果

$ roslaunch rbx1_bringup fake_turtlebot.launch
$ rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz
走进 机器人退后 ?离开机器人跟随
深度距离object_follower.launch默认 0.7 米 可修改