def circle_dock_bays(self): ''' Circle the dock until the bays are identified by the perception service ''' done_circle = False @txros.util.cancellableInlineCallbacks def circle_bay(): yield self.navigator.move.look_at(self.dock_pose).set_position(self.dock_pose)\ .backward(self.CIRCLE_RADIUS).look_at(self.dock_pose).go() yield self.navigator.move.circle_point(self.dock_pose, direction='ccw').go() done_circle = True # noqa flake8 doesn't see that it is defined above print_good("Circling dock to get bays") circle_bay() start_time = self.navigator.nh.get_time() while self.navigator.nh.get_time() - start_time < genpy.Duration(self.BAY_SEARCH_TIMEOUT) and not done_circle: res = yield self.call_get_bays() if not res.success: yield self.navigator.nh.sleep(0.1) continue self.bay_poses = (mil_tools.rosmsg_to_numpy(res.bays[0]), mil_tools.rosmsg_to_numpy(res.bays[1]), mil_tools.rosmsg_to_numpy(res.bays[2])) self.bay_normal = mil_tools.rosmsg_to_numpy(res.normal) print_good("Got GetDockShapes response") return raise Exception("Bays not found after circling or timed out")
def get_colored_buoy(self, color): """ Returns the closest colored buoy with the specified color """ buoy_field = yield self.database_query("BuoyField") buoy_field_point = mil_tools.rosmsg_to_numpy( buoy_field.objects[0].position) def _dist_from_bf(pt): return np.linalg.norm(buoy_field_point - pt) totems = yield self.database_query("totem") correct_colored = [ totem for totem in totems.objects if np.all( np.round( mil_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g', 'b'])) == color) ] if len(correct_colored) == 0: closest = None else: closest = sorted(correct_colored, key=lambda totem: _dist_from_bf( mil_tools.rosmsg_to_numpy(totem.position)))[0] defer.returnValue(closest)
def get_dock_search_markers(self): left_res = yield self.navigator.database_query("DockEdgeLeft") left_pose = mil_tools.rosmsg_to_numpy(left_res.objects[0].position) right_res = yield self.navigator.database_query("DockEdgeRight") right_pose = mil_tools.rosmsg_to_numpy(right_res.objects[0].position) search_line_vector = right_pose - left_pose search_line_vector_normal = search_line_vector / np.linalg.norm(search_line_vector) if np.isnan(search_line_vector_normal[0]): raise Exception("Gate Edge Markers are not in a line. Perhaps they were not placed") rot_right = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 0]]) search_line_rot = rot_right.dot(search_line_vector_normal) left_search = left_pose + search_line_rot * self.MARKER_DISTANCE right_search = right_pose + search_line_rot * self.MARKER_DISTANCE move_left = self.navigator.move.set_position(left_search).look_at(left_pose) move_right = self.navigator.move.set_position(right_search).look_at(right_pose) pose = self.navigator.pose[0][:2] distance_test = np.array([np.linalg.norm(pose - move_left.position[:2]), np.linalg.norm(pose - move_right.position[:2])]) if np.argmin(distance_test) == 0: self.search_moves = (move_left, move_right) else: self.search_moves = (move_right, move_left)
def get_closest_object(navigator): pose = yield navigator.tx_pose buoy_field = yield navigator.database_query("BuoyField") buoy_field_np = mil_tools.rosmsg_to_numpy(buoy_field.objects[0].position) # Find which totems we haven't explored yet totems = yield navigator.database_query("all", raise_exception=False) if not totems.found: # Need to search for more totems defer.returnValue([None, explored_ids]) u_totems = [totem for totem in totems.objects if totem.id not in explored_ids] u_totems_np = map(lambda totem: mil_tools.rosmsg_to_numpy(totem.position), u_totems) if len(u_totems_np) == 0: defer.returnValue([None, explored_ids]) # Find the closest buoys, favoring the ones closest to the boat. # J = wa * ca ** 2 + wb * c2 ** 2 # a := boat # b := buoy field ROI wa = .6 wb = .4 ca = np.linalg.norm(u_totems_np - pose[0], axis=1) cb = np.linalg.norm(u_totems_np - buoy_field_np, axis=1) J = wa * ca + wb * cb target_totem = u_totems[np.argmin(J)] explored_ids.append(target_totem.id) defer.returnValue([target_totem, explored_ids])
def from_vrx_urdf(cls, urdf_string): urdf = URDF.from_xml_string(urdf_string) buff = tf2_ros.Buffer() listener = tf2_ros.TransformListener(buff) # noqa names = [] positions = [] angles = [] for link in urdf.links: find = link.name.find('_propeller_link') if find == -1: continue name = link.name[:find] try: trans = buff.lookup_transform('base_link', link.name, rospy.Time(), rospy.Duration(10)) except tf2_ros.TransformException as e: raise Exception(e) translation = rosmsg_to_numpy(trans.transform.translation) rot = rosmsg_to_numpy(trans.transform.rotation) yaw = euler_from_quaternion(rot)[2] names.append(name) positions.append(translation[0:2]) angles.append(yaw) return cls(names, positions, angles, vrx_force_to_command, (250., -100.))
def run(self, parameters): # middle_point = np.array([-10, -70, 0]) est_coral_survey = yield self.database_query("CoralSurvey") # Going to get all the objects and using the closest one as the totem totem = yield self.database_query("all") # Get the closest totem object to the boat totems_np = map(lambda obj: mil_tools.rosmsg_to_numpy(obj.position), totem.objects) dist = map(lambda totem_np: np.linalg.norm( totem_np - mil_tools.rosmsg_to_numpy(est_coral_survey.objects[0].position)), totems_np) middle_point = totems_np[np.argmin(dist)] print "Totem sorted:", totems_np print "Totem selected: ", totems_np[0] quad = yield self.mission_params["acoustic_pinger_active_index_correct"].get() waypoint_from_center = np.array([10 * np.sqrt(2)]) # Publish ogrid with boundaries to stay inside ogrid = OgridFactory(middle_point, draw_borders=True) msg = ogrid.get_message() latched = self.latching_publisher("/mission_ogrid", OccupancyGrid, msg) # Construct waypoint list along NSEW directions then rotate 45 degrees to get a good spot to go to. directions = [EAST, NORTH, WEST, SOUTH] # for quad in quads_to_search: mid = self.move.set_position(middle_point).set_orientation(directions[quad - 1]) search_center = mid.yaw_left(45, "deg").forward(waypoint_from_center).set_orientation(NORTH) yield search_center.left(6).go() yield self.move.circle_point(search_center.position, direction='cw').go() yield self.mission_params["coral_survey_shape1"].set(shapes[0]) latched.cancel() defer.returnValue(None)
def get_dock_search_markers(self): left_res = yield self.navigator.database_query("DockEdgeLeft") left_pose = mil_tools.rosmsg_to_numpy(left_res.objects[0].position) right_res = yield self.navigator.database_query("DockEdgeRight") right_pose = mil_tools.rosmsg_to_numpy(right_res.objects[0].position) search_line_vector = right_pose - left_pose search_line_vector_normal = search_line_vector / np.linalg.norm(search_line_vector) if np.isnan(search_line_vector_normal[0]): raise Exception("Gate Edge Markers are not in a line. Perhaps they were not placed") rot_right = np.array([[0, 1, 0], [-1, 0, 0], [0,0,0]]) search_line_rot = rot_right.dot(search_line_vector_normal) left_search = left_pose + search_line_rot * self.MARKER_DISTANCE right_search = right_pose + search_line_rot * self.MARKER_DISTANCE move_left = self.navigator.move.set_position(left_search).look_at(left_pose) move_right = self.navigator.move.set_position(right_search).look_at(right_pose) pose = self.navigator.pose[0][:2] distance_test = np.array([np.linalg.norm(pose - move_left.position[:2]), np.linalg.norm(pose - move_right.position[:2])]) if np.argmin(distance_test) == 0: self.search_moves = (move_left, move_right) else: self.search_moves = (move_right, move_left)
def circle_dock_bays(self): ''' Circle the dock until the bays are identified by the perception service ''' done_circle = False @txros.util.cancellableInlineCallbacks def circle_bay(): yield self.navigator.move.look_at(self.dock_pose).set_position(self.dock_pose).backward(self.CIRCLE_RADIUS).look_at(self.dock_pose).go() yield self.navigator.move.circle_point(self.dock_pose, direction='ccw').go() done_circle = True print_good("Circling dock to get bays") circle_bay() start_time = self.navigator.nh.get_time() while self.navigator.nh.get_time() - start_time < genpy.Duration(self.BAY_SEARCH_TIMEOUT) and not done_circle: res = yield self.call_get_bays() if not res.success: yield self.navigator.nh.sleep(0.1) continue self.bay_poses = (mil_tools.rosmsg_to_numpy(res.bays[0]), mil_tools.rosmsg_to_numpy(res.bays[1]), mil_tools.rosmsg_to_numpy(res.bays[2])) self.bay_normal = mil_tools.rosmsg_to_numpy(res.normal) print_good("Got GetDockShapes response") return raise Exception("Bays not found after circling or timed out")
def get_target_pt(self, center_frame): msg = CameraToLidarTransformRequest() msg.header.stamp = center_frame[1].header.stamp msg.header.frame_id = center_frame[1].header.frame_id msg.point = Point(x=center_frame[0][0], y=center_frame[0][1], z=0.0) msg.tolerance = 500 pose_offset = yield self.camera_lidar_tf(msg) cam_to_enu = yield self.tf_listener.get_transform( 'enu', center_frame[1].header.frame_id) normal = rosmsg_to_numpy(pose_offset.normal) normal = cam_to_enu.transform_vector(normal) normal = normal[0:2] / np.linalg.norm(normal[0:2]) normal = np.append(normal, [0]) found_pt = rosmsg_to_numpy(pose_offset.closest) found_pt = cam_to_enu.transform_point(found_pt) found_pt[2] = 0 # Extend out by normal multiplier normal *= 3 found_pt_1 = found_pt + normal found_pt_2 = found_pt + -1 * normal # Which is closer boat_pos = (yield self.tx_pose)[0] if np.linalg.norm(found_pt_1 - boat_pos) > np.linalg.norm(found_pt_2 - boat_pos): found_pt = found_pt_2 else: found_pt = found_pt_1 defer.returnValue(found_pt)
def get_closest_totem(self, explored_ids): pose = yield self.tx_pose buoy_field = yield self.database_query("BuoyField") buoy_field_np = mil_tools.rosmsg_to_numpy( buoy_field.objects[0].position) # Find which totems we haven't explored yet totems = yield self.database_query("totem", raise_exception=False) if not totems.found: # Need to search for more totems defer.returnValue([None, explored_ids]) u_totems = [ totem for totem in totems.objects if totem.id not in explored_ids ] u_totems_np = map( lambda totem: mil_tools.rosmsg_to_numpy(totem.position), u_totems) if len(u_totems_np) == 0: defer.returnValue([None, explored_ids]) # Find the closest buoys, favoring the ones closest to the boat. # J = wa * ca ** 2 + wb * c2 ** 2 # a := boat # b := buoy field ROI wa = .7 wb = .3 ca = np.linalg.norm(u_totems_np - pose[0], axis=1) cb = np.linalg.norm(u_totems_np - buoy_field_np, axis=1) J = wa * ca + wb * cb target_totem = u_totems[np.argmin(J)] explored_ids.append(target_totem.id) defer.returnValue([target_totem, explored_ids])
def get_bouy_go_round_target(self): return_array = [] #gets the xy and state of the scan the code from the database scan_the_code = np.array([]) res = yield self.database_query('stc_platform') #makes sure that only 1 scan the code exists assert len(res.objects) == 1 #raises error if the scan the code platform is nto if not res.found: raise TaskException(query + ' not found in object database') point = rosmsg_to_numpy(res.objects[0].pose.position)[:2] #runs the function that retrives/runs the scan the code state True for circle scan #the code, False for circle the black totem scan_the_code = point return_array.append(scan_the_code) #print scan_the_code #this portion of the method gets the location of the nearest black totem #gets all of the black totems from the database num_of_black_totems = 1 black_totems = yield self.database_query('totem_black') black_totems_poses = [] for i in black_totems.objects: point = rosmsg_to_numpy(i.pose.position)[:2] black_totems_poses.append(point) #the follwing determins which is the closest #i wish python had a do while loop closest = black_totems_poses[0] dist = ((black_totems_poses[0][0] - self.pose[0][0])**2) + ( (black_totems_poses[0][1] - self.pose[0][1])**2) j = 0 #an index for populating the dist_temp array while j < len(black_totems_poses): dist_temp = ((black_totems_poses[j][0] - self.pose[0][0])**2) + ( (black_totems_poses[j][1] - self.pose[0][1])**2) if dist_temp < dist: dist = dist_temp closest = black_totems[j] j += 1 #closest now has the position of the closest black totem #closest is a np array return_array.append(closest) #returnValue has the scan the code and closest black totem location defer.returnValue(return_array)
def get_center_frame(self): msgf = yield self.bboxsub.get_next_message() msg = msgf.objects[0] # print msg c1 = rosmsg_to_numpy(msg.points[0]) c2 = rosmsg_to_numpy(msg.points[1]) tmp = (((c1 + c2) / 2.0), msgf, msg.name) defer.returnValue(tmp)
def get_objects(self): """Get position of 3 gates from database""" gate_1 = yield self.navigator.database_query("Gate_1") assert gate_1.found, "Gate 1 Not found" gate_1_pos = mil_tools.rosmsg_to_numpy(gate_1.objects[0].position)[:2] gate_2 = yield self.navigator.database_query("Gate_2") assert gate_2.found, "Gate 2 Not found" gate_2_pos = mil_tools.rosmsg_to_numpy(gate_2.objects[0].position)[:2] gate_3 = yield self.navigator.database_query("Gate_3") assert gate_3.found, "Gate 3 Not found" gate_3_pos = mil_tools.rosmsg_to_numpy(gate_3.objects[0].position)[:2] self.gate_poses = np.array([gate_1_pos, gate_2_pos, gate_3_pos])
def go_through_next_two_buoys(self): buoys = yield self.get_two_closest_cones(TwoClosestConesRequest()) self.task_done = buoys.no_more_buoys print(buoys) if self.task_done: return pos1 = rosmsg_to_numpy(buoys.object1) pos2 = rosmsg_to_numpy(buoys.object2) gate = self.get_gate(pos1, pos2, (yield self.tx_pose)[0]) yield self.go_thru_gate(gate)
def get_objects(self): """Get position of 3 gates from database""" gate_1 = yield self.database_query("Gate_1") assert gate_1.found, "Gate 1 Not found" gate_1_pos = mil_tools.rosmsg_to_numpy(gate_1.objects[0].position)[:2] gate_2 = yield self.database_query("Gate_2") assert gate_2.found, "Gate 2 Not found" gate_2_pos = mil_tools.rosmsg_to_numpy(gate_2.objects[0].position)[:2] gate_3 = yield self.database_query("Gate_3") assert gate_3.found, "Gate 3 Not found" gate_3_pos = mil_tools.rosmsg_to_numpy(gate_3.objects[0].position)[:2] self.gate_poses = np.array([gate_1_pos, gate_2_pos, gate_3_pos])
def get_closest_objects(position, objects, max_len=3, max_dist=30): num = len(objects) idx = max_len if num < max_len: idx = num objects = sorted(objects, key=lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position))) objects = objects[:idx] dists = map(lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position)), objects) final_objs = [] for i, d in enumerate(dists): if d < max_dist: final_objs.append(objects[i]) else: break return final_objs
def move_cb(self, msg): fprint("Move request received!", msg_color="blue") if msg.move_type not in ['hold', 'drive', 'skid', 'circle']: fprint("Move type '{}' not found".format(msg.move_type), msg_color='red') self.move_server.set_aborted(MoveResult('move_type')) return self.blind = msg.blind p = PoseStamped() p.header = make_header(frame="enu") p.pose = msg.goal self.goal_pose_pub.publish(p) # Sleep before you continue rospy.sleep(1) yaw = trns.euler_from_quaternion(rosmsg_to_numpy( msg.goal.orientation))[2] if not self.is_feasible( np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)): fprint("Not feasible", msg_color='red') self.move_server.set_aborted(MoveResult('occupied')) return fprint("Finished move!", newline=2) self.move_server.set_succeeded(MoveResult(''))
def circle_search(self): platform_np = mil_tools.rosmsg_to_numpy( self.waypoint_res.objects[0].position) yield self.move.look_at(platform_np).set_position(platform_np).backward(self.circle_radius)\ .yaw_left(90, unit='deg').go(move_type="drive") done_circle = False @txros.util.cancellableInlineCallbacks def do_circle(): yield self.move.circle_point(platform_np, direction=self.circle_direction).go() done_circle = True # noqa flake8 cant see that it is defined above do_circle() while not done_circle: res = yield self.get_any_shape() if res is False: yield self.nh.sleep(0.25) continue fprint( "Shape ({}found, using normal to look at other 3 shapes if needed" .format(res[0]), title="DETECT DELIVER", msg_color="green") # circle_defer.cancel() shape_color, found_shape_pose = res if self.correct_shape(shape_color): self.shape_pose = found_shape_pose return # Pick other 3 to look at rot_right = np.array([[0, -1], [1, 0]]) (shape_point, shape_normal) = found_shape_pose rotated_norm = np.append(rot_right.dot(shape_normal[:2]), 0) center_point = shape_point - shape_normal * (self.platform_radius / 2.0) point_opposite_side = center_point - shape_normal * self.circle_radius move_opposite_side = self.move.set_position( point_opposite_side).look_at(center_point).yaw_left(90, unit='deg') left_or_whatever_point = center_point + rotated_norm * self.circle_radius move_left_or_whatever = self.move.set_position( left_or_whatever_point).look_at(center_point).yaw_left( 90, unit='deg') right_or_whatever_point = center_point - rotated_norm * self.circle_radius move_right_or_whatever = self.move.set_position( right_or_whatever_point).look_at(center_point).yaw_left( 90, unit='deg') yield self.search_sides( (move_right_or_whatever, move_opposite_side, move_left_or_whatever)) return fprint("No shape found after complete circle", title="DETECT DELIVER", msg_color='red') raise Exception("No shape found on platform")
def move_cb(self, msg): fprint("Move request received!", msg_color="blue") if msg.move_type not in ['hold', 'drive', 'skid', 'circle']: fprint("Move type '{}' not found".format(msg.move_type), msg_color='red') self.move_server.set_aborted(MoveResult('move_type')) return self.blind = msg.blind p = PoseStamped() p.header = make_header(frame="enu") p.pose = msg.goal self.goal_pose_pub.publish(p) # Sleep before you continue rospy.sleep(1) yaw = trns.euler_from_quaternion(rosmsg_to_numpy(msg.goal.orientation))[2] if not self.is_feasible(np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)): fprint("Not feasible", msg_color='red') self.move_server.set_aborted(MoveResult('occupied')) return fprint("Finished move!", newline=2) self.move_server.set_succeeded(MoveResult(''))
def run(self, parameters): # Go to autonomous mode yield self.change_wrench('autonomous') if not parameters.pcodar: self.send_feedback('Please click between the end tower of the navigation pass.') target_point = yield self.rviz_point.get_next_message() target_point = rosmsg_to_numpy(target_point.point) us = (yield self.tx_pose)[0] distance = np.linalg.norm(target_point - us) + self.END_MARGIN_METERS distance_per_move = distance / parameters.num_moves for i in range(parameters.num_moves): self.send_feedback("Doing move {}/{}".format(i + 1, parameters.num_moves)) yield self.move.look_at(target_point).forward(distance_per_move).go(blind=True) defer.returnValue(True) else: _, closest_reds = yield self.get_sorted_objects("totem_red", 2) _, closest_greens = yield self.get_sorted_objects("totem_green", 2) # Rename the totems for their symantic name green_close = closest_greens[0] green_far = closest_greens[1] red_close = closest_reds[0] red_far = closest_reds[1] # Get the two center points between gata markers begin_midpoint = (green_close + red_close) / 2.0 end_midpoint = (green_far + red_far) / 2.0 # Start a little behind the entrance yield self.move.set_position(begin_midpoint).look_at(end_midpoint).backward(self.START_MARGIN_METERS).go() # Then move a little passed the exit yield self.move.look_at(end_midpoint).set_position(end_midpoint)\ .forward(self.END_MARGIN_METERS).go(blind=True) defer.returnValue(True)
def get_sorted_objects(self, name, n=-1, throw=True, **kwargs): ''' Get the closest N objects with a particular name from the PCODAR database @param name: the name of the object @param n: the number of objects to get, if -1, get all of them @param throw: If true, raise exception if not enough objects present @param **kwargs: other kwargs to give to database_query @return tuple([sorted_object_messages, [object_positions]) of sorted object messages and their positions as a Nx3 numpy array. ''' objects = (yield self.database_query(object_name=name, **kwargs)).objects if n != -1 and len(objects) < n: if throw: raise Exception('Could not get {} {} objects'.format(n, name)) else: n = len(objects) if n == 0: defer.returnValue(None) positions = np.empty((len(objects), 3)) for i, obj in enumerate(objects): positions[i, :] = mil_tools.rosmsg_to_numpy(obj.pose.position) nav_pose = (yield self.tx_pose)[0] distances = np.linalg.norm(positions - nav_pose, axis=1) distances_argsort = np.argsort(distances) if n != -1: distances_argsort = distances_argsort[:n] objects_sorted = [objects[i] for i in distances_argsort] defer.returnValue((objects_sorted, positions[distances_argsort, :]))
def go_to_objects(navigator, position, objs): objects = get_closest_objects(position, objs) for o in objects: fprint("MOVING TO OBJECT WITH ID {}".format(o.id), msg_color="green") yield navigator.nh.sleep(5) pos = nt.rosmsg_to_numpy(o.position) yield navigator.move.look_at(pos).set_position(pos).backward(7).go()
def go_to_objects(navigator, position, objs): objects = get_closest_objects(position, objs) for o in objects: fprint("MOVING TO OBJECT WITH ID {}".format(o.id), msg_color="green") yield navigator.nh.sleep(5) pos = nt.rosmsg_to_numpy(o.position) yield navigator.move.look_at(pos).set_position(pos).backward(7).go()
def find_stc(self): pose = None # see if we already got scan the code tower try: _, poses = yield self.get_sorted_objects(name='stc_platform', n=1) pose = poses[0] # incase stc platform not already identified except Exception as e: # get all pcodar objects try: _, poses = yield self.get_sorted_objects(name='UNKNOWN', n=-1) # if no pcodar objects, drive forward except Exception as e: yield self.move.forward(50).go() # get all pcodar objects _, poses = yield self.get_sorted_objects(name='UNKNOWN', n=-1) # if still no pcodar objects, guess RGB and exit mission # go to nearest obj to get better data on that obj print 'going to nearest object' yield self.move.set_position(poses[0]).go() # get data on closest obj msgs, poses = yield self.get_sorted_objects(name='UNKNOWN', n=1) if np.linalg.norm(rosmsg_to_numpy(msgs[0].scale)) > 6.64: # much bigger than scale of stc # then we found the dock yield self.pcodar_label(msgs[0].id, 'dock') # get other things msgs, poses = yield self.get_sorted_objects(name='UNKNOWN', n=1) # if no other things, throw error and exit mission yield self.pcodar_label(msgs[0].id, 'stc_platform') pose = poses[0] else: # if about same size as stc, lable it stc yield self.pcodar_label(msgs[0].id, 'stc_platform') pose = poses[0] defer.returnValue(pose)
def check_color(totem, color_map): for name, color in color_map: if mil_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g', 'b']) == color: return name return DEFAULT_COLOR
def run(self, args): # Get position of 3 gates based on position of totems gates = yield self.get_gates() # Get heading towards pinger from Andy hydrophone system self.send_feedback( 'All gates clicked on! Waiting for pinger heading...') heading = yield self.pinger_heading.get_next_message() self.send_feedback('Recieved pinger heading') # Convert heading and hydophones from to enu hydrophones_to_enu = yield self.tf_listener.get_transform( 'enu', heading.header.frame_id) hydrophones_origin = hydrophones_to_enu._p[0:2] heading = rosmsg_to_numpy(heading.vector) heading_enu = hydrophones_to_enu.transform_vector(heading) heading_enu = heading_enu[0:2] / np.linalg.norm(heading_enu[0:2]) pinger_line = self.line(hydrophones_origin, hydrophones_origin + heading_enu) gates_line = self.line(gates[0], gates[-1]) # Find intersection of these two lines. This is the approximate position of the pinger intersection = self.intersection(pinger_line, gates_line) if intersection is None: raise Exception('No intersection') self.send_feedback('Pinger is roughly at {}'.format(intersection)) distances = [] for gate in gates: distances.append(np.linalg.norm(gate[0:2] - intersection)) argmin = np.argmin(np.array(distances)) self.send_feedback('Pinger is likely at gate {}'.format(argmin + 1)) gate = gates[argmin][:2] between_vector = (gates[0] - gates[-1])[:2] # Rotate that vector to point through the buoys c = np.cos(np.radians(90)) s = np.sin(np.radians(90)) R = np.array([[c, -s], [s, c]]) direction_vector = R.dot(between_vector) direction_vector /= np.linalg.norm(direction_vector) position = self.pose[0][:2] if np.linalg.norm(position - (gate + direction_vector)) > np.linalg.norm( position - (gate - direction_vector)): direction_vector = -direction_vector before_distance = 3.0 after_distance = 5.0 before = np.append(gate + direction_vector * before_distance, 0) after = np.append(gate - direction_vector * after_distance, 0) self.send_feedback('Moving in front of gate') yield self.move.set_position(before).look_at(after).go() self.send_feedback('Going through') yield self.move.set_position(after).go() defer.returnValue('My god it actually worked!')
def from_urdf(cls, urdf_string, transmission_suffix='_thruster_transmission'): ''' Load from an URDF string. Expects each thruster to be connected a transmission ending in the specified suffix. A transform between the propeller joint and base_link must be available ''' urdf = URDF.from_xml_string(urdf_string) buff = tf2_ros.Buffer() listener = tf2_ros.TransformListener(buff) # noqa names = [] joints = [] positions = [] angles = [] limit = -1 ratio = -1 for transmission in urdf.transmissions: find = transmission.name.find(transmission_suffix) if find != -1 and find + len(transmission_suffix) == len(transmission.name): if len(transmission.joints) != 1: raise Exception('Transmission {} does not have 1 joint'.format(transmission.name)) if len(transmission.actuators) != 1: raise Exception('Transmission {} does not have 1 actuator'.format(transmission.name)) t_ratio = transmission.actuators[0].mechanicalReduction if ratio != -1 and ratio != t_ratio: raise Exception('Transmission {} has a different reduction ratio (not supported)'.format(t_ratio)) ratio = t_ratio joint = None for t_joint in urdf.joints: if t_joint.name == transmission.joints[0].name: joint = t_joint if joint is None: rospy.logerr('Transmission joint {} not found'.format(transmission.joints[0].name)) try: trans = buff.lookup_transform('base_link', joint.child, rospy.Time(), rospy.Duration(10)) except tf2_ros.TransformException as e: raise Exception(e) translation = rosmsg_to_numpy(trans.transform.translation) rot = rosmsg_to_numpy(trans.transform.rotation) yaw = euler_from_quaternion(rot)[2] names.append(transmission.name[0:find]) positions.append(translation[0:2]) angles.append(yaw) joints.append(joint.name) if limit != -1 and joint.limit.effort != limit: raise Exception('Thruster {} had a different limit, cannot proceed'.format(joint.name)) limit = joint.limit.effort return cls(names, positions, angles, ratio, limit, joints=joints)
def get_colored_buoy(navigator, color): """ Returns the closest colored buoy with the specified color """ buoy_field = yield navigator.database_query("BuoyField") buoy_field_point = mil_tools.rosmsg_to_numpy(buoy_field.objects[0].position) _dist_from_bf = lambda pt: np.linalg.norm(buoy_field_point - pt) totems = yield navigator.database_query("totem") correct_colored = [totem for totem in totems.objects if np.all(np.round(mil_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g', 'b'])) == color)] if len(correct_colored) == 0: closest = None else: closest = sorted(correct_colored, key=lambda totem: _dist_from_bf(mil_tools.rosmsg_to_numpy(totem.position)))[0] defer.returnValue(closest)
def myfunc(navigator, looking_for, center_marker): # Updated high_prob_objs = ["shooter", "dock"] pos = yield navigator.tx_pose pos = pos[0] if center_marker is None or center_marker == "None": defer.returnValue(True) try: center_marker = yield navigator.database_query( object_name=center_marker.name) center_marker = center_marker.objects[0] print center_marker.name except: fprint("A marker has not been set", msg_color="red") defer.returnValue(False) mark_pos = nt.rosmsg_to_numpy(center_marker.position) dist = np.linalg.norm(pos - mark_pos) if dist > 10: yield navigator.move.look_at(mark_pos).set_position(mark_pos).go() defer.returnValue(True) if looking_for is None or looking_for == "None": defer.returnValue(True) pos = yield navigator.tx_pose pos = pos[0] if looking_for in high_prob_objs: try: obj = yield navigator.database_query(object_name=looking_for) fprint("EXPLORER FOUND OBJECT", msg_color="blue") yield navigator.database_query( cmd="lock {} {}".format(obj.id, looking_for)) defer.returnValue(True) except MissingPerceptionObject: fprint("The object {} is not in the database".format(looking_for), msg_color="red") try: objs = yield navigator.database_query(object_name="all") objs = get_closest_objects(pos, objs.objects) except Exception as e: print e defer.returnValue(False) # for o in objs: # obj_pos = nt.rosmsg_to_numpy(o.position) # yield navigator.move.look_at(obj_pos).set_position(mark_pos).backward(7).go() # cam_obj = yield navigator.camera_database_query(object_name=looking_for, id=o.id) # if cam_obj.found: # yield navigator.database_query(cmd="lock {} {}".format(o.id, looking_for)) # fprint("EXPLORER FOUND OBJECT", msg_color="blue") # defer.returnValue(True) fprint("NO OBJECT FOUND", msg_color="blue") defer.returnValue(False)
def find_closest_object_given_ray(self, objects, ray, ray_base, tol = 3): # Compare a given bounding box with objects found by LIDAR. Find the closest object if any. for o in objects: distance = np.linalg.norm(np.cross(ray, mil_tools.rosmsg_to_numpy(o.pose.position) - ray_base)) if distance > tol: continue else: return o return None
def main(navigator, **kwargs): #middle_point = np.array([-10, -70, 0]) est_coral_survey = yield navigator.database_query("CoralSurvey") # Going to get all the objects and using the closest one as the totem totem = yield navigator.database_query("all") # Get the closest totem object to the boat totems_np = map(lambda obj: mil_tools.rosmsg_to_numpy(obj.position), totem.objects) dist = map( lambda totem_np: np.linalg.norm(totem_np - mil_tools.rosmsg_to_numpy( est_coral_survey.objects[0].position)), totems_np) middle_point = totems_np[np.argmin(dist)] print "Totem sorted:", totems_np print "Totem selected: ", totems_np[0] quads_to_search = [1, 2, 3, 4] quad = yield navigator.mission_params[ "acoustic_pinger_active_index_correct"].get() waypoint_from_center = np.array([10 * np.sqrt(2)]) # Publish ogrid with boundaries to stay inside ogrid = OgridFactory(middle_point, draw_borders=True) msg = ogrid.get_message() latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid, msg) # Construct waypoint list along NSEW directions then rotate 45 degrees to get a good spot to go to. directions = [EAST, NORTH, WEST, SOUTH] waypoints = [] #for quad in quads_to_search: mid = navigator.move.set_position(middle_point).set_orientation( directions[quad - 1]) search_center = mid.yaw_left( 45, "deg").forward(waypoint_from_center).set_orientation(NORTH) yield search_center.left(6).go() yield navigator.move.circle_point(search_center.position, direction='cw').go() yield navigator.mission_params["coral_survey_shape1"].set(shapes[0]) latched.cancel() defer.returnValue(None)
def _dist(self, x): if self.position is None: success = yield threads.deferToThread(self._wait_for_position) if not success: raise Exception("There is a problem with odom.") if self.navigator is not None: position = yield self.navigator.tx_pose position = position[0] self.position = position defer.returnValue(np.linalg.norm(nt.rosmsg_to_numpy(x.position) - self.position))
def get_pinger_pose(self): """Query pinger perception for the location of the pinger after observing""" res = yield self.pinger_client(FindPingerRequest()) if res.pinger_position.x == 0: self.pinger_pose = self.gate_poses[1] pinger_pose = res.pinger_position self.pinger_pose = mil_tools.rosmsg_to_numpy(pinger_pose)[:2] self.distances = np.array([np.linalg.norm(self.pinger_pose - self.gate_poses[0]), np.linalg.norm(self.pinger_pose - self.gate_poses[1]), np.linalg.norm(self.pinger_pose - self.gate_poses[2])])
def get_closest_objects(position, objects, max_len=3, max_dist=30): num = len(objects) idx = max_len if num < max_len: idx = num objects = sorted( objects, key=lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position))) objects = objects[:idx] dists = map( lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position)), objects) final_objs = [] for i, d in enumerate(dists): if d < max_dist: final_objs.append(objects[i]) else: break return final_objs
def myfunc(navigator, looking_for, center_marker): # Updated high_prob_objs = ["shooter", "dock"] pos = yield navigator.tx_pose pos = pos[0] if center_marker is None or center_marker == "None": defer.returnValue(True) try: center_marker = yield navigator.database_query(object_name=center_marker.name) center_marker = center_marker.objects[0] print center_marker.name except: fprint("A marker has not been set", msg_color="red") defer.returnValue(False) mark_pos = nt.rosmsg_to_numpy(center_marker.position) dist = np.linalg.norm(pos - mark_pos) if dist > 10: yield navigator.move.look_at(mark_pos).set_position(mark_pos).go() defer.returnValue(True) if looking_for is None or looking_for == "None": defer.returnValue(True) pos = yield navigator.tx_pose pos = pos[0] if looking_for in high_prob_objs: try: obj = yield navigator.database_query(object_name=looking_for) fprint("EXPLORER FOUND OBJECT", msg_color="blue") yield navigator.database_query(cmd="lock {} {}".format(obj.id, looking_for)) defer.returnValue(True) except MissingPerceptionObject: fprint("The object {} is not in the database".format(looking_for), msg_color="red") try: objs = yield navigator.database_query(object_name="all") objs = get_closest_objects(pos, objs.objects) except Exception as e: print e defer.returnValue(False) # for o in objs: # obj_pos = nt.rosmsg_to_numpy(o.position) # yield navigator.move.look_at(obj_pos).set_position(mark_pos).backward(7).go() # cam_obj = yield navigator.camera_database_query(object_name=looking_for, id=o.id) # if cam_obj.found: # yield navigator.database_query(cmd="lock {} {}".format(o.id, looking_for)) # fprint("EXPLORER FOUND OBJECT", msg_color="blue") # defer.returnValue(True) fprint("NO OBJECT FOUND", msg_color="blue") defer.returnValue(False)
def _dist(self, x): if self.position is None: success = yield threads.deferToThread(self._wait_for_position) if not success: raise Exception("There is a problem with odom.") if self.navigator is not None: position = yield self.navigator.tx_pose position = position[0] self.position = position defer.returnValue( np.linalg.norm(nt.rosmsg_to_numpy(x.position) - self.position))
def find_dock(self, override_scale): # Get Dock self.dock = yield self.get_sorted_objects(name='dock', n=1) self.dock = self.dock[0][0] # Get dock parameters self.dock_position = rosmsg_to_numpy(self.dock.pose.position) self.dock_orientation = rosmsg_to_numpy(self.dock.pose.orientation) self.dock_scale = rosmsg_to_numpy(self.dock.scale) self.dock_orientation = tform.euler_from_quaternion( self.dock_orientation) # If scale should be overwritten if override_scale: # Write long/short appropriately if self.dock_scale[0] > self.dock_scale[1]: self.dock_scale[0] = self.DOCK_SIZE_LONG self.dock_scale[1] = self.DOCK_SIZE_SHORT else: self.dock_scale[0] = self.DOCK_SIZE_SHORT self.dock_scale[1] = self.DOCK_SIZE_LONG
def _make_bounds(cls): fprint("Constructing bounds.", title="NAVIGATOR") if (yield cls.nh.has_param("/bounds/enforce")): _bounds = cls.nh.get_service_client('/get_bounds', navigator_srvs.Bounds) yield _bounds.wait_for_service() resp = yield _bounds(navigator_srvs.BoundsRequest()) if resp.enforce: cls.enu_bounds = [mil_tools.rosmsg_to_numpy(bound) for bound in resp.bounds] else: fprint("No bounds param found, defaulting to none.", title="NAVIGATOR") cls.enu_bounds = None
def find_stc2(self): pose = None print("entering find_stc") # see if we already got scan the code tower try: _, poses = yield self.get_sorted_objects(name='stc_platform', n=1) pose = poses[0] # incase stc platform not already identified except Exception as e: print("could not find stc_platform") # get all pcodar objects try: print("check for any objects") msgs, poses = yield self.get_sorted_objects(name='UNKNOWN', n=-1) # if no pcodar objects, drive forward except Exception as e: print("literally no objects?") yield self.move.forward(25).go() # get first pcodar objects msgs, poses = yield self.get_sorted_objects(name='UNKNOWN', n=-1) # if still no pcodar objects, guess RGB and exit mission # go to nearest obj to get better data on that obj print('going to nearest small object') # determine the dock and stc_buoy based on cluster size dock_pose = None for i in range(len(msgs)): #Sometimes the dock is perceived as multiple objects #Ignore any objects that are the dock #I haven't found an overlap for a cluster tolerance that #keeps the entire dock together 100% of the time #while not including the stc buoy if it is too close #if dock_pose is not None and \ # np.linalg.norm(dock_pose[0] - poses[i][0]) < 10: # continue if np.linalg.norm(rosmsg_to_numpy(msgs[i].scale)) > 4.0: # much bigger than scale of stc # then we found the dock yield self.pcodar_label(msgs[i].id, 'dock') dock_pose = poses[i] else: # if about same size as stc, lable it stc yield self.pcodar_label(msgs[i].id, 'stc_platform') pose = poses[i] break print("leaving find_stc") defer.returnValue(pose)
def ping_recv(self, p_message): try: # Transform the ping into enu hydrophones_to_enu = yield self.tf_listener.get_transform('enu', p_message.header.frame_id) hydrophones_origin = hydrophones_to_enu._p[0:2] heading = rosmsg_to_numpy(p_message.vector) heading_enu = hydrophones_to_enu.transform_vector(heading) heading_enu = heading_enu[0:2] / np.linalg.norm(heading_enu[0:2]) # Track the ping self.intersect_vectors.append((hydrophones_origin[0:2], hydrophones_origin[0:2] + heading_enu[0:2])) except tf2_ros.TransformException, e: self.send_feedback('TF Exception: {}'.format(e))
def get_objects_in_radius(self, pos, radius, objects="all"): req = ObjectDBQueryRequest() req.name = 'all' resp = yield self._database(req) ans = [] if not resp.found: defer.returnValue(ans) for o in resp.objects: if objects == "all" or o.name in objects: dist = np.linalg.norm(pos - nt.rosmsg_to_numpy(o.position)) if dist < radius: ans.append(o) defer.returnValue(ans)
def convert_cb(self, req): ''' Callback for conversion service. Converts all points from the in_frame to to_frame. ''' if req.frame not in self.FRAMES: return CoordinateConversionResponse(message='in_frame not valid') if req.to_frame not in self.FRAMES: return CoordinateConversionResponse(message='to_frame not valid') idx = self.FRAMES.index(req.to_frame) converted = np.zeros((len(req.points), 3)) func = getattr(self, req.frame) for i in range(len(req.points)): converted[i] = func(rosmsg_to_numpy(req.points[i]))[idx] return CoordinateConversionResponse(converted=numpy_to_points(converted))
def get_object_map(self): ''' Gets the latest objects returns tuple (object_dict, position_dict) object_dict: maps object id to classification position_dict: maps object_id to position in enu as numpy array ''' current_objects_msg = (yield self.database_query(name='all')).objects ret = {} positions = {} for obj in current_objects_msg: classification = obj.labeled_classification ret[obj.id] = classification positions[obj.id] = rosmsg_to_numpy(obj.pose.position) defer.returnValue((ret, positions))
def circle_search(self): platform_np = mil_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position) yield self.move.look_at(platform_np).set_position(platform_np).backward(self.circle_radius)\ .yaw_left(90, unit='deg').go(move_type="drive") done_circle = False @txros.util.cancellableInlineCallbacks def do_circle(): yield self.move.circle_point(platform_np, direction=self.circle_direction).go() done_circle = True # noqa flake8 cant see that it is defined above do_circle() while not done_circle: res = yield self.get_any_shape() if res is False: yield self.nh.sleep(0.25) continue fprint("Shape ({}found, using normal to look at other 3 shapes if needed".format( res[0]), title="DETECT DELIVER", msg_color="green") # circle_defer.cancel() shape_color, found_shape_pose = res if self.correct_shape(shape_color): self.shape_pose = found_shape_pose return # Pick other 3 to look at rot_right = np.array([[0, -1], [1, 0]]) (shape_point, shape_normal) = found_shape_pose rotated_norm = np.append(rot_right.dot(shape_normal[:2]), 0) center_point = shape_point - shape_normal * (self.platform_radius / 2.0) point_opposite_side = center_point - shape_normal * self.circle_radius move_opposite_side = self.move.set_position( point_opposite_side).look_at(center_point).yaw_left(90, unit='deg') left_or_whatever_point = center_point + rotated_norm * self.circle_radius move_left_or_whatever = self.move.set_position( left_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg') right_or_whatever_point = center_point - rotated_norm * self.circle_radius move_right_or_whatever = self.move.set_position( right_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg') yield self.search_sides((move_right_or_whatever, move_opposite_side, move_left_or_whatever)) return fprint("No shape found after complete circle", title="DETECT DELIVER", msg_color='red') raise Exception("No shape found on platform")
def myfunc(navigator, **kwargs): pos = yield navigator.tx_pose pos = pos[0] exploring = ["Exploring1", "Exploring2", "Exploring3", "Exploring4"] for e in exploring: try: objects = yield navigator.database_query(e) o = objects.objects[0] print o.name pos = nt.rosmsg_to_numpy(o.position) yield navigator.move.set_position(pos).go() nt.fprint(o.name, msg_color='green') except: nt.fprint("Missing Marker", msg_color="red")
def get_stc_points(self, msg, stc_pos): trans = yield self.my_tf.get_transform("/front_right_cam", "/velodyne", msg.header.stamp) trans1 = yield self.my_tf.get_transform("/front_right_cam", "/enu", msg.header.stamp) stc_pos = rosmsg_to_numpy(stc_pos) stc_pos = np.append(stc_pos, 1) position = trans1.as_matrix().dot(stc_pos) if position[3] < 1E-15: raise ZeroDivisionError position[0] /= position[3] position[1] /= position[3] position[2] /= position[3] position = position[:3] stereo_points = [] for point in pc2.read_points(msg, skip_nans=True): stereo_points.append(np.array([point[0], point[1], point[2], 1])) stereo_points = map(lambda x: trans.as_matrix().dot(x), stereo_points) points = [] for p in stereo_points: if p[3] < 1E-15: raise ZeroDivisionError p[0] /= p[3] p[1] /= p[3] p[2] /= p[3] points.append(p[:3]) points_keep = [] for p in points: # print npl.norm(p - poition) if npl.norm(p - position) < 20: points_keep.append(p) points_keep = sorted(points_keep, key=lambda x: x[1]) keep_num = int(.1 * len(points_keep)) points_keep = points_keep[:keep_num] # self.pers_points.extend(points_keep) # max_num = 200 # if len(self.pers_points) > max_num: # self.pers_points = self.pers_points[len(self.pers_points) - max_num:len(self.pers_points)] defer.returnValue(points_keep)
def get_shape_pos(self, normal_res, enu_cam_tf): enunormal = enu_cam_tf.transform_vector(mil_tools.rosmsg_to_numpy(normal_res.normal)) enupoint = enu_cam_tf.transform_point(mil_tools.rosmsg_to_numpy(normal_res.closest)) return (enupoint, enunormal)
def from_srv(cls, srv): return cls(mil_tools.rosmsg_to_numpy(srv.position), srv.color)
def get_waypoint(self): res = yield self.navigator.database_query(self.WAYPOINT_NAME) self.dock_pose = mil_tools.rosmsg_to_numpy(res.objects[0].position)
def normal_is_sane(self, vector3): return abs(mil_tools.rosmsg_to_numpy(vector3)[1]) < 0.4
def run(self, args): if not self.pose: raise Exception('Cant move: No odom') commands = args.commands arguments = commands[1::2] commands = commands[0::2] self.send_feedback('Switching wrench to autonomous') yield self.change_wrench('autonomous') for i in xrange(len(commands)): command = commands[i] argument = arguments[i] action_kwargs = {'move_type': args.movetype, 'speed_factor': args.speedfactor} action_kwargs['blind'] = args.blind if args.speedfactor is not None: if ',' in args.speedfactor: sf = np.array(map(float, args.speedfactor[1:-1].split(','))) else: sf = [float(args.speedfactor)] * 3 action_kwargs['speed_factor'] = sf if args.plantime is not None: action_kwargs['initial_plan_time'] = float(args.plantime) if args.focus is not None: focus = np.array(map(float, args.focus[1:-1].split(','))) focus[2] = 1 # Tell lqrrt we want to look at the point point = numpy_to_point(focus) action_kwargs['focus'] = point if command == 'custom': # Let the user input custom commands, the eval may be dangerous so do away with that at some point. self.send_feedback("Moving with the command: {}".format(argument)) res = yield eval("self.move.{}.go(move_type='{move_type}')".format(argument, **action_kwargs)) elif command == 'rviz': self.send_feedback('Select a 2D Nav Goal in RVIZ') target_pose = yield util.wrap_time_notice(self.rviz_goal.get_next_message(), 2, "Rviz goal") self.send_feedback('RVIZ pose recieved!') res = yield self.move.to_pose(target_pose).go(**action_kwargs) elif command == 'circle': self.send_feedback('Select a Publish Point in RVIZ') target_point = yield util.wrap_time_notice(self.rviz_point.get_next_message(), 2, "Rviz point") self.send_feedback('RVIZ point recieved!') target_point = rosmsg_to_numpy(target_point.point) distance = np.linalg.norm(target_point - self.pose[0]) target_point = rosmsg_to_numpy(target_point.point) direction = 'cw' if argument == '-1' else 'ccw' res = yield self.move.circle_point(target_point, direction=direction).go(radius=distance) else: shorthand = {"f": "forward", "b": "backward", "l": "left", "r": "right", "yl": "yaw_left", "yr": "yaw_right"} command = command if command not in shorthand.keys() else shorthand[command] movement = getattr(self.move, command) trans_move = command[:3] != "yaw" unit = "m" if trans_move else "rad" amount = argument # See if there's a non standard unit at the end of the argument if not argument[-1].isdigit(): last_digit_index = [i for i, c in enumerate(argument) if c.isdigit()][-1] + 1 amount = float(argument[:last_digit_index]) unit = argument[last_digit_index:] # Get the kwargs to pass to the action server station_hold = amount == '0' if station_hold: action_kwargs['move_type'] = 'hold' msg = "Moving {} ".format(command) if trans_move else "Yawing {} ".format(command[4:]) self.send_feedback(msg + "{}{}".format(amount, unit)) res = yield movement(float(amount), unit).go(**action_kwargs) if res.failure_reason is not '': raise Exception('Move failed. Reason: {}'.format(res.failure_reason)) defer.returnValue('Move completed successfully!')
def check_color(totem, color_map): for name, color in color_map: if mil_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g', 'b']) == color: return name return DEFAULT_COLOR
def main(navigator, **kwargs): # rgb color map to param vlues color_map = {'BLUE': [0, 0, 1], 'RED': [1, 0, 0], 'YELLOW': [1, 1, 0], 'GREEN': [0, 1, 0]} directions = {'RED': 'cw', 'GREEN': 'ccw', 'BLUE': 'cw', 'YELLOW': 'ccw'} ogrid = OccupancyGridFactory(navigator) explored_ids = [] all_found = False # Get colors of intrest and directions c1 = navigator.mission_params['scan_the_code_color1'].get() c2 = navigator.mission_params['scan_the_code_color2'].get() c3 = navigator.mission_params['scan_the_code_color3'].get() colors = [c1, c2, c3] buoy_field = yield navigator.database_query("BuoyField") buoy_field_point = mil_tools.rosmsg_to_numpy(buoy_field.objects[0].position) _dist_from_bf = lambda pt: np.linalg.norm(buoy_field_point - pt) # We want to go to an observation point based on solar position center = navigator.move.set_position(buoy_field_point).set_orientation(get_solar_q()) searched = [] for color in colors: target = None need_recolor = False color = yield color direction = directions[color] fprint("Going to totem colored {} in direction {}".format(color, direction), title="CIRCLE_TOTEM") target = yield get_colored_buoy(navigator, color_map[color]) if target is None or _dist_from_bf(mil_tools.rosmsg_to_numpy(target.position)) > (BF_WIDTH / 2 + BF_EST_COFIDENCE): # Need to do something fprint("No suitable totems found, going to circle any nearby totems.", msg_color='red', title="CIRCLE_TOTEM") target, searched = yield get_closest_totem(navigator, searched) need_recolor = True target_np = mil_tools.rosmsg_to_numpy(target.position) circler = navigator.move.d_circle_point(point=target_np, radius=TOTEM_SAFE_DIST, direction=direction) # Give the totem a look at for p in circler: res = yield p.go(speed_factor=SPEED_FACTOR) if res.failure_reason is '': break if need_recolor: fprint("Recoloring...") # Check for color? yield navigator.nh.sleep(5) target = yield navigator.database_query(str(target.id), raise_exception=False) if len(target.objects) == 0: direction = directions[DEFAULT_COLOR] else: direction = directions[check_color(target.objects[0], color_map)] mult = 1 if direction == 'cw' else -1 left_offset = mult * CIRCLE_OFFSET tangent_circler = navigator.move.d_circle_point(point=target_np, radius=ROT_SAFE_DIST, theta_offset=mult * 1.57, direction=direction) # Point tangent for p in tangent_circler: res = yield p.go(speed_factor=SPEED_FACTOR) if res.failure_reason is '': break msg, goal = ogrid.circle_around(target_np, direction=direction) latched_pub = ogrid.latching_publisher(msg) yield navigator.nh.sleep(2) goal = navigator.move.set_position(np.append(goal, 1)).look_at(navigator.pose[0]).left(left_offset).backward(2) yield goal.go(move_type='drive!', initial_plan_time=10) latched_pub.cancel() fprint("Canceling ogrid") yield navigator.nh.sleep(3) print "Mission result:", res defer.returnValue(None)