单线激光雷达(Lidar)学习四:使用雷达进行目标跟随(二)
前言:
结合上一篇内容,我使用gibhub的例程转移到了自己的雷达机器上,发现了一个问题,那便是在使用不同品牌的雷达进行跟随时会出现机器人乱动,达不到跟随的功能的情况,经过对多个不同的品牌的多次测试,我发现rplidar是可以直接使用的,但我在使用ydlidar与ls01b(镭神)时都出现了错乱的问题,经过测试与问题查找我发现是获取激光/scan数据的问题,GitHub上的例程过滤不掉inf这个值。
既然发现了问题那么接下来便是解决问题
直接上修改后的代码:
#!/usr/bin/env python
#test mail: chutter@uos.de
import rospy
import thread, threading
import time
import numpy as np
from sensor_msgs.msg import Joy, LaserScan
from geometry_msgs.msg import Twist, Vector3
from std_msgs.msg import String as StringMsg
from riki_lidar_follower.msg import position as PositionMsg
class laserTracker:
def __init__(self):
self.lastScan=None
self.winSize = rospy.get_param('~winSize')
self.deltaDist = rospy.get_param('~deltaDist')
self.scanSubscriber = rospy.Subscriber('/scan', LaserScan, self.registerScan)
self.positionPublisher = rospy.Publisher('/object_tracker/current_position', PositionMsg,queue_size=3)
self.infoPublisher = rospy.Publisher('/object_tracker/info', StringMsg, queue_size=3)
def registerScan(self, scan_data):
# registers laser scan and publishes position of closest object (or point rather)
ranges = np.array(scan_data.ranges)
# sort by distance to check from closer to further away points if they might be something real
sortedIndices = np.argsort(ranges)
minDistanceID = None
minDistance = float('inf')
if(not(self.lastScan is None)):
# if we already have a last scan to compare to:
for i in sortedIndices:
# check all distance measurements starting from the closest one
ranges[np.isinf(ranges)] = 0
if i <= 65 or i>295:
if ranges[i] >=0.05 and ranges[i] <= 12 :
tempMinDistance = ranges[i]
rospy.loginfo("{}\n".format(i))
rospy.loginfo("{}\n".format(tempMinDistance))
# now we check if this might be noise:
# get a window. in it we will check if there has been a scan with similar distance
# in the last scan within that window
# we kneed to clip the window so we don't have an index out of bounds
windowIndex = np.clip([i-self.winSize, i+self.winSize+1],0,len(self.lastScan))
window = self.lastScan[windowIndex[0]:windowIndex[1]]
with np.errstate(invalid='ignore'):
# check if any of the scans in the window (in the last scan) has a distance close enough to the current one
if(np.any(abs(window-tempMinDistance)<=self.deltaDist)):
# this will also be false for all tempMinDistance = NaN or inf
# we found a plausible distance
minDistanceID = i
minDistance = ranges[minDistanceID]
#rospy.logwarn("{}\n".format(minDistanceID))
#rospy.logwarn("Q:{}\n".format(minDistance))
break # at least one point was equally close
# so we found a valid minimum and can stop the loop
self.lastScan=ranges
#catches no scan, no minimum found, minimum is actually inf
if(minDistance > scan_data.range_max):
#means we did not really find a plausible object
# publish warning that we did not find anything
rospy.logwarn('laser no object found')
self.infoPublisher.publish(StringMsg('laser:nothing found'))
else:
if minDistanceID >45:
Angle = minDistanceID - 360
else :
Angle = minDistanceID
if minDistance <= 1.5 :
linear = minDistance
Angle = Angle
else:
linear = 0
Angle = 0
# calculate angle of the objects location. 0 is straight ahead
minDistanceAngle =Angle * scan_data.angle_increment
# here we only have an x angle, so the y is set arbitrarily
self.positionPublisher.publish(PositionMsg(minDistanceAngle, 42, minDistance))
#rospy.logwarn("{}\n".format(minDistanceAngle))
if __name__ == '__main__':
print('starting')
rospy.init_node('laser_tracker')
tracker = laserTracker()
print('seems to do something')
try:
rospy.spin()
except rospy.ROSInterruptException:
print('exception')
代码部分解读:
self.scanSubscriber = rospy.Subscriber('/scan', LaserScan, self.registerScan)
接收雷达/scan数据,此处话题需对应自己雷达发布的话题。
self.positionPublisher = rospy.Publisher('/object_tracker/current_position', PositionMsg,queue_size=3)
发布/scan处理后的数据,fllower节点计算cmd_vel的数据
ranges = np.array(scan_data.ranges)
获取/scan中的ranges的数据并排列为数组
sortedIndices = np.argsort(ranges)
将数组进行从小到大依次进行排序,且序号不变,
如数组[3,2,5,1]序号为[1,2,3,4],进行排序后数组为[1,2,3,5]序号为[4,2,1,3]
ranges[np.isinf(ranges)] = 0
将ranges中的inf转化为0
if i <= 65 or i>295:
设定角度,获取固定角度内的激光束
if ranges[i] >=0.05 and ranges[i] <= 12 :
过滤掉小于0.05的值,包括转换的inf
tempMinDistance = ranges[i]
读取距离值
if minDistanceID >45:
#主要获取的角度为[0-65]与[295-359]的数据
Angle = minDistanceID – 360
#设定的角度范围值为[-65,65]
else :
Angle = minDistanceID
minDistance = ranges[minDistanceID]
#读取距离值
minDistanceAngle =Angle * scan_data.angle_increment
#将所得角度乘以角度增量,得到角度偏移量
self.positionPublisher.publish(PositionMsg(minDistanceAngle, 42, minDistance))
#发布距离值与角度偏移量。
至此,经过测试,发现follower. py的计算只要laserTracker.py获取的数据正常且正确,不需要进行修改即可进行雷达跟随,发布正确的cmd_vel。一个小跟班就诞生了
若修改后小车跟随效果还是不太好,建议调节一下ros_simple_follower\simple_follower\parameters\PID_param.yaml
的pid参数
结语:
本人是一个正在学习ros的小菜鸟,如果此博文对您有所帮助,请不要忘记点赞哦,谢谢。
仅供参考,如问题诸多,还望指正修改,不吝赐教,多多包含,谢谢