目前，网上大部分源代码有的是基于摄像头检测人物，有的是基于激光雷达信号检测人物，两种方式各有利弊。摄像头检测准确度高，但有死角；激光雷达检测准确度偏低，但能做到360°无死角，基于这个考虑，笔者编写一个算法，集成两者优长，做到智能机器人360°高精度跟踪移动人物。
准备工作：
1.启动小车驱动，这个因不同小车而异，主要是能控制小车前进、后退、旋转，再次不做阐述；
2.启动opencv_app，这个程序及用法在ROS官网上有源代码，主要实现脸部检测，再次不做阐述.http://wiki.ros.org/opencv_apps；
3.启动find_moving_objects，这个程序及用法也同样在ROS官网上有源代码，主要实现基于激光雷达检测移动物体，可下载研究，再次不做阐述。http://wiki.ros.org/find_moving_objects;
4.启动主程序源代码，请结合注释阅读代码。
#!/usr/bin/env python
#-*- coding: utf-8 -*-
import rospy
import math
import time
import numpy as np
from std_msgs.msg import String
from sensor_msgs.msg import Imu,Image,LaserScan
from geometry_msgs.msg import Twist, Vector3
# from xtark_tutorials.msg import Num
from find_moving_objects.msg import MovingObject as MovingObjectMsg
from find_moving_objects.msg import MovingObjectArray as MovingObjectArrayMsg
from opencv_apps.msg import FaceArrayStamped as FaceArrayStampedMsg
class findPerson:
def __init__(self):
self.active_move = False
self.active_face = False
self.active_go = False
# self.active_scan = True
self.PI = 3.1415926
# 设置雷达检测移动对象接收器
self.objectSubscriber=rospy.Subscriber("moving_objects", MovingObjectArrayMsg, self.objectCallback)
# 设置陀螺仪信号接收器
self.imuSubscriber=rospy.Subscriber("imu", Imu, self.imuCallback)
# 设置脸部检测接收器
self.facesSubscriber = rospy.Subscriber("face_detection/faces", FaceArrayStampedMsg, self.facesCallback)
# self.imageSubscriber = rospy.Subscriber("face_detection/image", Image, self.imageCallback)
# 设置移动信号发射器
self.cmdVelPublisher = rospy.Publisher('/cmd_vel', Twist, queue_size =3)
# 设置雷达检测信号接收器
self.scanSubscriber = rospy.Subscriber('/scan', LaserScan, self.scanCallback)
# 角度控制PID
self.PID_controller_angle = simplePID(320, 0.003,0.003, 0)
# 位移控制PID
self.PID_controller_distance = simplePID(1.6, 0.5,0.04, 0)
# 程序退出时，小车停止
rospy.on_shutdown(self.controllerLoss)
# 用于检测周边最小距离避免碰撞；用于检测前方最小距离向脸部位置前进（保持在1.6米），当active_go信号为真时，小车开始移动
def scanCallback(self,scan_data):
ranges = np.array(scan_data.ranges)
rospy.loginfo("safe distance:{}".format(np.amin(np.hstack((ranges[0:90],ranges[-90:]))) ))
if np.amin(np.hstack((ranges[0:90],ranges[-90:]))) < 0.3:
self.controllerLoss()
# rospy.loginfo("scan_data:{}".format(ranges))
distance_array = np.hstack((ranges[0:5],ranges[-5:]))
# rospy.loginfo("distance_array:{}".format(distance_array))
# distance=np.mean(distance_array)
distance=np.amin(distance_array)
rospy.loginfo("min_distance:{}".format(distance))
if self.active_face == True and self.active_go == True:
go_distance = self.PID_controller_distance.update(distance)
rospy.loginfo("pid_distance:{}".format(go_distance))
velocity = Twist()
velocity.linear = Vector3(-go_distance,0,0)
velocity.angular= Vector3(0., 0.,0.)
self.cmdVelPublisher.publish(velocity)
def imageCallback(self,data):
rospy.loginfo(data.width)
rospy.sleep(1)
# 检测脸部信息，使小车移动让脸部在屏幕中心位置，当脸部在屏幕中心位置时，给出active_go信号，让车可以向前移动
def facesCallback(self,data):
# rospy.loginfo(data.faces)
if data.faces != []:
self.active_face = True
face_location = data.faces[0].face.x
rospy.loginfo("face_location:{}".format(face_location))
if face_location >= 290 and face_location <= 350:
self.active_go = True
else:
self.active_go = False
angularSpeed = self.PID_controller_angle.update(face_location)
angularSpeed = np.clip(angularSpeed, -0.5,0.5)
velocity = Twist()
velocity.linear = Vector3(0,0,0)
velocity.angular= Vector3(0., 0.,angularSpeed)
if self.active_move == False:
self.cmdVelPublisher.publish(velocity)
else:
self.active_face = False
# 标记小车是否在移动
def imuCallback(self,data):
move_angular_z=data.angular_velocity.z
# rospy.loginfo("move_angular_z:%s",self.move_angular_z)
if abs(move_angular_z) > 0.01:
self.active_move = True
# rospy.sleep(1)
else:
self.active_move = False
# 雷达检测反馈周边疑似移动物体，经过seen_width和value.speed两轮筛选，使小车转向筛选后的可疑角度，结合摄像头脸部检测，最终确定目标位置
def objectCallback(self,data):
# rospy.loginfo(data)
# rospy.loginfo(rospy.get_caller_id()+"I heard %d",data.num)
# data_objects=np.array(data.objects)
data_objects = np.array(data.objects)
a = np.array([])
if self.active_move == False:
for i in range(data_objects.size):
value=data_objects[i]
if value.seen_width > 0.2 or value.speed < 0.3:
a=np.append(a,i)
data_objects_new_array=np.delete(data_objects, a)
if data_objects_new_array.size == 1 and self.active_face == False:
data_objects_new=data_objects_new_array[0]
object_angle=(data_objects_new.angle_begin+data_objects_new.angle_end)/2
rospy.loginfo("I use lidir find object_angle:%s",object_angle)
if object_angle <= 0:
goal = self.PI+object_angle
else:
goal=object_angle - self.PI
velocity = Twist()
velocity.linear = Vector3(0,0,0)
velocity.angular= Vector3(0., 0.,goal)
self.cmdVelPublisher.publish(velocity)
# rospy.loginfo("I start turn to :%s",goal)
# rospy.sleep(3)
# if self.active_face == True:
# rospy.loginfo("I find people:%s",goal)
# else:
# rospy.loginfo("I can't find people")
# 避免小车碰撞，紧急刹车程序
def stopMoving(self):
velocity = Twist()
velocity.linear = Vector3(0.,0.,0.)
velocity.angular= Vector3(0.,0.,0.)
self.cmdVelPublisher.publish(velocity)
def controllerLoss(self):
self.stopMoving()
rospy.loginfo('lost connection')
class simplePID:
'''very simple discrete PID controller'''
def __init__(self, target, P, I, D):
# rospy.loginfo('PID initialised with P:{}, I:{}, D:{}'.format(P,I,D))
self.Kp =np.array(P)
self.Ki =np.array(I)
self.Kd =np.array(D)
self.setPoint =np.array(target)
self.last_error=0
self.integrator = 0
self.integrator_max = float('inf')
self.timeOfLastCall = None
def update(self, current_value):
current_value=np.array(current_value)
if(self.timeOfLastCall is None):
# the PID was called for the first time. we don't know the deltaT yet
# no controll signal is applied
self.timeOfLastCall = time.clock()
return np.zeros(np.size(current_value))
error = self.setPoint - current_value
P = error
# rospy.loginfo(P)
currentTime = time.clock()
deltaT = (currentTime-self.timeOfLastCall)
# integral of the error is current error * time since last update
self.integrator = self.integrator + (error*deltaT)
I = self.integrator
# derivative is difference in error / time since last update
D = (error-self.last_error)/deltaT
self.last_error = error
self.timeOfLastCall = currentTime
# return controll signal
return self.Kp*P + self.Ki*I + self.Kd*D
if __name__ == '__main__':
rospy.init_node('find_person', anonymous=True)
findperson = findPerson()
try:
rospy.spin()
except rospy.ROSInterruptException:
print('exception')实现效果：https://v.douyin.com/eFvRMQk/
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明智能机器人基于激光雷达和摄像头实现360°跟踪移动人物！
![[ros2学习]-CLI Tools – 创建启动文件(launch)](https://blog.75271.com/wp-content/themes/com75271/timthumb.php?src=https://img-blog.csdnimg.cn/20200402190414829.gif#pic_center&h=110&w=185&q=90&zc=1&ct=1)
