def get_normal(self): req = CameraToLidarTransformRequest() req.header = self.found_shape.header req.point = Point() req.point.x = self.found_shape.CenterX req.point.y = self.found_shape.CenterY rect = self._bounding_rect(self.found_shape.points) req.tolerance = int(min(rect[0] - rect[3], rect[1] - rect[4]) / 2.0) self.normal_res = yield self.cameraLidarTransformer(req) if self.normal_res.success: transformObj = yield self.navigator.tf_listener.get_transform( '/enu', '/' + req.header.frame_id) self.enunormal = transformObj.transform_vector( navigator_tools.rosmsg_to_numpy(self.normal_res.normal)) self.enupoint = transformObj.transform_point( navigator_tools.rosmsg_to_numpy(self.normal_res.closest)) target_marker = Marker() target_marker.scale.x = 0.1 target_marker.scale.y = 0.5 target_marker.scale.z = 0.5 target_marker.header.frame_id = "enu" target_marker.action = Marker.ADD target_marker.header.stamp = self.navigator.nh.get_time() target_marker.points.append( navigator_tools.numpy_to_point(self.enupoint)) target_marker.points.append( navigator_tools.numpy_to_point(self.enupoint + self.enunormal)) target_marker.type = Marker.ARROW #target_marker.text = "Shooter Target Pose" target_marker.ns = "detect_deliver" target_marker.id = 1 target_marker.color.r = 1 target_marker.color.a = 1 self.markers.markers.append(target_marker) defer.returnValue(self.normal_res.success)
def get_dock_search_markers(self): left_res = yield self.navigator.database_query("DockEdgeLeft") left_pose = navigator_tools.rosmsg_to_numpy(left_res.objects[0].position) right_res = yield self.navigator.database_query("DockEdgeRight") right_pose = navigator_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 = (navigator_tools.rosmsg_to_numpy(res.bays[0]), navigator_tools.rosmsg_to_numpy(res.bays[1]), navigator_tools.rosmsg_to_numpy(res.bays[2])) self.bay_normal = navigator_tools.rosmsg_to_numpy(res.normal) print_good("Got GetDockShapes response") return raise Exception("Bays not found after circling or timed out")
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 = (navigator_tools.rosmsg_to_numpy(res.bays[0]), navigator_tools.rosmsg_to_numpy(res.bays[1]), navigator_tools.rosmsg_to_numpy(res.bays[2])) self.bay_normal = navigator_tools.rosmsg_to_numpy(res.normal) print_good("Got GetDockShapes response") return raise Exception("Bays not found after circling or timed out")
def get_normal(self): req = CameraToLidarTransformRequest() req.header = self.found_shape.header req.point = Point() req.point.x = self.found_shape.CenterX req.point.y = self.found_shape.CenterY req.tolerance = self.normal_approx_tolerance_proportion*self.found_shape.img_width self.normal_res = yield self.cameraLidarTransformer(req) if self.normal_res.success: transformObj = yield self.navigator.tf_listener.get_transform('/enu', '/'+req.header.frame_id) self.enunormal = transformObj.transform_vector(navigator_tools.rosmsg_to_numpy(self.normal_res.normal)) self.enupoint = transformObj.transform_point(navigator_tools.rosmsg_to_numpy(self.normal_res.closest)) target_marker = Marker() target_marker.scale.x = 0.1; target_marker.scale.y = 0.5; target_marker.scale.z = 0.5; target_marker.header.frame_id = "enu" target_marker.action = Marker.ADD; target_marker.header.stamp = self.navigator.nh.get_time() target_marker.points.append(navigator_tools.numpy_to_point(self.enupoint)) target_marker.points.append(navigator_tools.numpy_to_point(self.enupoint+self.enunormal)) target_marker.type = Marker.ARROW #target_marker.text = "Shooter Target Pose" target_marker.ns = "detect_deliver" target_marker.id = 1 target_marker.color.r = 1 target_marker.color.a = 1 self.markers.markers.append(target_marker) defer.returnValue(self.normal_res.success)
def circle_search(self): print "Starting circle search" yield self.navigator.move.look_at(navigator_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position)).go() pattern = self.navigator.move.circle_point(navigator_tools.rosmsg_to_numpy( self.waypoint_res.objects[0].position), radius=self.circle_radius, theta_offset=self.theta_offset) yield next(pattern).go() searcher = self.navigator.search( vision_proxy='get_shape', search_pattern=pattern, Shape=self.Shape, Color=self.Color) yield searcher.start_search(timeout=self.search_timeout_seconds, spotings_req=self.spotings_req, move_type="skid") print "Ended Circle Search"
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 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 = navigator_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 = navigator_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 = navigator_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 check_color(totem, color_map): for name, color in color_map: if navigator_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g', 'b']) == color: return name return DEFAULT_COLOR
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 = navigator_tools.point_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( navigator_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( navigator_tools.point_to_numpy(totem.position)))[0] defer.returnValue(closest)
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 = navigator_tools.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(navigator_tools.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 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 get_pinger_pose(self): """Query pinger perception for the location of the pinger after observing""" res = yield self.pinger_client(FindPingerRequest()) pinger_pose = res.pinger_position self.pinger_pose = navigator_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_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 = navigator_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 = navigator_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 = navigator_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_pinger_pose(self): """Query pinger perception for the location of the pinger after observing""" res = yield self.pinger_client(FindPingerRequest()) pinger_pose = res.pinger_position self.pinger_pose = navigator_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 _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 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 get_normal(self): req = CameraToLidarTransformRequest() req.header = self.found_shape.header req.point = Point() req.point.x = self.found_shape.CenterX req.point.y = self.found_shape.CenterY rect = self._bounding_rect(self.found_shape.points) req.tolerance = int(min(rect[0]-rect[3],rect[1]-rect[4])/2.0) self.normal_res = yield self.cameraLidarTransformer(req) if not self.normal_res.success: defer.returnValue(False) if not self.normal_is_sane(self.normal_res.normal): self.normal_res.success = False self.normal_res.error = "UNREASONABLE NORMAL" defer.returnValue(False) enu_cam_tf = yield self.navigator.tf_listener.get_transform('/enu', '/'+req.header.frame_id) self.shooter_baselink_tf = yield self.navigator.tf_listener.get_transform('/base_link','/shooter') self.enunormal = enu_cam_tf.transform_vector(navigator_tools.rosmsg_to_numpy(self.normal_res.normal)) self.enupoint = enu_cam_tf.transform_point(navigator_tools.rosmsg_to_numpy(self.normal_res.closest)) defer.returnValue(True)
def _do_mission(self, mission, **kwargs): """Perform a mission, and ensure that all of the post conditions are enforced.""" if "redo" not in kwargs: redo = False else: redo = kwargs["redo"] if not redo and not self.sim_mode and mission.item_dep is not None: print mission.item_dep.name marker = yield self.navigator.database_query(mission.item_dep.name) marker = marker.objects[0] print nt.rosmsg_to_numpy(marker.position) move = self.navigator.move.set_position(nt.rosmsg_to_numpy(marker.position)) print move yield move.go() mission.start_time = self.nh.get_time() mission.attempts += 1 res = yield mission.do_mission(self.navigator, self, self.module, **kwargs) defer.returnValue(res)
def get_all_object_rois(self): req = ObjectDBQueryRequest() req.name = 'all' obj = yield self._database(req) if obj is None or not obj.found: defer.returnValue((None, None)) rois = [] ros_img = yield self._get_closest_image(obj.objects[0].header.stamp) if ros_img is None: defer.returnValue((None, None)) img = self.bridge.imgmsg_to_cv2(ros_img, "mono8") for o in obj.objects: if o.id not in self.id_to_perist: self.id_to_perist[o.id] = [] ppoints = self.id_to_perist[o.id] ppoints.extend(o.points) if len(ppoints) > 500: ppoints = ppoints[:500] if self.training and o.name not in self.classes: continue position = yield self._get_position() o_pos = nt.rosmsg_to_numpy(o.position) diff = np.linalg.norm(position - o_pos) if diff > self.max_dist: continue points, bbox = yield self._get_bounding_box_2d(ppoints, obj.objects[0].header.stamp) if bbox is None: continue xmin, ymin, xmax, ymax = bbox h, w, r = img.shape if xmin < 0 or xmax < 0 or xmin > w or xmax > w or xmax - xmin == 0 or ymax - ymin == 0: continue if ymin < 0: ymin = 0 print "bbox", bbox roi = img[ymin:ymax, xmin:xmax] print "preshape", roi.shape roi = self._resize_image(roi) print "postshape", roi.shape ret_obj = {} ret_obj["id"] = o.id ret_obj["points"] = points ret_obj["img"] = roi ret_obj["time"] = o.header.stamp ret_obj["name"] = o.name ret_obj["bbox"] = [xmin, ymin, xmax, ymax] rois.append(ret_obj) defer.returnValue((img, rois))
def get_dock_search_markers(self): left_res = yield self.navigator.database_query("DockEdgeLeft") left_pose = navigator_tools.rosmsg_to_numpy( left_res.objects[0].position) right_res = yield self.navigator.database_query("DockEdgeRight") right_pose = navigator_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_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 get_colored_buoy(navigator, color): """ Returns the closest colored buoy with the specified color """ buoy_field = yield navigator.database_query("BuoyField") buoy_field_point = navigator_tools.point_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(navigator_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(navigator_tools.point_to_numpy(totem.position)))[0] defer.returnValue(closest)
def circle_search(self): print "Starting circle search" #yield self.navigator.move.look_at(navigator_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position)).go() pattern = self.navigator.move.circle_point( navigator_tools.rosmsg_to_numpy( self.waypoint_res.objects[0].position), radius=self.circle_radius, theta_offset=self.theta_offset) yield next(pattern).go() searcher = self.navigator.search(vision_proxy='get_shape', search_pattern=pattern, Shape=self.Shape, Color=self.Color) yield searcher.start_search(timeout=self.search_timeout_seconds, spotings_req=self.spotings_req, move_type="skid") print "Ended Circle Search"
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") pass
def circle_search(self): platform_np = navigator_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position) yield self.navigator.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.navigator.move.circle_point(platform_np, direction=self.circle_direction).go() done_circle = True circle_defer = do_circle() while not done_circle: res = yield self.get_any_shape() if res == False: yield self.navigator.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.navigator.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.navigator.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.navigator.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 _get_bounding_box_2d(self, points_3d_enu, time): if self.cam_info is None: defer.returnValue((None, None)) self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.cam_info) points_2d = [] try: trans = yield self.tf_listener.get_transform(self.tf_frame, "/enu", time) except Exception: defer.returnValue((None, None)) transformation = trans.as_matrix() for point in points_3d_enu: point = nt.rosmsg_to_numpy(point) point = np.append(point, 1.0) t_p = transformation.dot(point) if t_p[3] < 1E-15: defer.returnValue((None, None)) t_p[0] /= t_p[3] t_p[1] /= t_p[3] t_p[2] /= t_p[3] t_p = t_p[0:3] if t_p[2] < 0: defer.returnValue((None, None)) point_2d = self.camera_model.project3dToPixel(t_p) points_2d.append((int(point_2d[0]), int(point_2d[1]))) xmin, ymin = sys.maxint, sys.maxint xmax, ymax = -sys.maxint, -sys.maxint for i, point in enumerate(points_2d): if point[0] < xmin: xmin = point[0] if point[0] > xmax: xmax = point[0] if point[1] < ymin: ymin = point[1] if point[1] > ymax: ymax = point[1] defer.returnValue((points_2d, (xmin, ymin, xmax, ymax)))
def ping_cb(self, ping): print rospkg.get_ros_package_path() print "PINGERFINDER: freq={p.freq:.0f} amp={p.amplitude:.0f}".format( p=ping) if abs( ping.freq - self.target_freq ) < self.freq_tol and ping.amplitude > self.min_amp and self.listen: trans, rot = None, None try: self.tf_listener.waitForTransform(self.map_frame, self.hydrophone_frame, ping.header.stamp, rospy.Duration(0.25)) trans, rot = self.tf_listener.lookupTransform( self.map_frame, self.hydrophone_frame, ping.header.stamp) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: print e return p0 = np.array([trans[0], trans[1]]) R = tf.transformations.quaternion_matrix(rot)[:3, :3] delta = R.dot(navigator_tools.rosmsg_to_numpy(ping.position))[:2] p1 = p0 + self.heading_pseudorange * delta / npl.norm(delta) line_coeffs = np.array([[p0[0], p0[1], p1[0], p1[1]]]) # p0 and p1 define a line self.visualize_arrow(Point(p0[0], p0[1], 0), Point(p1[0], p1[1], 0)) self.line_array = np.append(self.line_array, line_coeffs, 0) self.observation_amplitudes = np.append( self.observation_amplitudes, ping.amplitude) if len( self.line_array ) >= self.max_observations: # delete softest samples if we have over max_observations softest_idx = np.argmin(self.observation_amplitudes) self.line_array = np.delete(self.line_array, softest_idx, axis=0) self.observation_amplitudes = np.delete( self.observation_amplitudes, softest_idx) print "PINGERFINDER: Observation collected. Total: {}".format( self.line_array.shape[0])
def get_stc_points(self, msg, stc_pos): trans = yield self.my_tf.get_transform("/stereo_right_cam", "/velodyne", msg.header.stamp) trans1 = yield self.my_tf.get_transform("/stereo_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 ping_cb(self, ping): print rospkg.get_ros_package_path() print "PINGERFINDER: freq={p.freq:.0f} amp={p.amplitude:.0f}".format(p=ping) if abs(ping.freq - self.target_freq) < self.freq_tol and ping.amplitude > self.min_amp and self.listen: trans, rot = None, None try: self.tf_listener.waitForTransform(self.map_frame, self.hydrophone_frame, ping.header.stamp, rospy.Duration(0.25)) trans, rot = self.tf_listener.lookupTransform(self.map_frame, self.hydrophone_frame, ping.header.stamp) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: print e return p0 = np.array([trans[0], trans[1]]) R = tf.transformations.quaternion_matrix(rot)[:3, :3] delta = R.dot(navigator_tools.rosmsg_to_numpy(ping.position))[:2] p1 = p0 + self.heading_pseudorange * delta / npl.norm(delta) line_coeffs = np.array([[p0[0], p0[1], p1[0], p1[1]]]) # p0 and p1 define a line self.visualize_arrow(Point(p0[0], p0[1], 0), Point(p1[0], p1[1], 0)) self.line_array = np.append(self.line_array, line_coeffs, 0) self.observation_amplitudes = np.append(self.observation_amplitudes, ping.amplitude) if len(self.line_array) >= self.max_observations: # delete softest samples if we have over max_observations softest_idx = np.argmin(self.observation_amplitudes) self.line_array = np.delete(self.line_array, softest_idx, axis=0) self.observation_amplitudes = np.delete(self.observation_amplitudes, softest_idx) print "PINGERFINDER: Observation collected. Total: {}".format(self.line_array.shape[0])
def get_shape_pos(self, normal_res, enu_cam_tf): enunormal = enu_cam_tf.transform_vector(navigator_tools.rosmsg_to_numpy(normal_res.normal)) enupoint = enu_cam_tf.transform_point(navigator_tools.rosmsg_to_numpy(normal_res.closest)) return (enupoint, enunormal)
def get_colored_buoys(self): estimated_3_endbuoy = self.gate_poses[2] + (self.g_line * 5.0) estimated_1_endbuoy = self.gate_poses[0] - (self.g_line * 5.0) if self.negate: estimated_circle_buoy = self.gate_poses[1] + ( self.g_perp * 30.0 ) #should check on correct side (negate bool) else: estimated_circle_buoy = self.gate_poses[1] - ( self.g_perp * 30.0 ) #should check on correct side (negate bool) cur_time = yield self.navigator.nh.get_time() self.new_marker(position=np.append(estimated_3_endbuoy, 0), color=(1, 1, 1), time=cur_time) self.new_marker(position=np.append(estimated_circle_buoy, 0), time=cur_time) self.new_marker(position=np.append(estimated_1_endbuoy, 0), color=(1, 1, 1), time=cur_time) totems = yield self.navigator.database_query("totem", raise_exception=False) if totems.found: sorted_1 = sorted(totems.objects, key=lambda t: np.linalg. norm(estimated_1_endbuoy - navigator_tools. rosmsg_to_numpy(t.position)[:2])) sorted_3 = sorted(totems.objects, key=lambda t: np.linalg. norm(estimated_3_endbuoy - navigator_tools. rosmsg_to_numpy(t.position)[:2])) sorted_circle = sorted( totems.objects, key=lambda t: abs( np.linalg.norm(estimated_circle_buoy - navigator_tools. rosmsg_to_numpy(t.position)[:2]))) self.new_marker(position=navigator_tools.rosmsg_to_numpy( sorted_circle[0].position), color=(1, 1, 1), time=cur_time) if np.linalg.norm( navigator_tools.rosmsg_to_numpy(sorted_circle[0].position) [:2] - estimated_circle_buoy) < self.MAX_CIRCLE_BUOY_ERROR: self.circle_totem = navigator_tools.rosmsg_to_numpy( sorted_circle[0].position) fprint("PINGER: found buoy to circle at {}".format( self.circle_totem), msg_color='green') else: fprint( "PINGER: circle buoy is too far from where it should be", msg_color='red') if sorted_3[0].color.r > 0.9: self.color_wrong = True color_3 = "RED" self.new_marker(position=navigator_tools.rosmsg_to_numpy( sorted_3[0].position), color=(1, 0, 0), time=cur_time) elif sorted_3[0].color.g > 0.9: color_3 = "GREEN" self.new_marker(position=navigator_tools.rosmsg_to_numpy( sorted_3[0].position), color=(0, 1, 0), time=cur_time) else: color_3 = "UNKNOWN" self.new_marker(position=navigator_tools.rosmsg_to_numpy( sorted_3[0].position), color=(1, 1, 1), time=cur_time) if sorted_1[0].color.r > 0.9: color_1 = "RED" self.new_marker(position=navigator_tools.rosmsg_to_numpy( sorted_1[0].position), color=(1, 0, 0), time=cur_time) elif sorted_1[0].color.g > 0.9: self.color_wrong = True color_1 = "GREEN" self.new_marker(position=navigator_tools.rosmsg_to_numpy( sorted_1[0].position), color=(0, 1, 0), time=cur_time) else: color_1 = "UNKNOWN" self.new_marker(position=navigator_tools.rosmsg_to_numpy( sorted_1[0].position), color=(1, 1, 1), time=cur_time) if int(self.gate_index) == 0: if color_1 == "RED": active_colors = "RED-WHITE" elif color_1 == "GREEN": active_colors = "WHITE-GREEN" else: active_colors = "UNKNOWN" elif int(self.gate_index) == 2: if color_3 == "RED": active_colors = "RED-WHITE" elif color_3 == "GREEN": active_colors = "WHITE-GREEN" else: active_colors = "UNKNOWN" else: active_colors = "WHITE-WHITE" if active_colors != "UNKNOWN": fprint("PINGER: setting active pinger colors to {}".format( active_colors), msg_color='green') yield self.navigator.mission_params[ "acoustic_pinger_active"].set(active_colors) else: fprint("PINGER: cannot determine gate colors".format( sorted_3[0].color), msg_color='red') # fprint("PINGER: gate 3 color {}".format(sorted_3[0].color), msg_color='blue') # fprint("PINGER: gate 1 color {}".format(sorted_1[0].color), msg_color='blue') else: fprint("PINGER: no totems found", msg_color='red') yield self.marker_pub.publish(self.markers)
def get_colored_buoys(self): estimated_3_endbuoy = self.gate_poses[2] + (self.g_line * 5.0) estimated_1_endbuoy = self.gate_poses[0] - (self.g_line * 5.0) if self.negate: estimated_circle_buoy = self.gate_poses[1] + (self.g_perp * 30.0) #should check on correct side (negate bool) else: estimated_circle_buoy = self.gate_poses[1] - (self.g_perp * 30.0) #should check on correct side (negate bool) cur_time = yield self.navigator.nh.get_time() self.new_marker(position=np.append(estimated_3_endbuoy,0), color=(1,1,1), time=cur_time) self.new_marker(position=np.append(estimated_circle_buoy,0), time=cur_time) self.new_marker(position=np.append(estimated_1_endbuoy,0), color=(1,1,1), time=cur_time) totems = yield self.navigator.database_query("totem", raise_exception=False) if totems.found: sorted_1 = sorted(totems.objects, key=lambda t: np.linalg.norm(estimated_1_endbuoy - navigator_tools.rosmsg_to_numpy(t.position)[:2])) sorted_3 = sorted(totems.objects, key=lambda t: np.linalg.norm(estimated_3_endbuoy - navigator_tools.rosmsg_to_numpy(t.position)[:2])) sorted_circle = sorted(totems.objects, key=lambda t: abs(np.linalg.norm(estimated_circle_buoy - navigator_tools.rosmsg_to_numpy(t.position)[:2]))) self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_circle[0].position), color=(1,1,1), time=cur_time) if np.linalg.norm(navigator_tools.rosmsg_to_numpy(sorted_circle[0].position)[:2] - estimated_circle_buoy) < self.MAX_CIRCLE_BUOY_ERROR: self.circle_totem = navigator_tools.rosmsg_to_numpy(sorted_circle[0].position) fprint("PINGER: found buoy to circle at {}".format(self.circle_totem), msg_color='green') else: fprint("PINGER: circle buoy is too far from where it should be", msg_color='red') if sorted_3[0].color.r > 0.9: self.color_wrong = True color_3 = "RED" self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_3[0].position), color=(1,0,0), time=cur_time) elif sorted_3[0].color.g > 0.9: color_3 = "GREEN" self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_3[0].position), color=(0,1,0), time=cur_time) else: color_3 = "UNKNOWN" self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_3[0].position), color=(1,1,1), time=cur_time) if sorted_1[0].color.r > 0.9: color_1 = "RED" self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_1[0].position), color=(1,0,0), time=cur_time) elif sorted_1[0].color.g > 0.9: self.color_wrong = True color_1 = "GREEN" self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_1[0].position), color=(0,1,0), time=cur_time) else: color_1 = "UNKNOWN" self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_1[0].position), color=(1,1,1), time=cur_time) if int(self.gate_index) == 0: if color_1 == "RED": active_colors = "RED-WHITE" elif color_1 == "GREEN": active_colors = "WHITE-GREEN" else: active_colors = "UNKNOWN" elif int(self.gate_index) == 2: if color_3 == "RED": active_colors = "RED-WHITE" elif color_3 == "GREEN": active_colors = "WHITE-GREEN" else: active_colors = "UNKNOWN" else: active_colors = "WHITE-WHITE" if active_colors != "UNKNOWN": fprint("PINGER: setting active pinger colors to {}".format(active_colors), msg_color='green') yield self.navigator.mission_params["acoustic_pinger_active"].set(active_colors) else: fprint("PINGER: cannot determine gate colors".format(sorted_3[0].color), msg_color='red') # fprint("PINGER: gate 3 color {}".format(sorted_3[0].color), msg_color='blue') # fprint("PINGER: gate 1 color {}".format(sorted_1[0].color), msg_color='blue') else: fprint("PINGER: no totems found", msg_color='red') yield self.marker_pub.publish(self.markers)
def circle_search(self): platform_np = navigator_tools.rosmsg_to_numpy( self.waypoint_res.objects[0].position) yield self.navigator.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.navigator.move.circle_point( platform_np, direction=self.circle_direction).go() done_circle = True circle_defer = do_circle() while not done_circle: res = yield self.get_any_shape() if res == False: yield self.navigator.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.navigator.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.navigator.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.navigator.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 get_waypoint(self): res = yield self.navigator.database_query(self.WAYPOINT_NAME) self.dock_pose = navigator_tools.rosmsg_to_numpy( res.objects[0].position)
def get_shape_pos(self, normal_res, enu_cam_tf): enunormal = enu_cam_tf.transform_vector( navigator_tools.rosmsg_to_numpy(normal_res.normal)) enupoint = enu_cam_tf.transform_point( navigator_tools.rosmsg_to_numpy(normal_res.closest)) return (enupoint, enunormal)
def normal_is_sane(self, vector3): return abs(navigator_tools.rosmsg_to_numpy(vector3)[1]) < 0.4
def get_waypoint(self): res = yield self.navigator.database_query(self.WAYPOINT_NAME) self.dock_pose = navigator_tools.rosmsg_to_numpy(res.objects[0].position)
import tf from tf import transformations as trans ping_sub = yield navigator.nh.subscribe("/hydrophones/processed", ProcessedPing) yield ping_sub.get_next_message() target_freq = 35000 while True: processed_ping = yield ping_sub.get_next_message() print processed_ping if isinstance(processed_ping, ProcessedPing): print "Got processed ping message:\n{}".format(processed_ping) if processed_ping.freq > 35000 and processed_ping.freq < 36000: freq_dev = abs(target_freq - processed_ping.freq) print "Trustworthy pinger heading" hydrophones_enu_p, hydrophones_enu_q = tf.lookupTransform("/hydrophones", "/enu", processed_ping.header.stamp) pinger_enu_p = navigator_tools.rosmsg_to_numpy(tf.transformPoint()) dir_ = navigator_tools.rosmsg_to_numpy(processed_ping.position) mv_mag = 2 mv_hyd_frame = dir_ / np.linalg.norm(dir_) pinger_move = navigator.move.set_position(navigator_tools.rosmsg_to_numpy(processed_ping.position)).go() print "Heading towards pinger" else: print "Untrustworthy pinger heading. Freq = {} kHZ".format(processed_ping.freq) else: print "Expected ProcessedPing, got {}".format(type(processed_ping)) # Hydrophone locate mission @txros.util.cancellableInlineCallbacks def main(navigator): kill_alarm_broadcaster, kill_alarm = single_alarm('kill', action_required=True, problem_description="Killing wamv to listen to pinger")