很久没有写博客了,主要是最近在忙着写论文的报告,效率也很低,论文可真难写啊,创新点真的难找,归根到底还是理论知识太薄弱,我论文的方向是单目视觉机器人跟踪人,我们的机器人平台上安装有Kinect深度相机,大约半年以前我曾试过使用rbx1包中的follower.py,效果还不错,在这里记录一下:
准备工作:
1. 安装kinect的驱动
2. 下载rbx1包并编译
运行步骤:
1. 首先运行机器人底盘程序(根据自己使用的底盘决定)
roslaunch ros_arduino_python arduino.python
2. 运行Kinect驱动
roslaunch openni_launch openni.launch
3. 运行rbx1包中的follower
rosrun rbx1_apps follower.py
follower.py代码的详细注释:
# -*- coding:utf-8 -*-
#!/usr/bin/env python
"""
follower.py - Version 1.1 2013-12-20
Follow a "person" by tracking the nearest object in x-y-z space.
Based on the follower application by Tony Pratkanis at:
http://ros.org/wiki/turtlebot_follower
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
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)
# x,y,z坐标实际上在Kinect前方设定了一个长方体的区域
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
# goal_z参数决定了小车和人需要保持的距离
self.goal_z = rospy.get_param("~goal_z", 0.6)
# How far away from the goal distance (in meters) before the robot reacts
# z_threshold是一个距离阈值函数,如果小车前后晃动剧烈,可适当增大该参数
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
# x_threshold是x方向的方向阈值函数,若小车左右晃动剧烈,可适当增大该参数
self.x_threshold = rospy.get_param("~x_threshold", 0.05)
# How much do we weight the goal distance (z) when making a movement
# z_scale是线速度的权值,用来控制线速度的加速度
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
# x_scale是角速度权值,控制角速度的加速度
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)
# 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):
# 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.")
参考博客:https://blog.csdn.net/Prpri/article/details/52844728