示例#1
0
def run(sub):
    # Go to max height to see as much as we can.
    yield sub.move.depth(SEARCH_DEPTH).zero_roll_and_pitch().go(speed=SPEED)
    yield sub._node_handle.sleep(1.0)

    # Do a little jig - change this for the pool.
    pattern = [sub.move.right(1), sub.move.forward(1), sub.move.left(1), sub.move.backward(1),
               sub.move.right(2), sub.move.forward(2), sub.move.left(2), sub.move.backward(2)]
    s = Searcher(sub, sub.channel_marker.get_pose, pattern)
    resp = None
    try:
        resp = yield s.start_search(loop=False, timeout=600)
    except:
        print "MARKER_MISSION - Timed out probably."
        defer.returnValue(None)

    if resp is None:
        print "MARKER_MISSION - Marker not found."
        defer.returnValue(None)

    yield sub.move.set_position(pose_to_numpy(resp.pose.pose)[0]).go(speed=SPEED)

    # How many times should we attempt to reposition ourselves
    iterations = 2
    # To make sure we don't go too far off.
    est_target_rotations = []
    for i in range(iterations):
        print "Iteration {}.".format(i + 1)
        response = yield sub.channel_marker.get_pose()
        if response is None:
            continue

        response = pose_to_numpy(response.pose.pose)

        # Using euler - shoot me.
        yaw = tf.transformations.euler_from_quaternion(response[1])[2]

        est_target_rotations.append(yaw)
        avg_rotation = sum(est_target_rotations) / len(est_target_rotations)

        yield sub.move.set_position(response[0]).yaw_left(yaw).zero_roll_and_pitch().go(speed=SPEED)

        yield sub._node_handle.sleep(3.0)

    print "MARKER_MISSION - Done!"
    defer.returnValue(True)
示例#2
0
def run(sub_singleton):
    global marker_found

    print "Searching"
    nh = sub_singleton._node_handle
    next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
    cam = PinholeCameraModel()

    # Go to max height to see as much as we can.
    yield sub_singleton.move.depth(0.6).zero_roll_and_pitch().go()
    yield nh.sleep(1.0)

    # This is just to get camera info
    resp = yield sub_singleton.channel_marker.get_2d()
    cam.fromCameraInfo(resp.camera_info)

    intial_pose = sub_singleton.last_pose()
    intial_height = yield sub_singleton.get_dvl_range()
    raidus = calculate_visual_radius(cam, intial_height)

    s = SearchPoseRequest()
    s.intial_position = (yield intial_pose).pose.pose
    s.search_radius = raidus
    s.reset_search = True
    yield next_pose(s)
    s.reset_search = False

    marker_found = False
    goer = go_to_marker(nh, sub_singleton, cam)

    while not marker_found:
        # Move in search pattern until we find the marker
        resp = yield next_pose(s)
        print resp

        if resp.area > .8:
            continue

        print pose_to_numpy(resp.target_pose)[0]
        yield sub_singleton.move.set_position(
            pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
        print "Searcher arrived"

    yield goer
示例#3
0
def run(sub_singleton):
    global marker_found

    print "Searching"
    nh = sub_singleton._node_handle
    next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
    cam = PinholeCameraModel()

    # Go to max height to see as much as we can.
    yield sub_singleton.move.depth(0.6).zero_roll_and_pitch().go()
    yield nh.sleep(1.0)

    # This is just to get camera info
    resp = yield sub_singleton.channel_marker.get_2d()
    cam.fromCameraInfo(resp.camera_info)

    intial_pose = sub_singleton.last_pose()
    intial_height = yield sub_singleton.get_dvl_range()
    raidus = calculate_visual_radius(cam, intial_height)

    s = SearchPoseRequest()
    s.intial_position = (yield intial_pose).pose.pose
    s.search_radius = raidus
    s.reset_search = True
    yield next_pose(s)
    s.reset_search = False

    marker_found = False
    goer = go_to_marker(nh, sub_singleton, cam)

    while not marker_found:
        # Move in search pattern until we find the marker
        resp = yield next_pose(s)
        print resp

        if resp.area > .8:
            continue

        print pose_to_numpy(resp.target_pose)[0]
        yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
        print "Searcher arrived"

    yield goer
示例#4
0
文件: buoy.py 项目: uf-mil/Sub8
def sanity_check(sub, est_pos):
    yield None
    est_pos_np = pose_to_numpy(est_pos.pose)[0]
    print (est_pos_np == np.array([5, 5, 5])).all()
    if (est_pos_np == np.array([5, 5, 5])).all():
        print "BUOY MISSION - Problem with guess."
        defer.returnValue(None)
    if est_pos_np[2] > -0.2:
        print 'BUOY MISSION - Detected buoy above the water'
        defer.returnValue(None)

    defer.returnValue(True)
示例#5
0
    def state_cb(self, msg):
        """
        Position is offset so first message is taken as zero point. (More reflective of actual sub).
        Z position is absolute and so is rotation.

        TODO: Add noise
        """
        if self.target not in msg.name:
            return

        if self.last_odom is None or self.position_offset is None:
            pose = msg.pose[msg.name.index(self.target)]
            position, orientation = sub8_utils.pose_to_numpy(pose)

            self.position_offset = position
            self.position_offset[2] = 0

        self.last_odom = msg
示例#6
0
    def state_cb(self, msg):
        '''
        Position is offset so first message is taken as zero point. (More reflective of actual sub).
        Z position is absolute and so is rotation.

        TODO: Add noise
        '''
        if self.target not in msg.name:
            return

        if (self.last_odom is None or self.position_offset is None):
            pose = msg.pose[msg.name.index(self.target)]
            position, orientation = sub8_utils.pose_to_numpy(pose)

            self.position_offset = position
            self.position_offset[2] = 0

        self.last_odom = msg
示例#7
0
    def publish_odom(self, *args):
        if self.last_odom is None or self.position_offset is None:
            return

        msg = self.last_odom
        if self.target in msg.name:
            header = sub8_utils.make_header(frame="/map")

            target_index = msg.name.index(self.target)
            twist = msg.twist[target_index]

            # Add position offset to make the start position (0, 0, -depth)
            position_np, orientation_np = sub8_utils.pose_to_numpy(msg.pose[target_index])
            pose = sub8_utils.numpy_quat_pair_to_pose(position_np - self.position_offset, orientation_np)

            self.state_pub.publish(
                header=header,
                child_frame_id="/base_link",
                pose=PoseWithCovariance(pose=pose),
                twist=TwistWithCovariance(twist=twist),
            )

            header = sub8_utils.make_header(frame="/world")
            twist = msg.twist[target_index]
            pose = msg.pose[target_index]
            self.world_state_pub.publish(
                header=header,
                child_frame_id="/base_link",
                pose=PoseWithCovariance(pose=pose),
                twist=TwistWithCovariance(twist=twist),
            )

            dist = np.linalg.norm(sub8_utils.twist_to_numpy(twist)) * self.odom_freq
            self.pedometry += dist

        else:
            # fail
            return
示例#8
0
    def publish_odom(self, *args):
        if self.last_odom is None or self.position_offset is None:
            return

        msg = self.last_odom
        if self.target in msg.name:
            header = sub8_utils.make_header(frame='/map')

            target_index = msg.name.index(self.target)
            twist = msg.twist[target_index]

            # Add position offset to make the start position (0, 0, -depth)
            position_np, orientation_np = sub8_utils.pose_to_numpy(
                msg.pose[target_index])
            pose = sub8_utils.numpy_quat_pair_to_pose(
                position_np - self.position_offset, orientation_np)

            self.state_pub.publish(header=header,
                                   child_frame_id='/base_link',
                                   pose=PoseWithCovariance(pose=pose),
                                   twist=TwistWithCovariance(twist=twist))

            header = sub8_utils.make_header(frame='/world')
            twist = msg.twist[target_index]
            pose = msg.pose[target_index]
            self.world_state_pub.publish(
                header=header,
                child_frame_id='/base_link',
                pose=PoseWithCovariance(pose=pose),
                twist=TwistWithCovariance(twist=twist))

            dist = np.linalg.norm(
                sub8_utils.twist_to_numpy(twist)) * self.odom_freq
            self.pedometry += dist

        else:
            # fail
            return