在ROS中实现基于激光雷达的目标跟随

ubuntu版本：20.04

ROS版本：Noetic Ninjemys

本文使用了github上开源的ros_simple_follower功能包来实现基于激光雷达的目标跟随。

源码地址：https://github.com/rauwuckl/ros_simple_follower

下面讲解该功能包的使用方法，以及本人在调试过程中遇到的一些问题和解决方法。

一、下载源码

从github上下载源码的压缩包，解压后复制到自己工作空间的src文件夹下，然后编译工作空间并配置环境变量。

二、修改话题名称

将ros_simple_follower包中follower.py文件第25行的/cmd_vel/yolo话题改为自己小车的速度控制话题名称。

self.cmdVelPublisher = rospy.Publisher('/cmd_vel/yolo', Twist, queue_size =3)

将ros_simple_follower包中laserTracker.py文件第18行的/hokuyo_base/scan话题改为自己的激光雷达扫描数据对应的话题名称。

self.scanSubscriber = rospy.Subscriber('/hokuyo_base/scan', LaserScan, self.registerScan)

三、修改头文件

如果python版本是python3，需要将follower.py文件和laserTracker.py文件中导入的头文件中的thread模块名称更改为_thread，否则在运行laser_follower.launch文件时会出现如下报错：

ModuleNotFoundError: No module named 'thread'

四、下载安装assimp库

修改完话题名和头文件后再次运行laser_follower.launch文件时出现了如下报错：

ModuleNotFoundError: No module named 'simple_follower'

通过查找资料，发现是因为python3的lib中缺少assimp模型加载库，需要自己手动下载安装。

下载地址：https://github.com/assimp/assimp

将下载好的压缩包解压到主目录下，重命名文件夹，去掉文件名末尾的-master。

打开/assimp/port/PyAssimp，然后右键空白处选择在终端中打开，输入如下指令：

sudo python setup.py install

安装完成后，打开新的终端，重新对工作空间进行编译并配置环境变量。

五、注释部分代码

成功安装assimp库之后再次运行laser_follower.launch文件，程序没有报错，但小车无法对前方物体进行跟随，通过查看/object_tracker/current_position话题（在laserTracker.py文件中发布，消息内容为距离激光雷达最近的障碍物与激光雷达的相对位置数据），发现程序已经检测到了前方物体，如下图所示：

检查follower.py文件中的代码发现，该程序在启动后，需要通过按下某个按键才能够激活跟随功能。为了使程序在运行之后能够直接启动跟随功能，这里注释掉与按键控制相关的代码，并将跟随功能设置为默认开启，修改之后的follower.py代码如下：

#!/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 simple_follower.msg import position as PositionMsg
from std_msgs.msg import String as StringMsg

class Follower:
	def __init__(self):
		# as soon as we stop receiving Joy messages from the ps3 controller we stop all movement:
		# self.controllerLossTimer = threading.Timer(1, self.controllerLoss) #if we lose connection
		# self.controllerLossTimer.start()
		# self.switchMode= rospy.get_param('~switchMode') # if this is set to False the O button has to be kept pressed in order for it to move
		self.max_speed = rospy.get_param('~maxSpeed') 
		# self.controllButtonIndex = rospy.get_param('~controllButtonIndex')

		# self.buttonCallbackBusy=False
		self.active=True

		self.cmdVelPublisher = rospy.Publisher('/cmd_vel', Twist, queue_size =3)
		# the topic for the messages from the ps3 controller (game pad)
		# self.joySubscriber = rospy.Subscriber('joy', Joy, self.buttonCallback)

		# the topic for the tracker that gives us the current position of the object we are following
		self.positionSubscriber = rospy.Subscriber('/object_tracker/current_position', PositionMsg, self.positionUpdateCallback)
		# an info string from that tracker. E.g. telling us if we lost the object
		self.trackerInfoSubscriber = rospy.Subscriber('/object_tracker/info', StringMsg, self.trackerInfoCallback)

		# PID parameters first is angular, dist
		targetDist = rospy.get_param('~targetDist')
		PID_param = rospy.get_param('~PID_controller')
		# the first parameter is the angular target (0 degrees always) the second is the target distance (say 1 meter)
		self.PID_controller = simplePID([0, targetDist], PID_param['P'], PID_param['I'], PID_param['D'])

		# this method gets called when the process is killed with Ctrl+C
		# rospy.on_shutdown(self.controllerLoss)

	def trackerInfoCallback(self, info):
		# we do not handle any info from the object tracker specifically at the moment. just ignore that we lost the object for example
		rospy.logwarn(info.data)
	
	def positionUpdateCallback(self, position):
		# gets called whenever we receive a new position. It will then update the motorcomand
	
		if(not(self.active)):
			return #if we are not active we will return imediatly without doing anything

		angleX= position.angleX
		distance = position.distance

		rospy.loginfo('Angle: {}, Distance: {}, '.format(angleX, distance))
		
		# call the PID controller to update it and get new speeds
		[uncliped_ang_speed, uncliped_lin_speed] = self.PID_controller.update([angleX, distance])
			
		# clip these speeds to be less then the maximal speed specified above
		angularSpeed = np.clip(-uncliped_ang_speed, -self.max_speed, self.max_speed)
		linearSpeed  = np.clip(-uncliped_lin_speed, -self.max_speed, self.max_speed)
		
		# create the Twist message to send to the cmd_vel topic
		velocity = Twist()	
		velocity.linear = Vector3(linearSpeed,0,0.)
		velocity.angular= Vector3(0., 0.,angularSpeed)
		rospy.loginfo('linearSpeed: {}, angularSpeed: {}'.format(linearSpeed, angularSpeed))
		self.cmdVelPublisher.publish(velocity)
		

	def buttonCallback(self, joy_data):
		# this method gets called whenever we receive a message from the joy stick

		# there is a timer that always gets reset if we have a new joy stick message
		# if it runs out we know that we have lost connection and the controllerLoss function
		# will be called
		self.controllerLossTimer.cancel()
		self.controllerLossTimer = threading.Timer(0.5, self.controllerLoss)
		self.controllerLossTimer.start()

		# if we are in switch mode, one button press will make the follower active / inactive 
		# but 'one' button press will be visible in roughly 10 joy messages (since they get published to fast) 
		# so we need to drop the remaining 9
		
		if self.buttonCallbackBusy:
			# we are busy with dealing with the last message
			return 
		else:
			# we are not busy. i.e. there is a real 'new' button press
			# we deal with it in a seperate thread to be able to drop the other joy messages arriving in the mean
			# time
			thread.start_new_thread(self.threadedButtonCallback,  (joy_data, ))

	def threadedButtonCallback(self, joy_data):
		self.buttonCallbackBusy = True

		if(joy_data.buttons[self.controllButtonIndex]==self.switchMode and self.active):
			# we are active
			# switchMode = false: we will always be inactive whenever the button is not pressed (buttons[index]==false)
			# switchMode = true: we will only become inactive if we press the button. (if we keep pressing it, 
			# we would alternate between active and not in 0.5 second intervalls)
			rospy.loginfo('stoping')
			self.stopMoving()
			self.active = False
			rospy.sleep(0.5)
		elif(joy_data.buttons[self.controllButtonIndex]==True and not(self.active)):
			# if we are not active and just pressed the button (or are constantly pressing it) we become active
			rospy.loginfo('activating')
			self.active = True #enable response
			rospy.sleep(0.5)

		self.buttonCallbackBusy = False

	def stopMoving(self):
		velocity = Twist()
		velocity.linear = Vector3(0.,0.,0.)
		velocity.angular= Vector3(0.,0.,0.)
		self.cmdVelPublisher.publish(velocity)

	def controllerLoss(self):
		# we lost connection so we will stop moving and become inactive
		self.stopMoving()
		self.active = False
		rospy.loginfo('lost connection')


		
class simplePID:
	'''very simple discrete PID controller'''
	def __init__(self, target, P, I, D):
		'''Create a discrete PID controller
		each of the parameters may be a vector if they have the same length
		
		Args:
		target (double) -- the target value(s)
		P, I, D (double)-- the PID parameter

		'''

		# check if parameter shapes are compatabile. 
		if(not(np.size(P)==np.size(I)==np.size(D)) or ((np.size(target)==1) and np.size(P)!=1) or (np.size(target )!=1 and (np.size(P) != np.size(target) and (np.size(P) != 1)))):
			raise TypeError('input parameters shape is not compatable')
		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):
		'''Updates the PID controller. 

		Args:
			current_value (double): vector/number of same legth as the target given in the constructor

		Returns:
			controll signal (double): vector of same length as the target

		'''
		current_value=np.array(current_value)
		if(np.size(current_value) != np.size(self.setPoint)):
			raise TypeError('current_value and target do not have the same shape')
		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
		
		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__':
	print('starting')
	rospy.init_node('follower')
	follower = Follower()
	try:
		rospy.spin()
	except rospy.ROSInterruptException:
		print('exception')

六、修改time.clock()函数

修改完follower.py代码后再次运行laser_follower.launch文件，又出现了新的报错，如下所示：

AttributeError: module 'time' has no attribute 'clock'

通过查找资料发现是因为python在3.8版本以后不再支持time.clock()，可以使用time.perf_counter()来替换。

将follower.py文件第173行和第180行的time.clock()替换为time.perf_counter()即可。

七、调整参数

启动自己的小车底盘控制和激光雷达之后，运行laser_follower.launch文件启动目标跟随功能。

roslaunch simple_follower laser_follower.launch

此时小车应该已经能够对目标正常进行跟随了。

如果发现小车一直往后退，可能是因为设置的目标距离（默认是2m）大于激光雷达的检测距离，可以在follower.launch文件中进行修改。

<launch>
  <node name='follower' pkg="simple_follower" type="follower.py">
    <!-- switchMode: (if true one button press will change betwenn active, not active. If false the button will have to be kept pressed all the time to be active -->
    <param name="switchMode" value="True" type="bool" />
    <!-- maximal speed (angular and linear both), tartet distance: the robot will try to keep this fixed distance -->
    <param name='maxSpeed' value='0.4' type='double' />
    <param name='targetDist' value='2.0' type='double' />
    <!-- index of the button in the buttons field of the joy message from the ps3 controller, that switches active/inactive  -->
    <param name='controllButtonIndex' value='-4' type='int' />
    <rosparam ns='PID_controller' command='load' file='$(find simple_follower)/parameters/PID_param.yaml' />
  </node>
</launch>

其中，maxSpeed参数表示小车在自动跟随过程中可以达到的最大速度，targetDist参数表示小车在跟随过程中与目标保持的距离（如果小车到目标的实际距离大于这个距离，则小车会向目标靠近，反之小车会远离目标），可以根据需求自行更改。switchMode和controllButtonIndex是和按键控制相关的参数，可以忽略。

注意：小车只会一直跟随激光雷达检测到的距离小车最近的物体。

另外，可以在laserTracker.launch文件中调整滤波参数。

<launch>
  <node name='laser_tracker' pkg="simple_follower" type="laserTracker.py">
    <!-- This is to avoid treating noise in the laser ranges as objects -->
    <!-- we check for all range measurements around the currently closest distance measurement (size of this window specified by winSize) if they where similarly close in the last scan (threshold specified by deltaDist-->
    <param name="winSize" value="2" type="int" />
    <param name="deltaDist" value="0.2" type="double" />
  </node>
</launch>

其中，winSize参数表示滤波范围（默认是2m），deltaDist参数表示如果激光雷达某一条线连续的两次扫描结果相差0.2m以上，则认为是噪声，会自动滤除掉。

最后，通过调整PID_param.yaml文件中的PID参数可以进一步优化小车的跟随效果。

P: [0.7, 0.6] 
I: [0.07,0.04]
D: [0.00, 0.000]

其中，第一列是控制小车旋转速度的PID参数，第二列是控制小车直行速度的PID参数，可以根据实际效果进行调整。