how to make Tb3 follower demo have obstacle avoidance
I have to make the turtlebot3 to follow a vehicle and I have change the range of the angle so it will follow at the left back of the vehicle..Then , I am not sure what is the suitable coding for the obstacle avoidance and where to put it inside this code: Hope someone can help me as soon as possible cause it was my final year project
import rospy import os import pickle from sensormsgs.msg import LaserScan from geometrymsgs.msg import Twist import numpy as np
class follower: def init(self): rospy.loginfo('Follower node initialized') self.configdir = os.path.join(os.path.dirname(file)) self.configdir = self.configdir.replace('nodes', 'config') self.pub = rospy.Publisher('cmdvel', Twist, queuesize = 1) self.clf = pickle.load(open(self.configdir + '/clf', "rb")) self.clf2 = pickle.load(open(self.configdir + '/clf2', "rb")) self.labels = {'300':0, '30l':1, '30r':2, '450':3, '45l':4, '45r':5,'150':6, 'empty':7} rospy.loginfo('Tree initialized') self.follow()
def check_people(self):
laser_data=[]
laser_data_set=[]
result=[]
ret = 0
self.msg = rospy.wait_for_message("scan_filtered", LaserScan)
for i in range(50,-2,-1) + range(359, 269,-1):
if np.nan_to_num( self.msg.intensities[i] ) != 0 :
laser_data.append(np.nan_to_num(self.msg.intensities[i]))
elif (i+1) in range(50,-2,-1) + range(359, 269,-1) and (i-1) in range(50,-2,-1) + range(359, 269,-1) and np.nan_to_num(self.msg.intensities[i]) == 0:
laser_data.append((np.nan_to_num(self.msg.intensities[i+1])+np.nan_to_num(self.msg.intensities[i-1]))/2)
else :
laser_data.append(np.nan_to_num(self.msg.intensities[i]))
laser_data_set.append(laser_data)
[x for (x , y) in self.labels.iteritems() if y == self.clf2.predict(laser_data_set) ] ## Predict the position
if result == ['empty']:
ret = 0
else:
ret = 1
return ret
def laser_scan(self):
data_test=[]
data_test_set=[]
self.msg = rospy.wait_for_message("scan_filtered", LaserScan)
for i in range(50,-2,-1) + range(359, 269,-1):
if np.nan_to_num( self.msg.ranges[i] ) != 0 :
data_test.append(np.nan_to_num(self.msg.ranges[i]))
elif (i+1) in range(50,-2,-1) + range(359, 269,-1) and (i-1) in range(50,-2,-1) + range(359, 269,-1) and np.nan_to_num(self.msg.ranges[i]) == 0:
data_test.append((np.nan_to_num(self.msg.ranges[i+1])+np.nan_to_num(self.msg.ranges[i-1]))/2)
else :
data_test.append(np.nan_to_num(self.msg.ranges[i]))
data_test_set.append(data_test)
return [x for (x , y) in self.labels.iteritems() if y == self.clf.predict(data_test_set) ]
def follow(self):
while not rospy.is_shutdown():
check = self.check_people()
if check == 1:
x = self.laser_scan()
twist = Twist()
## Do something according to each position##
if x == ['30_0']:
twist.linear.x = 0.18; twist.angular.z = 0.0;
elif x== ['30_l']:
twist.linear.x = 0.13; twist.angular.z = 0.4;
elif x== ['30_r']:
twist.linear.x = 0.13; twist.angular.z = -0.4;
elif x== ['45_0']:
twist.linear.x = 0.18; twist.angular.z = 0.0;
elif x== ['45_l']:
twist.linear.x = 0.13; twist.angular.z = 0.3;
elif x== ['45_r']:
twist.linear.x = 0.13; twist.angular.z = -0.3;
elif x== ['15_0']:
twist.linear.x = 0.0; twist.angular.z = 0.0;
elif x== ['empty']:
twist.linear.x = 0.0; twist.angular.z = 0.0;
else:
twist.linear.x = 0.0; twist.angular.z = 0.0;
self.pub.publish(twist)
elif check == 0:
x = self.laser_scan()
twist = Twist()
if x== ['empty']:
twist.linear.x = 0.0; twist.angular.z = 0.0;
else:
twist.linear.x = 0.0; twist.angular.z = 0.4;
self.pub.publish(twist)
def main():
rospy.init_node('follower', anonymous=True)
try:
follow = follower()
except rospy.ROSInterruptException:
pass #print("Shutting down")
if name == 'main': main()
Asked by irsyad on 2020-07-14 09:52:46 UTC
Comments
Have you got any idea to solve this problem? I am also curious abuot this. Thanks~
Asked by pqLee on 2021-01-27 02:33:09 UTC