ROS理论与实践(以移动机器人为例)连载(六) ——机器视觉处理

---

本文软件包
链接：https://pan.baidu.com/s/1mAHCr4JI-9xMyXa4V_lyIQ
密码：1tmo

1. ROS摄像头驱动及数据接口

①二维摄像头

可以参考以前写的博客：https://blog.csdn.net/qq_44455588/article/details/104998284

$ sudo apt-get install ros-melodic-usb-cam
$ roslaunch usb-cam usb_cam-test.launch
$ rqt_image_view

usb_cam-test.launch

<launch>
	<!-- 摄像头驱动，和上图的摄像头参数设置 -->
	<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    	<param name="video_device" value="/dev/video2" />
    	<param name="image_width" value="640" />
    	<param name="image_height" value="480" />
    	<param name="pixel_format" value="yuyv" />
    	<param name="camera_frame_id" value="usb_cam" />
    	<param name="io_method" value="mmap"/>
  	</node>
  	<!-- 订阅摄像头数据，用做显示的窗口 -->
  	<node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    	<remap from="image" to="/usb_cam/image_raw"/>
    	<param name="autosize" value="true" />
  	</node>
</launch>

二维图像的数据结构：

• Header： 消息头，包含消息序号（ROS默认累加），时间戳和绑定坐标系；
• height： 图像的纵向分辨率；
• width： 图像的横向分辨率；
• encoding： 图像的编码格式，包含RGB、YUV等常用格式，不涉及图像压缩编码；
• is_begendian： 图像数据的大小端存储模式；
  • 大端： 低字节存放在地址高位，高字节存放在地址低位
  • 小端： 高字节存放在地址高位，低字节存放在地址低位
• step： 一行图像数据的字节数量，做为数据的步长参数width*3（RGB）；
• data： 存储图像数据的数组，大小为step*height个字节

1280*720分辨率的摄像头产生一帧图像的数据大小是：3*1280*720=2764800字节，即2.7648MB。

压缩图像消息：

• format： 图像的压缩编码格式（jpeg、png、bmp）
• data： 存储图像数据数组
  

②深度RGB-D摄像头——kinect

• 安装驱动

$ sudo apt-get install ros-melodic-freenect-*
$ git clone https://github.com/avin2/SensorKinect.git
$ cd SensorKinect/Bin
$ tar xvf SensorKinect093-Bin-Linux-x64-v5.1.2.1.tar.bz2
$ sudo ./install.sh	（在解压出来的文件夹目录下）

• 运行

$ roslaunch freenect_launch freenect.launch 

freenect.launch

<launch>
    <!-- Launch the freenect driver -->
    <include file="$(find freenect_launch)/launch/freenect.launch">
        <arg name="publish_tf"                      value="false" /> 

        <!-- use device registration -->
        <arg name="depth_registration"              value="true" /> 

        <arg name="rgb_processing"                  value="true" />
        <arg name="ir_processing"                   value="false" />
        <arg name="depth_processing"                value="false" />
        <arg name="depth_registered_processing"     value="true" />
        <arg name="disparity_processing"            value="false" />
        <arg name="disparity_registered_processing" value="false" />
        <arg name="sw_registered_processing"        value="false" />
        <arg name="hw_registered_processing"        value="true" />
    </include>
</launch>

图像数据结构：

• height： 点云图像的纵向分辨率；
• width： 点云图像的横向分辨率；
• fields： 每个点的数据类型；
• is_bigendian： 数据的大小端存储模式；
• point_step： 单点的数据字节步长；
• row_step： 一行数据的字节步长；
• data： 点云数据的存储数值，总字节大小为row_step*height；
• is_dense： 是否有无效点。
  
  点云即包括RGB信息还有该位置的XYZ信息，点云单帧数据量也很大，如果使用分布式网络传输，需要考虑能否满足数据的传输要求，或者针对数据进行压缩。

③英特尔Intelsense

• 安装SDK https://github.com/intel-ros/realsense/releases

$ mkdir build
$ cd build
$ cmake ..
$ make
$ sudo make install

• 安装ROS驱动 https://github.com/IntelRealSense/librealsense/releases

$ catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
$ catkin_make install
$ echo "source `/catkin_ws/devel/setup.bash" >> ~/.bashrc
$ source ~/.bashrc

参考链接：

1. https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md
2. https://blog.csdn.net/u012926144/article/details/80761342

点云显示

$ roslaunch realsense2_camera rs_rgbd.launch
$ rosrun rviz rviz

2. 摄像头参数标定

①标定rgb摄像头

安装标定功能包

$ sudo apt-get install ros-melodic-camera-calibration

摄像头标定流程

• 启动摄像头

$ roslaunch robot_vision usb_cam.launch

• 启动标定包

$ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/usb_cam/image_raw camera:=/usb_cam

1. size： 标定棋盘格的内部焦点个数，这里使用的棋盘一共有六行，每行有八个内部角点；
2. square： 这个参数对应每个棋盘格的边长，单位是米；
3. image和camera： 设置摄像头发布的图像话题。

棋盘格标定靶8x6

网盘下载链接：
链接: https://pan.baidu.com/s/1UmjJ3-uWHOHPsCUjdIh3mw 密码: jqv7

将棋盘靶打印在A4纸上，贴在一个平面硬纸板上，按照标定流程启动脚本，开始标定，按图中提示，左右上下平移和旋转图片，知道彩色条变绿；点击CALIBRATE，等待内部计算后，点击SAVE保存标定文件，再点击COMMIT，会在.ros文件夹中生成一个camera_info的文件夹里面存放着标定文件。

• X： 标定靶在摄像头视野中的左右移动；
• Y： 标定靶在摄像头视野的上下移动；
• Size： 标定靶在摄像头视野中的前后移动；
• Skew： 标定靶在摄像头视野中的倾斜转动。

②标定Kinect流程

• 启动Kinect

$ roslaunch robot_vision freenect.launch

• 标定彩色摄像头

$ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/camera/rgb/image_raw camera:=/camera/rgb

• 标定红外摄像头

$ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/camera/ir/image_raw camera:=/camera/ir

3. ROS+OpenCV图像处理方法及案例

OpenCV(Open Source Computer Vision Library) 官方turtorial

安装OpenCV

$ sudo apt-get install ros-melodic-vision-opencv libopencv-dev python-opencv

CvBridge用来做OpenCV和ROS图像信息的转换

• imgmsg_to_cv2()： 将ROS图像消息转换成OpenCV图像数据；
• cv2_to_imgmsg()： 将OpenCV格式的图像数据转换成ROS图像消息。

转换示例

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

class image_converter:
    def __init__(self):    
        # 创建cv_bridge，声明图像的发布者和订阅者
        self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.callback)

    def callback(self,data):
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print e

        # 在opencv的显示窗口中绘制一个圆，作为标记
        (rows,cols,channels) = cv_image.shape
        if cols > 60 and rows > 60 :
            cv2.circle(cv_image, (60, 60), 30, (0,0,255), -1)

        # 显示Opencv格式的图像
        cv2.imshow("Image window", cv_image)
        cv2.waitKey(3)

        # 再将opencv格式额数据转换成ros image格式的数据发布
        try:
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
        except CvBridgeError as e:
            print e

if __name__ == '__main__':
    try:
        # 初始化ros节点
        rospy.init_node("cv_bridge_test")
        rospy.loginfo("Starting cv_bridge_test node")
        image_converter()
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down cv_bridge_test node."
        cv2.destroyAllWindows()

①基于Haar特征的级联分类器对象检测算法

相关算法可以扩展阅读https://blog.csdn.net/stdcoutzyx/article/details/34842233。

启动人脸识别实例

$ roslaunch robot_vision usb_cam.launch
$ roslaunch robot_vision face_detector.launch
$ rqt_image_view

示例：

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

class faceDetector:
    def __init__(self):
        rospy.on_shutdown(self.cleanup);

        # 创建cv_bridge
        self.bridge = CvBridge()
        self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)

        # 获取haar特征的级联表的XML文件，文件路径在launch文件中传入
        cascade_1 = rospy.get_param("~cascade_1", "")
        cascade_2 = rospy.get_param("~cascade_2", "")

        # 使用级联表初始化haar特征检测器
        self.cascade_1 = cv2.CascadeClassifier(cascade_1)
        self.cascade_2 = cv2.CascadeClassifier(cascade_2)

        # 设置级联表的参数，优化人脸识别，可以在launch文件中重新配置
        self.haar_scaleFactor  = rospy.get_param("~haar_scaleFactor", 1.2)
        self.haar_minNeighbors = rospy.get_param("~haar_minNeighbors", 2)
        self.haar_minSize      = rospy.get_param("~haar_minSize", 40)
        self.haar_maxSize      = rospy.get_param("~haar_maxSize", 60)
        self.color = (50, 255, 50)

        # 初始化订阅rgb格式图像数据的订阅者，此处图像topic的话题名可以在launch文件中重映射
        self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)

    def image_callback(self, data):
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")     
            frame = np.array(cv_image, dtype=np.uint8)
        except CvBridgeError, e:
            print e

        # 创建灰度图像
        grey_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        # 创建平衡直方图，减少光线影响
        grey_image = cv2.equalizeHist(grey_image)

        # 尝试检测人脸
        faces_result = self.detect_face(grey_image)

        # 在opencv的窗口中框出所有人脸区域
        if len(faces_result)>0:
            for face in faces_result: 
                x, y, w, h = face
                cv2.rectangle(cv_image, (x, y), (x+w, y+h), self.color, 2)

        # 将识别后的图像转换成ROS消息并发布
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))

    def detect_face(self, input_image):
        # 首先匹配正面人脸的模型
        if self.cascade_1:
            faces = self.cascade_1.detectMultiScale(input_image, 
                    self.haar_scaleFactor, 
                    self.haar_minNeighbors, 
                    cv2.CASCADE_SCALE_IMAGE, 
                    (self.haar_minSize, self.haar_maxSize))
                                         
        # 如果正面人脸匹配失败，那么就尝试匹配侧面人脸的模型
        if len(faces) == 0 and self.cascade_2:
            faces = self.cascade_2.detectMultiScale(input_image, 
                    self.haar_scaleFactor, 
                    self.haar_minNeighbors, 
                    cv2.CASCADE_SCALE_IMAGE, 
                    (self.haar_minSize, self.haar_maxSize))
        
        return faces

    def cleanup(self):
        print "Shutting down vision node."
        cv2.destroyAllWindows()

if __name__ == '__main__':
    try:
        # 初始化ros节点
        rospy.init_node("face_detector")
        faceDetector()
        rospy.loginfo("Face detector is started..")
        rospy.loginfo("Please subscribe the ROS image.")
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down face detector node."
        cv2.destroyAllWindows()

②通过帧差法跟踪物体特征点

启动物体跟踪实例：

$ roslaunch robot_vision usb_cam.launch
$ roslaunch robot_vision motion_detector.launch
$ rqt_image_view

示例：

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

class motionDetector:
    def __init__(self):
        rospy.on_shutdown(self.cleanup);

        # 创建cv_bridge
        self.bridge = CvBridge()
        self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)

        # 设置参数：最小区域、阈值
        self.minArea   = rospy.get_param("~minArea",   500)
        self.threshold = rospy.get_param("~threshold", 25)

        self.firstFrame = None
        self.text = "Unoccupied"

        # 初始化订阅rgb格式图像数据的订阅者，此处图像topic的话题名可以在launch文件中重映射
        self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)

    def image_callback(self, data):
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")     
            frame = np.array(cv_image, dtype=np.uint8)
        except CvBridgeError, e:
            print e

        # 创建灰度图像
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        gray = cv2.GaussianBlur(gray, (21, 21), 0)

        # 使用两帧图像做比较，检测移动物体的区域
        if self.firstFrame is None:
            self.firstFrame = gray
            return  
        frameDelta = cv2.absdiff(self.firstFrame, gray)
        thresh = cv2.threshold(frameDelta, self.threshold, 255, cv2.THRESH_BINARY)[1]

        thresh = cv2.dilate(thresh, None, iterations=2)
        binary, cnts, hierarchy= cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        for c in cnts:
            # 如果检测到的区域小于设置值，则忽略
            if cv2.contourArea(c) < self.minArea:
               continue 

            # 在输出画面上框出识别到的物体
            (x, y, w, h) = cv2.boundingRect(c)
            cv2.rectangle(frame, (x, y), (x + w, y + h), (50, 255, 50), 2)
            self.text = "Occupied"

        # 在输出画面上打当前状态和时间戳信息
        cv2.putText(frame, "Status: {}".format(self.text), (10, 20),
            cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)

        # 将识别后的图像转换成ROS消息并发布
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))

    def cleanup(self):
        print "Shutting down vision node."
        cv2.destroyAllWindows()

if __name__ == '__main__':
    try:
        # 初始化ros节点
        rospy.init_node("motion_detector")
        rospy.loginfo("motion_detector node is started...")
        rospy.loginfo("Please subscribe the ROS image.")
        motionDetector()
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down motion detector node."
        cv2.destroyAllWindows()

例子中的算法都很粗糙，这里我们并不对算法进行优化，而是展示ROS中如何使用OpenCV。

4. ROS+Tensorflow物体识别方法及案例

• 安装Tensorflow

$ sudo apt-get install python-pip python-dev python-virtualenv
$ virtualenv --system-site-packages ~/tensorflow
$ source ~/tensorflow/bin/activate
$ easy_install -U pip
$ pip install --upgrade tensorflow

• 安装功能包

$ cd ~/catkin_ws/src
$ git clone https://github.com/Kukanani/vision msgs.git
$ git clone https://github.com/osrf/tensorflow_object_detector.git

• 编译功能包

$ cd ~/catkin_ws && catkin_make

加载Tensorflow环境变量

$ source tensorflow/bin/activate

运行官方提供的物体识别代码（做了ROS封装）

$ roslaunch tensorflow_object_detector usb_cam_detector.launch

执行后，通过

$ rostopic list

可以查找当前物体识别的结果发送出的Object数据，通过这个数据可以控制机器人的运动。

①核心代码

#!/usr/bin/env python
## Author: Rohit
## Date: July, 25, 2017
# Purpose: Ros node to detect objects using tensorflow

import os
import sys
import cv2
import numpy as np
try:
    import tensorflow as tf
except ImportError:
    print("unable to import TensorFlow. Is it installed?")
    print("  sudo apt install python-pip")
    print("  sudo pip install tensorflow")
    sys.exit(1)

# ROS related imports
import rospy
from std_msgs.msg import String , Header
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose

# Object detection module imports
import object_detection
from object_detection.utils import label_map_util
from object_detection.utils import visualization_utils as vis_util

# SET FRACTION OF GPU YOU WANT TO USE HERE
GPU_FRACTION = 0.4

######### Set model here ############
MODEL_NAME =  'ssd_mobilenet_v1_coco_11_06_2017'
# By default models are stored in data/models/
MODEL_PATH = os.path.join(os.path.dirname(sys.path[0]),'data','models' , MODEL_NAME)
# Path to frozen detection graph. This is the actual model that is used for the object detection.
PATH_TO_CKPT = MODEL_PATH + '/frozen_inference_graph.pb'
######### Set the label map file here ###########
LABEL_NAME = 'mscoco_label_map.pbtxt'
# By default label maps are stored in data/labels/
PATH_TO_LABELS = os.path.join(os.path.dirname(sys.path[0]),'data','labels', LABEL_NAME)
######### Set the number of classes here #########
NUM_CLASSES = 90

detection_graph = tf.Graph()
with detection_graph.as_default():
    od_graph_def = tf.GraphDef()
    with tf.gfile.GFile(PATH_TO_CKPT, 'rb') as fid:
        serialized_graph = fid.read()
        od_graph_def.ParseFromString(serialized_graph)
        tf.import_graph_def(od_graph_def, name='')

## Loading label map
# Label maps map indices to category names, so that when our convolution network predicts `5`,
# we know that this corresponds to `airplane`.  Here we use internal utility functions,
# but anything that returns a dictionary mapping integers to appropriate string labels would be fine
label_map = label_map_util.load_labelmap(PATH_TO_LABELS)
categories = label_map_util.convert_label_map_to_categories(label_map, max_num_classes=NUM_CLASSES, use_display_name=True)
category_index = label_map_util.create_category_index(categories)

# Setting the GPU options to use fraction of gpu that has been set
config = tf.ConfigProto()
config.gpu_options.per_process_gpu_memory_fraction = GPU_FRACTION

# Detection

class Detector:

    def __init__(self):
        self.image_pub = rospy.Publisher("debug_image",Image, queue_size=1)
        self.object_pub = rospy.Publisher("objects", Detection2DArray, queue_size=1)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("image", Image, self.image_cb, queue_size=1, buff_size=2**24)
        self.sess = tf.Session(graph=detection_graph,config=config)

    def image_cb(self, data):
        objArray = Detection2DArray()
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)
        image=cv2.cvtColor(cv_image,cv2.COLOR_BGR2RGB)

        # the array based representation of the image will be used later in order to prepare the
        # result image with boxes and labels on it.
        image_np = np.asarray(image)
        # Expand dimensions since the model expects images to have shape: [1, None, None, 3]
        image_np_expanded = np.expand_dims(image_np, axis=0)
        image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
        # Each box represents a part of the image where a particular object was detected.
        boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
        # Each score represent how level of confidence for each of the objects.
        # Score is shown on the result image, together with the class label.
        scores = detection_graph.get_tensor_by_name('detection_scores:0')
        classes = detection_graph.get_tensor_by_name('detection_classes:0')
        num_detections = detection_graph.get_tensor_by_name('num_detections:0')

        (boxes, scores, classes, num_detections) = self.sess.run([boxes, scores, classes, num_detections],
            feed_dict={image_tensor: image_np_expanded})

        objects=vis_util.visualize_boxes_and_labels_on_image_array(
            image,
            np.squeeze(boxes),
            np.squeeze(classes).astype(np.int32),
            np.squeeze(scores),
            category_index,
            use_normalized_coordinates=True,
            line_thickness=2)

        objArray.detections =[]
        objArray.header=data.header
        object_count=1

        for i in range(len(objects)):
            object_count+=1
            objArray.detections.append(self.object_predict(objects[i],data.header,image_np,cv_image))

        self.object_pub.publish(objArray)

        img=cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
        image_out = Image()
        try:
            image_out = self.bridge.cv2_to_imgmsg(img,"bgr8")
        except CvBridgeError as e:
            print(e)
        image_out.header = data.header
        self.image_pub.publish(image_out)

    def object_predict(self,object_data, header, image_np,image):
        image_height,image_width,channels = image.shape
        obj=Detection2D()
        obj_hypothesis= ObjectHypothesisWithPose()

        object_id=object_data[0]
        object_score=object_data[1]
        dimensions=object_data[2]

        obj.header=header
        obj_hypothesis.id = str(object_id)
        obj_hypothesis.score = object_score
        obj.results.append(obj_hypothesis)
        obj.bbox.size_y = int((dimensions[2]-dimensions[0])*image_height)
        obj.bbox.size_x = int((dimensions[3]-dimensions[1] )*image_width)
        obj.bbox.center.x = int((dimensions[1] + dimensions [3])*image_height/2)
        obj.bbox.center.y = int((dimensions[0] + dimensions[2])*image_width/2)

        return obj

def main(args):
    rospy.init_node('detector_node')
    obj=Detector()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("ShutDown")
    cv2.destroyAllWindows()

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

Detection2DArray的数据类型链图

std_msgs/Header header
  ├uint32 seq
  ├time stamp
  └string frame_id
vision_msgs/Detection2D[] detections
  ├std_msgs/Header header
    ├uint32 seq
    ├time stamp
    └string frame_id
  ├vision_msgs/ObjectHypothesisWithPose[] results
    ├string id
    ├float64 score
    └geometry_msgs/PoseWithCovariance pose
      ├geometry_msgs/Pose pose
        ├geometry_msgs/Point position
          ├float64 x
          ├float64 y
          └float64 z
        └geometry_msgs/Quaternion orientation
          ├float64 x
          ├float64 y
          ├float64 z
          └float64 w
      └float64[36] covariance
  ├vision_msgs/BoundingBox2D bbox
    ├geometry_msgs/Pose2D center
      ├float64 x
      ├float64 y
      └float64 theta
    ├float64 size_x
    └float64 size_y
  ├sensor_msgs/Image source_img
    ├std_msgs/Header header
      ├uint32 seq
      ├time stamp
      └string frame_id
    ├uint32 height
    ├uint32 width
    ├string encoding
    ├uint8 is_bigendian
    ├uint32 step
    └uint8[] data
  ├bool is_tracking
  └string tracking_id

本讲大多都是Python实现的代码，并且目的并不在优化算法上，而是如何使用ROS和OpenCV、TensorFlow做数据交互。通过TensorFlow发出的数据经过cv_bridge变成ROS的图像消息类型，进而控制移动机器人移动和旋转。

②使用识别到的目标，控制上讲的移动机器人仿真代码示例

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <vision_msgs/Detection2DArray.h>

float center_x=0, center_y=0, size_x=0, size_y=0;
int id=0;

void Callback(const vision_msgs::Detection2DArray::ConstPtr& obj);

int main(int argc, char** argv)
{
    ros::init(argc, argv, "tf_obj_detection_ctrl_bot");
	ros::NodeHandle n;

	ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 100);
	ros::Subscriber sub = n.subscribe("/objects", 100, Callback);

	ros::Rate loop_rate(10);
	while(ros::ok())
	{
		geometry_msgs::Twist msg;
		if(id==47 && center_x<=200)
		{
			msg.angular.z=-0.3;
			vel_pub.publish(msg);
			ROS_INFO("rotate left!");
		}
		else if(id==47 && center_x>=280)
		{
			msg.angular.z=0.3;
			vel_pub.publish(msg);
			ROS_INFO("rotate right!");
		}
		else if(id==47 && size_x<=220)
		{
			msg.linear.x=0.3;
			vel_pub.publish(msg);
			ROS_INFO("move front!");
		}
		else if(id==47 && size_x>=320)
		{
			msg.linear.x=-0.3;
			vel_pub.publish(msg);
			ROS_INFO("move back!");
		}
		ros::spinOnce();
		loop_rate.sleep();
	}    
	return 0;
}

void Callback(const vision_msgs::Detection2DArray::ConstPtr& obj)
{
	int frame_id = atoi(obj->detections.back().header.frame_id.c_str());
	id = obj->detections.back().results.back().id;
	center_x = obj->detections.back().bbox.center.x;
	center_y = obj->detections.back().bbox.center.y;
	size_x = obj->detections.back().bbox.size_x;
	size_y = obj->detections.back().bbox.size_y;
	ROS_INFO("frame_id:[%d], id:[%d]", frame_id, id);
	ROS_INFO("center_point:[%f,%f], size_x:[%f], size_y:[%f]", center_x, center_y, size_x, size_y);
}