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