视觉控制结合视觉处理和运动控制
?关注两个应用 ：?
?? ? 目标跟踪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 米 可修改