ROS 教程4 机器人视觉识别控制 OpenCV OpenNI2 PCL 2D 3D目标检测 目标跟踪object tracking人体跟踪person followin

机器人视觉

一、准备工作

1、开源库：

 OpenCV,                           
   二维图像处理和机器学习
   https://github.com/Ewenwan/MVision/tree/master/opencv_app
 OpenNI2 +OpenKinect(freenect),   
    深度传感器(MicrosoftKinect and Asus Xtion Pro)驱动和处理库
 PCL.                         
    点云库 处理三维点云数据
    https://github.com/Ewenwan/MVision/tree/master/PCL_APP

 2维特征提取包  find_object_2d (ROS package)
               https://github.com/introlab/find-object
               http://wiki.ros.org/find_object_2d
               补充参考：https://github.com/introlab/find-object

 3D物体识别（https://github.com/wg-perception）

2D & 3D Object Detection 目标检测 算法和论文

 直接安装
 # ROS Kinetic:
  $ sudo apt-get install ros-kinetic-find-object-2d
 # ROS Jade:
  $ sudo apt-get install ros-jade-find-object-2d
 # ROS Indigo:
  $ sudo apt-get install ros-indigo-find-object-2d
 # ROS Hydro:
  $ sudo apt-get install ros-hydro-find-object-2d
 源码安装
  $ cd ~/catkin_ws
  $ git clone https://github.com/introlab/find-object.git src/find_object_2d
  $ catkin_make

 运行
  $ roscore &
  # Launch your preferred usb camera driver
  $ rosrun uvc_camera uvc_camera_node &
  $ rosrun find_object_2d find_object_2d image:=image_raw

2、图像分辨率：

 160x120 (QQVGA), 320x240 (QVGA), 640x480 (VGA) ， 1280x960 (SXGA).

3、安装深度传感器驱动： ROS OpenNI and OpenKinect (freenect) Drivers
sudo apt-get install ros-indigo-openni-* ros-indigo-openni2-* ros-indigo-freenect-*
$ rospack profile

4、安装普通摄像头驱动 Webcam Driver

 源码安装
 usb_cam:
 cd ~/catkin_ws/src
 git clone https://github.com/bosch-ros-pkg/usb_cam.git
 cd ~/catkin_ws
 catkin_make
 rospack profile

 libuvc_ros:
 https://github.com/ros-drivers/libuvc_ros

 uvc_cam:
 cd cd ~/catkin_ws/src
 git clone https://github.com/ericperko/uvc_cam.git  
 rosmake uvc_cam

5、测试图像传感器

 对于 rgb-d传感器
 使用 image_view 包 测试  http://wiki.ros.org/image_view

 发布数据 在/camera/rgb/image_raw话题上
   Microsoft Kinect:
 $ roslaunch freenect_launch freenect-registered-xyzrgb.launch

   Asus Xtion, Xtion Pro, or Primesense 1.08/1.09 cameras:
 $ roslaunch openni2_launch openni2.launch depth_registration:=true

 可视化查看 rgb图像
 rosrun image_view image_view image:=/camera/rgb/image_raw
 可视化查看 深度图像
 $ rosrun image_view image_view image:=/camera/depth_registered/image_rect

 对于Webcam Driver
 发布消息
 $ roslaunch rbx1_vision usb_cam.launch video_device:=/dev/video0
 or
 $ roslaunch rbx1_vision usb_cam.launch video_device:=/dev/video1
 可视化查看
 rosrun image_view image_view image:=/camera/rgb/image_raw


 
        驱动名字

     ##节点信息

        话题重定向
      

         
         
         
         
           这里注意需要和驱动程序输出的图片格式一致 原先为mmp格式
           对比度
          亮度
          饱和度
         
         

     
 



 rqt 也可以查看

6、安装 ros 支持的opencv

 $ sudo apt-get install ros-indigo-vision-opencv libopencv-dev \
 python-opencv
 $ rospack profile

7、安装 ros连接opencv 桥梁包 cv_bridge Packag 转换 ros图片格式 与 opencv图片格式

http://wiki.ros.org/cv_bridge
rbx1_vision/nodes/cv_bridge.demo.py 展示了怎样使用  cv_bridge

cv_bridge.demo.py

#!/usr/bin/env python
# -*- coding:utf-8 -*-
import rospy
import sys
import cv2
import cv2.cv as cv
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import numpy as np

class cvBridgeDemo():
    def __init__(self):
        self.node_name = "cv_bridge_demo"
        
        rospy.init_node(self.node_name)
        
        # 关闭
        rospy.on_shutdown(self.cleanup)
        
        # 创建 rgb图像 显示窗口
        self.cv_window_name = self.node_name
        cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
        cv.MoveWindow(self.cv_window_name, 25, 75)
        
        # 创建深度图像显示窗口
        cv.NamedWindow("Depth Image", cv.CV_WINDOW_NORMAL)
        cv.MoveWindow("Depth Image", 25, 350)
        
        # 创建 ros 图 到 opencv图像转换 对象
        self.bridge = CvBridge()
        
        # 订阅 深度图像和rgb图像数据发布的话题  以及定义 回调函数
        # the appropriate callbacks
        self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
        self.depth_sub = rospy.Subscriber("input_depth_image", Image, self.depth_callback, queue_size=1)
        # 登陆信息
        rospy.loginfo("Waiting for image topics...")
        rospy.wait_for_message("input_rgb_image", Image)
        rospy.loginfo("Ready.")

    # 收到rgb图像后的回调函数
    def image_callback(self, ros_image):
        # 转换图像格式到opencv格式
        try:
            frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
        except CvBridgeError, e:
            print e
        # 转换成 numpy 图像数组格式
        frame = np.array(frame, dtype=np.uint8)
        
        # 简单处理图像数据 颜色 滤波 边缘检测等
        display_image = self.process_image(frame)
                       
        # 显示图像
        cv2.imshow(self.node_name, display_image)
        
        # 检测键盘按键事件
        self.keystroke = cv2.waitKey(5)
        if self.keystroke != -1:
            cc = chr(self.keystroke & 255).lower()
            if cc == 'q':
                # The user has press the q key, so exit
                rospy.signal_shutdown("User hit q key to quit.")

    # 收到深度图像后的回调函数            
    def depth_callback(self, ros_image):
        # 转换图像格式到opencv格式
        try:
            # Convert the depth image using the default passthrough encoding
            depth_image = self.bridge.imgmsg_to_cv2(ros_image, "passthrough")
        except CvBridgeError, e:
            print e

        # 转换成 numpy 图像数组格式
        depth_array = np.array(depth_image, dtype=np.float32)
                
        # 深度图像 数据 正则化到 二值图像
        cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
        
        # 深度图像处理
        depth_display_image = self.process_depth_image(depth_array)
    
        # 现实结果
        cv2.imshow("Depth Image", depth_display_image)

    # rgb图像处理      
    def process_image(self, frame):
        # 转化成灰度图像
        grey = cv2.cvtColor(frame, cv.CV_BGR2GRAY)
        
        # 均值滤波
        grey = cv2.blur(grey, (7, 7))
        
        # Canny 边缘检测
        edges = cv2.Canny(grey, 15.0, 30.0)
        
        return edges
    
    # 深度图像处理
    def process_depth_image(self, frame):
        # 这里直接返回原图   可做其他处理
        return frame

    # 关闭节点 销毁所有 窗口
    def cleanup(self):
        print "Shutting down vision node."
        cv2.destroyAllWindows()   

# 主函数    
def main(args):       
    try:
        cvBridgeDemo()
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down vision node."
        cv.DestroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

安装 照相机驱动程序 获取视频流 usb_cam

      cd catkin_ws/src
      git clone git://github.com/bosch-ros-pkg/usb_cam.git
      catkin_make
      source ~/catkin_ws/devel/setup.bash
      
使用rgb 摄像头获取的图像数据测试 上述节点功能   注意发布数据的话题 重映射到 上述节点指定的话题

 
   
     
     
     
     
     
     
     
   

    
   
   
 

鼠标 选取 感兴趣区域

#!/usr/bin/env python
# -*- coding:utf-8 -*-
"""
    使用opencv2 跟踪 用户鼠标选择的 区域
"""

import rospy # ros系统操作
import sys   # 系统程序 输入参数的获取等 
import cv2   # 新版opencv2 api 
import cv2.cv as cv # 老版 opencv api
from std_msgs.msg import String       # ros系统 消息类型 来自 std_msgs.msg 标准消息类型 
from sensor_msgs.msg import Image, RegionOfInterest, CameraInfo # ros 系统 消息类型 来自 sensor_msgs 传感器消息类型包 
from cv_bridge import CvBridge, CvBridgeError                   # ros 系统 图像 格式 转换到 opencv图像格式  以及转换失败的错误信息处理
import time        # 计时
import numpy as np # numpy 的数组

class ROS2OpenCV2(object):
    def __init__(self, node_name):        
        self.node_name = node_name
        rospy.init_node(node_name)
        rospy.loginfo("启动节点 " + str(node_name))
        # 关闭 
        rospy.on_shutdown(self.cleanup)
        
        # 一些参数 可由 launch文件 修改的参数
        self.show_text = rospy.get_param("~show_text", True)
        self.show_features = rospy.get_param("~show_features", True)
        self.show_boxes = rospy.get_param("~show_boxes", True)
        self.flip_image = rospy.get_param("~flip_image", False)
        self.feature_size = rospy.get_param("~feature_size", 10) # 点  圆形 的 半径

        # 传感器消息类型  感兴趣区域  发布话题 
        self.ROI = RegionOfInterest()
        self.roi_pub = rospy.Publisher("/roi", RegionOfInterest, queue_size=1)
        
        # 初始化 全局变量
        self.frame = None
        self.frame_size = None
        self.frame_width = None
        self.frame_height = None
        self.depth_image = None
        self.marker_image = None
        self.display_image = None
        self.grey = None
        self.prev_grey = None
        self.selected_point = None
        self.selection = None
        self.drag_start = None
        self.keystroke = None
        self.detect_box = None # 检测的目标区域位置框
        self.track_box = None  # 跟踪的目标区域位置框
        self.display_box = None
        self.keep_marker_history = False
        self.night_mode = False
        self.auto_face_tracking = False
        self.cps = 0            # 每秒 处理 次数 Cycles per second = number of processing loops per second.
        self.cps_values = list()
        self.cps_n_values = 20
        self.busy = False
        self.resize_window_width = 0  #窗口大小
        self.resize_window_height = 0
        self.face_tracking = False
        
        # 创建 显示 窗口
        self.cv_window_name = self.node_name
        cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
        if self.resize_window_height > 0 and self.resize_window_width > 0:
            cv.ResizeWindow(self.cv_window_name, self.resize_window_width, self.resize_window_height)

        # 创建 ros 图 到 opencv图像转换 对象 bridge
        self.bridge = CvBridge()
        # 设置对应窗口 鼠标点击事件 回调函数
        cv.SetMouseCallback (self.node_name, self.on_mouse_click, None)
        
        # 订阅 深度图像和rgb图像数据发布的话题  以及定义 回调函数
        self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
        self.depth_sub = rospy.Subscriber("input_depth_image", Image, self.depth_callback, queue_size=1)
        
    # 鼠标点击事件回调函数                               
    def on_mouse_click(self, event, x, y, flags, param):
      # 允许用户用鼠标选者一个感兴趣区域   一个矩形框图  全局变量 selection 矩形框  <左上角点x 左上角点y  宽度 高度>
        if self.frame is None:
            return
        # 鼠标按下
        if event == cv.CV_EVENT_LBUTTONDOWN and not self.drag_start:
            self.features = []    #  初始化感兴趣区域
            self.track_box = None # 跟踪 框
            self.detect_box = None# 检测 框
            self.selected_point = (x, y)# 选择点
            self.drag_start = (x, y)    # 拖拽起点 后 开始拖拽
        # 鼠标抬起    
        if event == cv.CV_EVENT_LBUTTONUP:
            self.drag_start = None # 拖拽结束
            self.classifier_initialized = False
            self.detect_box = self.selection
        # 鼠标拖拽    
        if self.drag_start:
            xmin = max(0, min(x, self.drag_start[0]))# 起点 横坐标 为 鼠标选者区域的 横向 最小值 min(x, self.drag_start[0])
            ymin = max(0, min(y, self.drag_start[1]))# 起点 纵坐标
            xmax = min(self.frame_width, max(x, self.drag_start[0])) # 终点为最大值 鼠标选者区域的 横向 最大值 man(x, self.drag_start[0])
            ymax = min(self.frame_height, max(y, self.drag_start[1]))
            self.selection = (xmin, ymin, xmax - xmin, ymax - ymin)  # 矩形区域 左上角 起点（x,y） 长 宽
            
    def image_callback(self, data):
        # 图像 header  时间  大小等信息
        self.image_header = data.header
        # 这次循环开始时间
        start = time.time()      
        # 转换成opencv2图像格式
        frame = self.convert_image(data)       
        # 翻转图像  翻转方向：1：水平翻转；0：垂直翻转；-1：水平垂直翻转
        if self.flip_image:
            frame = cv2.flip(frame, 0)        
        # 设置图像 尺寸
        if self.frame_width is None:
            self.frame_size = (frame.shape[1], frame.shape[0])# 列 为 宽度  行为高度
            self.frame_width, self.frame_height = self.frame_size                        
        # 创建可视化  marker image  marker图像  可在rviz中查看
        if self.marker_image is None:
            self.marker_image = np.zeros_like(frame)           
        # 将祯图像 设置成全局变量
        self.frame = frame.copy()
        # 如果 不 记录历史图像 重置 可视化  marker image 
        if not self.keep_marker_history:
            self.marker_image = np.zeros_like(self.marker_image)
            
        # 处理图像 检测和跟踪 目标
        processed_image = self.process_image(frame)
        
        # 如果是 单通道  转化成  多通道  创建全局 处理后的图像
        #if processed_image.channels == 1:
            #cv.CvtColor(processed_image, self.processed_image, cv.CV_GRAY2BGR)
        #else:       
        # 创建全局 处理后的图像
        self.processed_image = processed_image.copy()
        
        # 显示户选择的 矩形框
        self.display_selection()
        
        # Night mode: 仅显示  processed_image
        if self.night_mode:
            self.processed_image = np.zeros_like(self.processed_image)
            
        # 混合 原图 marker image  和处理后的 图像
        self.display_image = cv2.bitwise_or(self.processed_image, self.marker_image)

        # 显示跟踪的 矩形框图
        # cvRect (x,y,w,h) or a rotated Rect (center, size, angle).
        if self.show_boxes:
            if self.track_box is not None and self.is_rect_nonzero(self.track_box):
                if len(self.track_box) == 4:
                    x,y,w,h = self.track_box
                    size = (w, h)
                    center = (x + w / 2, y + h / 2)
                    angle = 0
                    self.track_box = (center, size, angle)
                else:
                    (center, size, angle) = self.track_box   

                # 对于人脸检测 的框  用垂直矩形框 更合适
                if self.face_tracking:
                    pt1 = (int(center[0] - size[0] / 2), int(center[1] - size[1] / 2))
                    pt2 = (int(center[0] + size[0] / 2), int(center[1] + size[1] / 2))
                    cv2.rectangle(self.display_image, pt1, pt2, cv.RGB(50, 255, 50), self.feature_size, 8, 0)# 画 矩形
                else:
                    # 其他 画 旋转的 矩形 
                    vertices = np.int0(cv2.cv.BoxPoints(self.track_box))
                    cv2.drawContours(self.display_image, [vertices], 0, cv.RGB(50, 255, 50), self.feature_size)

            # 显示检测 的 矩形 框图 
            elif self.detect_box is not None and self.is_rect_nonzero(self.detect_box):
                (pt1_x, pt1_y, w, h) = self.detect_box
                if self.show_boxes:
                    cv2.rectangle(self.display_image, (pt1_x, pt1_y), (pt1_x + w, pt1_y + h), cv.RGB(50, 255, 50), self.feature_size, 8, 0)
        
        # 发布感兴趣区域到 感兴趣区域 消息类型 话题上
        self.publish_roi()
            
        # 计算处理时间  返回 处理速度 帧/s
        end = time.time()
        duration = end - start
        fps = int(1.0 / duration)
        self.cps_values.append(fps)
        if len(self.cps_values) > self.cps_n_values:# 保持最新的 几个  处理速度 帧/s 
            self.cps_values.pop(0)
        self.cps = int(sum(self.cps_values) / len(self.cps_values)) # 计算均值
        
        # 显示 处理速度 帧/s CPS and image 以及图像分辨率 
        if self.show_text:
            font_face = cv2.FONT_HERSHEY_SIMPLEX
            font_scale = 0.5

            """ 显示 的位置 """
            if self.frame_size[0] >= 640:
                vstart = 25
                voffset = int(50 + self.frame_size[1] / 120.)
            elif self.frame_size[0] == 320:
                vstart = 15
                voffset = int(35 + self.frame_size[1] / 120.)
            else:
                vstart = 10
                voffset = int(20 + self.frame_size[1] / 120.)
            cv2.putText(self.display_image, "CPS: " + str(self.cps), (10, vstart), font_face, font_scale, cv.RGB(255, 255, 0))
            cv2.putText(self.display_image, "RES: " + str(self.frame_size[0]) + "X" + str(self.frame_size[1]), (10, voffset), font_face, font_scale, cv.RGB(255, 255, 0))
        
        # 更新图像显示
        cv2.imshow(self.node_name, self.display_image)
        
        # 处理键盘 按键命令
        self.keystroke = cv2.waitKey(5)
        if self.keystroke is not None and self.keystroke != -1:
            try:
                cc = chr(self.keystroke & 255).lower()
                if cc == 'n':
                    self.night_mode = not self.night_mode
                elif cc == 'f':
                    self.show_features = not self.show_features
                elif cc == 'b':
                    self.show_boxes = not self.show_boxes
                elif cc == 't':
                    self.show_text = not self.show_text
                elif cc == 'q':
                    # The has press the q key, so exit
                    rospy.signal_shutdown("User hit q key to quit.")
            except:
                pass
    # 深度图像的 回调函数         
    def depth_callback(self, data):
        # 转换图像格式
        depth_image = self.convert_depth_image(data)      
        # 翻转图像  翻转方向：1：水平翻转；0：垂直翻转；-1：水平垂直翻转
        if self.flip_image:    
            depth_image = cv2.flip(depth_image, 0)       
        # 处理
        processed_depth_image = self.process_depth_image(depth_image)
        
        # 全局变量
        self.depth_image = depth_image.copy()
        self.processed_depth_image = processed_depth_image.copy()
    # 转换 rgb图像     
    def convert_image(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        try:
            cv_image = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")       
            return np.array(cv_image, dtype=np.uint8)
        except CvBridgeError, e:
            print e
    # 转换深度图像      
    def convert_depth_image(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        try:
            depth_image = self.bridge.imgmsg_to_cv2(ros_image, "passthrough")            
            depth_image = np.array(depth_image, dtype=np.float32)           
            return depth_image        
        except CvBridgeError, e:
            print e
    # 发布  感兴趣区域 的尺寸 位置 到/roi话题上      
    def publish_roi(self):
        if not self.drag_start:
            if self.track_box is not None:
                roi_box = self.track_box
            elif self.detect_box is not None:
                roi_box = self.detect_box
            else:
                return
        try:
            roi_box = self.cvBox2D_to_cvRect(roi_box)# 转换成 矩形区域
        except:
            return
        
        # 下限 为 0
        roi_box[0] = max(0, roi_box[0])
        roi_box[1] = max(0, roi_box[1])
        
        try:
            ROI = RegionOfInterest()
            ROI.x_offset = int(roi_box[0])
            ROI.y_offset = int(roi_box[1])
            ROI.width = int(roi_box[2])
            ROI.height = int(roi_box[3])
            self.roi_pub.publish(ROI)
        except:
            rospy.loginfo("发布ROI失败")
    # 处理图像  跟踪 感兴趣区域  
    def process_image(self, frame): 
        return frame   
    def process_depth_image(self, frame):
        return frame
    
    # 显示户选择的 矩形框
    def display_selection(self):
        # 显示 举行框
        if self.drag_start and self.is_rect_nonzero(self.selection):# 矩形框
            x,y,w,h = self.selection
            cv2.rectangle(self.marker_image, (x, y), (x + w, y + h), (0, 255, 255), self.feature_size)
            self.selected_point = None
        # 显示 点 圆 
        elif not self.selected_point is None:# 点
            x = self.selected_point[0]
            y = self.selected_point[1]
            cv2.circle(self.marker_image, (x, y), self.feature_size, (0, 255, 255), self.feature_size)
    # 检测矩形框 是否 为空   
    def is_rect_nonzero(self, rect):
        # 左上角点  宽度 高度 类型
        try:
            (_,_,w,h) = rect
            return (w > 0) and (h > 0)
        except:
            try:
                # 左上角点  大小  角度
                ((_,_),(w,h),a) = rect
                return (w > 0) and (h > 0)
            except:
                return False
    # 矩形款 格式转换      <左上角点  大小  角度>  到 <左上角点x 左上角点y  宽度 高度>
    def cvBox2D_to_cvRect(self, roi):
        try:
            if len(roi) == 3:
                (center, size, angle) = roi
                pt1 = (int(center[0] - size[0] / 2), int(center[1] - size[1] / 2))
                pt2 = (int(center[0] + size[0] / 2), int(center[1] + size[1] / 2))
                rect = [pt1[0], pt1[1], pt2[0] - pt1[0], pt2[1] - pt1[1]] # <左上角点x 左上角点y  宽度 高度>
            else:
                rect = list(roi)
        except:
            return [0, 0, 0, 0]
            
        return rect
    # 矩形框  格式转换      <左上角点x 左上角点y  宽度 高度>   到   <左上角点  大小  角度>  
    def cvRect_to_cvBox2D(self, roi):
        try:
            if len(roi) == 3:
                box2d = roi
            else:
                (p1_x, p1_y, width, height) = roi
                center = (int(p1_x + width / 2), int(p1_y + height / 2))
                size = (width, height)
                angle = 0
                box2d = (center, size, angle)
        except:
            return None            
        return list(box2d)
    
    # 清理退出函数      关闭节点 销毁所有 窗口
    def cleanup(self):
        print "关闭了视觉信息处理节点。"
        cv2.destroyAllWindows()       # 关闭所有窗口 
# 主函数
def main(args):    
    try:
        node_name = "ros2opencv2"
        ROS2OpenCV2(node_name)
        rospy.spin()
    except KeyboardInterrupt:
        print "关闭了视觉信息处理节点。"
        cv.DestroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

ros_object_analytics　单帧点云(欧氏距离聚类分割) + Yolo_v2/MobileNet_SSD 3D物体检测

代码

简介：

   物体分析　Object Analytics (OA) 是一个ros包，
   支持实时物体检测定位和跟踪(realtime object detection, localization and tracking.)
   使用　RGB-D 相机输入,提供物体分析服务，为开发者开发更高级的机器人高级特性应用， 
   例如智能壁障(intelligent collision avoidance),和语义SLAM(semantic SLAM). 

   订阅:
         RGB-D　相机发布的点云消息[sensor_msgs::PointClould2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html) 
   发布话题到:
         物体检测　[object detection](https://github.com/intel/object_msgs), 
         物体跟踪　[object tracking](https://github.com/intel/ros_object_analytics/tree/master/object_analytics_msgs), 
         物体3d定位　[object localization](https://github.com/intel/ros_object_analytics/object_analytics_msgs) 

2D 目标检测算法：

 1. 基于 图形处理器(GPU) 运行的目标检测　, 
    ros_opencl_caffe, Yolo v2 model and OpenCL Caffe framework
    https://github.com/intel/ros_opencl_caffe
    
 2. 基于 视觉处理器（VPU） 运行的目标检测 , 
     ros_intel_movidius_ncs (devel branch), 
     with MobileNet_SSD model and Caffe framework.
     
     https://github.com/Ewenwan/ros_intel_movidius_ncs
     
 (Movidius神经计算棒,首个基于USB模式的深度学习推理工具和独立的人工智能（AI）加速器)
 英特尔的子公司Movidius宣布推出Movidius Myriad X视觉处理器（VPU），
 该处理器是一款低功耗SoC，主要用于基于视觉的设备的深度学习和AI算法加速，比如无人机、智能相机、VR/AR头盔。
 就在不久前的上个月，Movidius还推出了基于Myriad 2芯片的神经计算棒Movidius Neural Compute Stick。

节点订阅的　传感器发布的话题

RGB图像　object_analytics/rgb (sensor_msgs::Image)

点云　object_analytics/pointcloud (sensor_msgs::PointCloud2)

节点发布的处理后得到的信息

定位信息(3d边框)　object_analytics/localization (object_analytics_msgs::ObjectsInBoxes3D)

跟踪信息 object_analytics/tracking (object_analytics_msgs::TrackedObjects)

检测信息(2d边框)object_analytics/detection (object_msgs::ObjectsInBoxes)

object_analytics 节点分析

  1. RGBD传感器预处理分割器 splitter  
     输入: /camera/depth_registered/points
     输出: pointcloud   3d点 
     输出: rgb 2d图像
     object_analytics_nodelet/splitter/SplitterNodelet 
     
  2. 点云分割处理器 segmenter
     object_analytics_launch/launch/includes/segmenter.launch
     输入: pointcloud   3d点
     输出: segmentation 分割
     object_analytics_nodelet/segmenter/SegmenterNodelet 
     object_analytics_nodelet/src/segmenter/segmenter_nodelet.cpp
     订阅发布话题后
     std::unique_ptr impl_;
     点云话题回调函数:
        boost::shared_ptr msg = boost::make_shared();// 3d框
        msg->header = points->header;
        impl_->segment(points, msg);//检测
        pub_.publish(msg);          //发布检测消息
        
     object_analytics_nodelet/src/segmenter/segmenter.cpp
     a. 首先　ros点云消息转化成 pcl点云消息
           const sensor_msgs::PointCloud2::ConstPtr& points；
           PointCloudT::Ptr pointcloud(new PointCloudT);
           fromROSMsg(*points, pcl_cloud);
           
     b. 执行分割　Segmenter::doSegment()
        std::vector cluster_indices;// 点云所属下标
        PointCloudT::Ptr cloud_segment(new PointCloudT);// 分割点云
          std::unique_ptr provider_;
        std::shared_ptr seg = provider_->get();//　分割算法
        seg->segment(cloud, cloud_segment, cluster_indices);// 执行分割
        
         AlgorithmProvider -> virtual std::shared_ptr get() = 0;
         Algorithm::segment()
         object_analytics_nodelet/src/segmenter/organized_multi_plane_segmenter.cpp
         class OrganizedMultiPlaneSegmenter : public Algorithm
         OrganizedMultiPlaneSegmenter 类集成　Algorithm类
         分割算法 segment(){} 基于pcl算法
           1. 提取点云法线 OrganizedMultiPlaneSegmenter::estimateNormal()
           2. 分割平面     OrganizedMultiPlaneSegmenter::segmentPlanes()           平面系数模型分割平面
           3. 去除平面后 分割物体  OrganizedMultiPlaneSegmenter::segmentObjects() 　欧氏距离聚类分割
         
     c. 生成消息  Segmenter::composeResult()
        for (auto& obj : objects)
        {
        object_analytics_msgs::ObjectInBox3D oib3;
        oib3.min = obj.getMin();
        oib3.max = obj.getMax();
        oib3.roi = obj.getRoi();
        msg->objects_in_boxes.push_back(oib3);
        }
        
  3. 3d定位器　merger
     输入: 2d检测分割结果 detection
     输入: 3d检测分割结果 segmentation
     输出: 3d定位结果　　 localization
     object_analytics_nodelet/merger/MergerNodelet
     
      2d物体 和 3d物体 关系 ====== 
        遍历 每一个2d物体
           遍历   每一个3d物体(3d物体点云反投影到像素平面上的2d框)
             计算 2d物体边框 和3d物体投影2d边框的相似度  两边框的匹配相似度 match = IOU * distance /  AvgSize
               记录和 该2d边框最相似的 3d物体id

  4. 目标跟踪器 tracker
     输入: 2d图像        rgb        input_rgb 
     输入: 2d检测分割结果 detection  input_2d 
     输出: 跟踪结果　　　 tracking　　output  
     参数: 跟踪帧队列长度: aging_th：default="30"；
     参数: 跟踪置信度: probability_th" default="0.5"
     object_analytics_nodelet/tracker/TrackingNodelet

object_analytics_visualization 可视化

  5. 3d定位可视化　visualization3d　localization
     输入: 2d检测结果 "detection_topic" default="/object_analytics/detection" 
     输入: 3d检测结果 "localization_topic" default="/object_analytics/localization" 
     输入: 2d跟踪结果 "tracking_topic" default="/object_analytics/tracking" 
     输出: rviz可视化话题　"output_topic" default="localization_rviz" 

  6. 2d跟踪可视化　visualization2d 
     输入: 2d检测结果 "detection_topic" default="/object_analytics/detection" 
     输入: 2d跟踪结果 "tracking_topic" default="/object_analytics/tracking" 
     输入: 2d图像     "image_topic" default="/object_analytics/rgb" 
     输出: rviz可视化话题　""output_topic" default="tracking_rviz"    

目标检测接口

GPU yolo_v2　目标检测后端

  opencl_caffe_launch/launch/includes/nodelet.launch 

来源　ros_opencl_caffe　

  输入: 2d图像          /usb_cam/image_raw    input_topic
  输出: 2d检测分割结果  input_detection        output_topic
  参数文件: param_file default= find opencl_caffe_launch/launch/includes/default.yaml"
        模型文件 net_config_path:  "/opt/clCaffe/models/yolo/yolo416/yolo_fused_deploy.prototxt"
        权重文件 weights_path:     "/opt/clCaffe/models/yolo/yolo416/fused_yolo.caffemodel"
        类别标签文件　labels_path: "/opt/clCaffe/data/yolo/voc.txt"

  节点：　opencl_caffe/opencl_caffe_nodelet
  opencl_caffe/src/nodelet.cpp
  Nodelet::onInit()  --->  loadResources() 
  检测器　detector_.reset(new DetectorGpu());
  载入配置文件　detector_->loadResources(net_config_path, weights_path, labels_path)
  订阅话题回调函数　 sub_ = getNodeHandle().subscribe("/usb_cam/image_raw", 1, &Nodelet::cbImage, this);
  Nodelet::cbImage();
     网络前向推理　detector_->runInference(image_msg, objects)
     发布话题　　　pub_.publish(objects);


  DetectorGpu 类
  opencl_caffe/src/detector_gpu.cpp
  网络初始化:
  net.reset(new caffe::Net(net_cfg, caffe::TEST, caffe::Caffe::GetDefaultDevice()));
  net->CopyTrainedLayersFrom(weights);

  模式：
  caffe::Caffe::set_mode(caffe::Caffe::GPU);
  caffe::Caffe::SetDevice(0);
  载入图像:
  cv::cvtColor(cv_bridge::toCvShare(image_msg, "rgb8")->image, image, cv::COLOR_RGB2BGR);
  initInputBlob(resizeImage(image), input_channels);
  网络前传:
  net->Forward();
  获取网络结果:
  caffe::Blob* result_blob = net->output_blobs()[0];
  const Dtype* result = result_blob->cpu_data();
  const int num_det = result_blob->height();
  检测结果:
  object_msgs::ObjectInBox object_in_box;
  object_in_box.object.object_name = labels_list[classid];
  object_in_box.object.probability = confidence;
  object_in_box.roi.x_offset = left;
  object_in_box.roi.y_offset = top;
  object_in_box.roi.height = bot - top;
  object_in_box.roi.width = right - left;
  objects.objects_vector.push_back(object_in_box);

VPU mobileNetSSD 目标检测后端

  movidius_ncs_launch/launch/includes/ncs_stream_detection.launch

来源　ros_intel_movidius_ncs　

  输入: 2d图像         input_rgb        input_topic
  输出: 2d检测分割结果  input_detection  output_topic
  参数: 
     模型类型 name="cnn_type" default="mobilenetssd"
     模型配置文件　name="param_file" default="$(find movidius_ncs_launch)/config/mobilenetssd.yaml"
        网络图配置文件 graph_file_path: "/opt/movidius/ncappzoo/caffe/SSD_MobileNet/graph"
        类别文件voc21  category_file_path: "/opt/movidius/ncappzoo/data/ilsvrc12/voc21.txt"
        网络输入尺寸   network_dimension: 300
        通道均值 :
          channel1_mean: 127.5
          channel2_mean: 127.5
          channel3_mean: 127.5
        归一化:
          scale: 0.007843
      节点文件　movidius_ncs_stream/NCSNodelet　movidius_ncs_stream/src/ncs_nodelet.cpp
  输入话题　input_topic ： 2d图像　      /camera/rgb/image_raw
   输出话题  output_topic : 2d检测框结果  detected_objects
      ncs_manager_handle_ = std::make_shared();
      movidius_ncs_lib::NCSManager 来自 movidius_ncs_lib/src/ncs_manager.cpp
      NCSImpl::init(){ }
      订阅 rgb图像的回调函数 cbDetect()
      sub_ = it->subscribe("/camera/rgb/image_raw", 1, &NCSImpl::cbDetect, this);
      cbDetect(){ }
      1. 从话题复制图像
        cv::Mat camera_data = cv_bridge::toCvCopy(image_msg, "bgr8")->image;
      2. 提取检测结果回调函数
        FUNP_D detection_result_callback = boost::bind(&NCSImpl::cbGetDetectionResult, this, _1, _2);
      3. 进行目标检测 
        ncs_manager_handle_->detectStream(camera_data, detection_result_callback, image_msg);
      NCSManager::detectStream(){}
      得到检测结果 : movidius_ncs_lib::DetectionResultPtr result;
      检测结果:
       for (auto item : result->items_in_boxes)
        object_msgs::ObjectInBox obj;
        obj.object.object_name = item.item.category;
        obj.object.probability = item.item.probability;
        obj.roi.x_offset = item.bbox.x;
        obj.roi.y_offset = item.bbox.y;
        obj.roi.width = item.bbox.width;
        obj.roi.height = item.bbox.height;
        objs_in_boxes.objects_vector.push_back(obj);
     发布检测结果:  
        objs_in_boxes.header = header;
        objs_in_boxes.inference_time_ms = result->time_taken;
        pub_.publish(objs_in_boxes);

视觉控制结合视觉处理和运动控制

 关注两个应用 ： 
	 目标跟踪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 米 可修改