Beispiel #1
0
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 = []
Beispiel #2
0
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 = []
Beispiel #3
0
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
Beispiel #4
0
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