单线激光雷达（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的小菜鸟，如果此博文对您有所帮助，请不要忘记点赞哦，谢谢。
仅供参考，如问题诸多，还望指正修改，不吝赐教，多多包含，谢谢