ROS视觉控制结合视觉处理和运动控制
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/video02）启动脸部追踪节点 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.")