def buoy_callback(msg): global current_position,green_buoy,red_buoy,traversed_buoys [p1,p2] = find_closest_buoys(msg) if ((not p1) or (not p2)): pass else: if (traversed_buoys): previous_center = center_of_points((traversed_buoys[-2],traversed_buoys[-1])) point1_pos = three_d(previous_center) + get_perpendicular(three_d(p1)-three_d(p2),(0,0,1)) print 'going to point1',point1_pos send_waypoint(point1_pos,lookat(get_perpendicular(three_d(traversed_buoys[-2])-three_d(traversed_buoys[-1]),(0,0,1)))) center_pos = center_of_points((p1,p2)) point2_pos = three_d(center_pos) - 1.5*get_perpendicular(three_d(p1)-three_d(p2),(0,0,1)) print 'going to point2',point2_pos send_waypoint(point2_pos,lookat(get_perpendicular(three_d(p1)-three_d(p2),(0,0,1)))) print 'done' traversed_buoys.append(p1) traversed_buoys.append(p2) red_buoy = [] green_buoy = []
def buoy_callback(msg):
global current_position, green_buoy, red_buoy, traversed_buoys
[p1, p2] = find_closest_buoys(msg)
if ((not p1) or (not p2)):
pass
else:
if (traversed_buoys):
previous_center = center_of_points(
(traversed_buoys[-2], traversed_buoys[-1]))
point1_pos = three_d(previous_center) + get_perpendicular(
three_d(p1) - three_d(p2), (0, 0, 1))
print 'going to point1', point1_pos
send_waypoint(
point1_pos,
lookat(
get_perpendicular(
three_d(traversed_buoys[-2]) -
three_d(traversed_buoys[-1]), (0, 0, 1))))
center_pos = center_of_points((p1, p2))
point2_pos = three_d(center_pos) - 1.5 * get_perpendicular(
three_d(p1) - three_d(p2), (0, 0, 1))
print 'going to point2', point2_pos
send_waypoint(
point2_pos,
lookat(get_perpendicular(three_d(p1) - three_d(p2), (0, 0, 1))))
print 'done'
traversed_buoys.append(p1)
traversed_buoys.append(p2)
red_buoy = []
green_buoy = []
def check_obstacle_in_path(msg, p1, p2):
global current_position, traversed_buoys
yellow = ColorRGBA(1.0, 1.0, 0, 1.0)
for marker in msg.markers:
if (marker.color == yellow):
center_pos = center_of_points((p1, p2))
if (distance(current_position, center_pos) <= distance(
current_position,
(marker.pose.position.x, marker.pose.position.y))):
print 'yellow buoy detected in path. redirecting'
point1_pos = three_d(
center_of_points(
(marker.pose.position.x, marker.pose.position.y),
traversed_buoys[-1]))
print 'going to point1', point1_pos
send_waypoint(
point1_pos,
lookat(
get_perpendicular(
three_d((marker.pose.position.x,
marker.pose.position.y)) -
three_d(traversed_buoys[-1]), (0, 0, 1))))
point2_pos = three_d(
center_of_points(
(marker.pose.position.x, marker.pose.position.y), p2))
print 'going to point2', point2_pos
send_waypoint(
point2_pos,
lookat(
get_perpendicular(
three_d((marker.pose.position.x,
marker.pose.position.y)) - three_d(p2),
(0, 0, 1))))
point3_pos = three_d(center_pos) - 1.5 * get_perpendicular(
three_d(p1) - three_d(p2), (0, 0, 1))
print 'going to point3', point3_pos
send_waypoint(
point3_pos,
lookat(
get_perpendicular(
three_d(p1) - three_d(p2), (0, 0, 1))))
print 'done'
return True
return False
def check_obstacle_in_path(msg,p1,p2): global current_position,traversed_buoys yellow = ColorRGBA(1.0,1.0,0,1.0) for marker in msg.markers: if (marker.color == yellow): center_pos = center_of_points((p1,p2)) if (distance(current_position,center_pos) <= distance(current_position,(marker.pose.position.x,marker.pose.position.y))): print 'yellow buoy detected in path. redirecting' point1_pos = three_d(center_of_points((marker.pose.position.x,marker.pose.position.y),traversed_buoys[-1])) print 'going to point1',point1_pos send_waypoint(point1_pos,lookat(get_perpendicular(three_d((marker.pose.position.x,marker.pose.position.y))-three_d(traversed_buoys[-1]),(0,0,1)))) point2_pos = three_d(center_of_points((marker.pose.position.x,marker.pose.position.y),p2)) print 'going to point2',point2_pos send_waypoint(point2_pos,lookat(get_perpendicular(three_d((marker.pose.position.x,marker.pose.position.y))-three_d(p2),(0,0,1)))) point3_pos = three_d(center_pos) - 1.5*get_perpendicular(three_d(p1)-three_d(p2),(0,0,1)) print 'going to point3',point3_pos send_waypoint(point3_pos,lookat(get_perpendicular(three_d(p1)-three_d(p2),(0,0,1)))) print 'done' return True return False