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