def get_next_path(self): # Here we want to find the item in the list which is the closest to the # current position of the robot. closest = 999999.9 closest_index = 0 for i in range(len(self.target_list)): d = dist((self.pose.position.x, self.pose.position.y), (self.target_list[i][0], self.target_list[i][1])) if d < closest: closest = d closest_index = i # Get the target that is closest pos_t = self.target_list.pop(closest_index) # Find the x,y values for robot position and target x_t, y_t = self.m_s.xy_from_xy_pos(pos_t[0], pos_t[1]) x_r, y_r = self.m_s.xy_from_xy_pos(self.pose.position.x, self.pose.position.y) # Search for path path = astar_search(self.m_s, (x_r, y_r), (x_t, y_t)) return path
def run(self): # Here we need to act on the information previously received regarding # robot pose and list of coordinates to follow. # Using these parameters we have to figure out: # 1) When we are close enough to a coordinate to knock it off the list # 1.1) Is the target the goal? If so -> What to do? # 2) What the next coordinate is # 3) The angle towards the next coordinate # 4) The angular and linear speed we need to set in order to hit the coordinate # Find closeness to current coordinate curr_dist = dist((self.pose.position.x, self.pose.position.y), self.curr_target) if curr_dist < self.coord_dist_cutoff: # Drop off a marker to show that we have been here self.taken_markers.place_marker([copy.deepcopy(self.real_pose.pose.position)], [copy.deepcopy(self.real_pose.pose.orientation)]) # Check if we have arrived at the last point in the list (the ultimate goal) if len(self.xy_pos_path) == 0: if curr_dist < 0.2: # Poop goal marker self.goal_markers.place_marker([self.real_pose.pose.position], [self.real_pose.pose.orientation]) # Check if there are more targets to go to if len(self.target_list) > 0: # Change the colour for the planned path and path taken to differentiate # it from the previous path self.taken_markers.random_color() self.planned_markers.random_color() # Find a new target and path self.xy_path = self.get_next_path() # Convert the path to xy_pos format self.convert_current_path() # Mark it self.mark_current_path() # if no more targets go to sleep else: rospy.sleep() # If we have more coordinates to visit pop off the next one! else: self.curr_target = self.xy_pos_path.pop(0) curr_dist = dist((self.pose.position.x, self.pose.position.y), self.curr_target) # Adjust the speed depending on how close we are to the current goal if len(self.xy_pos_path) < 5: self.K1 = self.K1_slow self.K2 = self.K2_slow else: self.K1 = self.K1_default self.K2 = self.K2_default # Figure out the angle we want to travel in to reach the current target # Assumption: This is given by atan2 between the target and the robot? # This makes sense as it would transpose the origin to the centre of the # robot, leaving the calculation to a "virtual" coordinate system around # the robot. theta = math.atan2(self.curr_target[1] - self.pose.position.y, self.curr_target[0] - self.pose.position.x) # Find the difference in theta that should go towards zero when the # correct heading is reached d_theta = theta - self.yaw d_theta = self.normalize(d_theta) # Find a new set of linear and angular speeds to publish lin_speed = self.new_speed(curr_dist, d_theta) ang_speed = self.new_angular_speed(d_theta) self.vel.linear.x = lin_speed self.vel.angular.z = ang_speed self.vel_publisher.publish(self.vel)