def obstacle_avoidance(self):
while not rospy.is_shutdown():
if not self.noturn:
self.scan = msg
quad = [np.array(self.scan.ranges[i*self.range:(i+1)*self.range]) for i in range(self.num_quads)]
quad = [quad[0],quad[3]]
# remove the zeros
quad = [q[q.nonzero()] for q in quad]
quad_average = np.array([np.mean(q) for q in quad])
# if nan, make big!
for i, e in enumerate(quad_average):
if np.isnan(e):
quad_average[i] = 10
# get the range and quadrant number (q, i) of the closet object
q = np.min(quad_average[quad_average.nonzero()])
i = list(quad_average).index(q)
# if i'm still close to an object
if not self.adjust_angle_flag:
if self.turn_target:
self.twist.angular.z = self.turn_k*(angle_diff(self.turn_target, self.yaw))
# if it has reached the desired angle
# print "angle_diff: ", np.abs(angle_diff(self.turn_target, self.yaw))
if np.abs(angle_diff(self.turn_target, self.yaw)) < self.epsilon:
self.params_to_go_forward()
self.adjust_angle_flag = True
else:
if q < self.object_distance:
if self.yaw:
self.turn_target = [self.yaw-math.pi/2, self.yaw+math.pi/2][i]
else:
self.params_to_go_forward()
# once I'm in safe distance
else:
self.adjust_original_angle()
else:
self.twist.linear.x = 1
if self.moving_object_detected:
return ComboPFOA.PERSON_FOLLOW_STATE
def adjust_original_angle(self):
self.twist.angular.z = self.turn_k*(angle_diff(self.final_target, self.yaw))
self.adjust_angle_flag = True
if np.abs(angle_diff(self.final_target, self.yaw)) < self.epsilon:
self.params_to_go_forward()
self.adjust_angle_flag = False
