單線激光雷達（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的小菜鳥，如果此博文對您有所幫助，請不要忘記點贊哦，謝謝，
僅供參考，如問題諸多，還望指正修改，不吝賜教，多多包含，謝謝