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)
