예제 #1
0
파일: controller.py 프로젝트: ErolB/Sub8
    def update(self, dt, desired, current):
        (p, o), (p_dot, o_dot) = current
        (desired_p, desired_o), (desired_p_dot, desired_o_dot), (desired_p_dotdot, desired_o_dotdot) = desired
        world_from_body = transformations.quaternion_matrix(o)[:3, :3]
        x_dot = numpy.concatenate([
            world_from_body.dot(p_dot),
            world_from_body.dot(o_dot),
        ])

        # world_from_desiredbody = transformations.quaternion_matrix(desired_o)[:3, :3]
        desired_x_dot = numpy.concatenate([
            world_from_body.dot(desired_p_dot),
            world_from_body.dot(desired_o_dot),
        ])
        desired_x_dotdot = numpy.concatenate([
            world_from_body.dot(desired_p_dotdot),
            world_from_body.dot(desired_o_dotdot),
        ])

        error_position_world = numpy.concatenate([
            desired_p - p,
            quat_to_rotvec(transformations.quaternion_multiply(
                desired_o,
                transformations.quaternion_inverse(o),
            )),
        ])
        if self.config['two_d_mode']:
            error_position_world = error_position_world * [1, 1, 0, 0, 0, 1]

        world_from_body2 = numpy.zeros((6, 6))
        world_from_body2[:3, :3] = world_from_body2[3:, 3:] = world_from_body

        # Permitting lambda assignment b/c legacy
        body_gain = lambda x: world_from_body2.dot(x).dot(world_from_body2.T)  # noqa

        error_velocity_world = (desired_x_dot + body_gain(numpy.diag(self.config['k'])).dot(error_position_world)) - x_dot
        if self.config['two_d_mode']:
            error_velocity_world = error_velocity_world * [1, 1, 0, 0, 0, 1]

        pd_output = body_gain(numpy.diag(self.config['ks'])).dot(error_velocity_world)

        output = pd_output
        if self.config['use_rise']:
            rise_term_int = body_gain(numpy.diag(self.config['ks'] * self.config['alpha'])).dot(error_velocity_world) + \
                body_gain(numpy.diag(self.config['beta'])).dot(numpy.sign(error_velocity_world))

            self._rise_term = self._rise_term + dt / 2 * (rise_term_int + self._rise_term_int_prev)
            self._rise_term_int_prev = rise_term_int

            output = output + self._rise_term
        else:
            # zero rise term so it doesn't wind up over time
            self._rise_term = numpy.zeros(6)
            self._rise_term_int_prev = numpy.zeros(6)
        output = output + body_gain(numpy.diag(self.config['accel_feedforward'])).dot(desired_x_dotdot)
        output = output + body_gain(numpy.diag(self.config['vel_feedforward'])).dot(desired_x_dot)

        # Permitting lambda assignment b/c legacy
        wrench_from_vec = lambda output: (world_from_body.T.dot(output[0:3]), world_from_body.T.dot(output[3:6]))  # noqa
        return wrench_from_vec(pd_output), wrench_from_vec(output)
예제 #2
0
    def _odom_update(self):
        pos = self.last_odom_msg.pose.pose.position
        yaw = quat_to_rotvec(
            xyzw_array(self.last_odom_msg.pose.pose.orientation))[2]
        vel = self.last_odom_msg.twist.twist.linear
        dYaw = self.last_odom_msg.twist.twist.angular.z

        self._odom_x_label.setText('X: %3.3f' % pos.x)
        self._odom_y_label.setText('Y: %3.3f' % pos.y)
        self._odom_yaw_label.setText('Yaw: %3.3f' % yaw)
        self._odom_d_x_label.setText('dX: %3.3f' % vel.x)
        self._odom_d_y_label.setText('dY: %3.3f' % vel.y)
        self._odom_d_yaw_label.setText('dYaw: %3.3f' % dYaw)
예제 #3
0
def main(nh, entrance='x', exit='1'):
    boat = yield boat_scripting.get_boat(nh)

    try:
        boat.switch_path_planner('a_star_rpp')
        while True:
            gates = yield boat.get_gates()
            if len(gates) == 0:
                print 'No gates detected'
                continue

            current_boat_pos = boat.odom.position
            yaw = quat_to_rotvec(boat.odom.orientation)[2] % np.pi

            # Zip up numpy positions in enu - boat_pos
            gate_pos = [
                xyz_array(g.position) - current_boat_pos for g in gates
            ]
            gates = zip(gate_pos, gates)

            # Filter out yaw

            gate = None
            if len(gates) > 0:
                gate = min(gates, key=lambda x: np.linalg.norm(x[0]))
            else:
                gate = gates[0]

            #print gate

            # Translate back to enu
            gate_pos = gate[0] + current_boat_pos
            gate_orientation = gate[1].yaw + np.pi / 2

            if abs(gate_orientation - yaw) < np.pi / 2:
                yield move_on_line.main(nh, gate_pos,
                                        np.array([0, 0, gate_orientation]))
            else:
                yield move_on_line.main(nh,
                                        gate_pos,
                                        np.array([0, 0, gate_orientation]),
                                        flip=True)

    finally:
        boat.default_state()
예제 #4
0
def main(nh, entrance='x', exit='1'):
    boat = yield boat_scripting.get_boat(nh)

    try:
        boat.switch_path_planner('a_star_rpp')
        while True:
            gates = yield boat.get_gates()
            if len(gates) == 0:
                print 'No gates detected'
                continue

            current_boat_pos = boat.odom.position
            yaw = quat_to_rotvec(boat.odom.orientation)[2] % np.pi

            # Zip up numpy positions in enu - boat_pos
            gate_pos = [xyz_array(g.position) - current_boat_pos for g in gates]
            gates = zip(gate_pos, gates)

            # Filter out yaw 


            gate = None
            if len(gates) > 0:
                gate = min(gates, key = lambda x: np.linalg.norm(x[0]))
            else:
                gate = gates[0]

            #print gate

            # Translate back to enu
            gate_pos = gate[0] + current_boat_pos
            gate_orientation = gate[1].yaw + np.pi/2


            if abs(gate_orientation - yaw) < np.pi / 2:
                yield move_on_line.main(nh, gate_pos, np.array([0, 0, gate_orientation]))
            else:
                yield move_on_line.main(nh, gate_pos, np.array([0, 0, gate_orientation]), flip=True)


    finally:
        boat.default_state()
예제 #5
0
def orient(boat):

    angle_to_move = 0
    final_cloud = []
    x_mean = 0
    y_mean = 0
    numerator = 0
    denom = 0
    pointcloud_base = []

    # loops until lidar gets a good filtered lidar reading
    while len(final_cloud) <= 0:
        pointcloud = yield boat.get_pointcloud()
        yield util.sleep(0.1)  # sleep to avoid tooPast errors
        pointcloud_base = yield boat.to_baselink(pointcloud)
        yield util.sleep(0.1)  # sleep to avoid tooPast errors

        pointcloud_enu = []

        # Append pointcloud data to enu
        for p in pc2.read_points(pointcloud, field_names=("x", "y", "z"), skip_nans=False, uvs=[]):
            pointcloud_enu.append((p[0], p[1], p[2]))
        yield util.sleep(0.1)  # sleep to avoid tooPast errors

        final_cloud = []

        # Filter lidar data to only data right in front of the boat
        # Since both pointcloud have the sane index for the same points,
        # we then use the baselink pointcloud data to fiter enu points in front of the boat

        print "Filtering lidar data"
        for i in range(0, len(pointcloud_base)):
            temp = pointcloud_base[i]
            if abs(temp[1]) < 0.3:
                final_cloud.append(pointcloud_enu[i])

    # Use final point cloud to get x and y mean to be used in line generation
    for i in final_cloud:
        x_mean += i[0]
        y_mean += i[1]

    # Find x-mean and y-mean
    y_mean = y_mean / len(final_cloud)
    x_mean = x_mean / len(final_cloud)

    for i in final_cloud:
        numerator += (i[0] - x_mean) * (i[1] - y_mean)
        denom += (i[0] - x_mean) * (i[0] - x_mean)

    if denom == 0:
        denom = 1
    m = numerator / denom
    b = y_mean - (m * x_mean)

    x1 = 1
    x2 = 100

    # Vector we want to be parallel to
    parallel_vector = m * 1 + b

    # Gets heading of the boat as unit vector
    position = boat.odom.position[0:2]
    yaw = quat_to_rotvec(boat.odom.orientation)[2]
    heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])

    # Vector to be parrallel to
    vec1 = numpy.array([x1, parallel_vector, 0])
    # Vector of the boat
    vec2 = numpy.array([(heading[0]), (heading[1]), 0])

    angle_to_move = 2
    move = 0
    angle_to_move = 1

    # Finds angle between boat unit vector and dock
    numerator = numpy.dot(vec1, vec2)
    one = numpy.linalg.norm(vec1)
    two = numpy.linalg.norm(vec2)
    cosine = numerator / (one * two)
    # Angle betwen the boat and the shore
    angle_to_move = math.acos(cosine)

    # Scan lidar to get the distances from the shore to the boat for use in creating triangle
    distances = yield boat.get_distance_from_object(0.05)

    # Caluculate distance to move to be directly in front of point
    theda_one = 0.9 - angle_to_move
    L1 = distances[0]
    distance_to_move = math.sin(theda_one) * L1

    if vec2[0] < 0:
        # print "Yawing left ", angle_to_move
        print "Moving right ", distance_to_move
        # boat.move.turn_left(angle_to_move).go()
        left = boat.move.left(-distance_to_move - 2).go()
        forward = boat.move.forward(distance_to_move - 3).go()
        yield left, forward

    if vec2[0] > 0:
        # print "Yawing right ", angle_to_move
        print "Moving left ", distance_to_move
        # boat.move.turn_left(angle_to_move).go()
        left = boat.move.left(distance_to_move - 2).go()
        forward = boat.move.forward(distance_to_move - 3).go()
        yield left, forward
예제 #6
0
def main(nh):
    boat = yield boat_scripting.get_boat(nh)

    boat_x = boat.odom.position[0]
    boat_y = boat.odom.position[1]

    closest = None
    summation = 100

    closest_buoy = 0
    buoys = yield boat.get_bouys()

    while closest is None:
        while len(buoys.buoys) <= 0:
            buoys = yield boat.get_bouys()

        for i in buoys.buoys:

            x_dif = boat_x - i.position.x
            y_dif = boat_y - i.position.y

            temp_sum = math.sqrt(x_dif ** 2 + y_dif ** 2)
            if temp_sum > 10:
                # Don't go more than 10 meters
                continue
            #print temp_sum

            if temp_sum < summation:
                summation = temp_sum
                closest = i


    x_move = boat_x - closest.position.x
    y_move = boat_y - closest.position.y


    point = numpy.array([closest.position.x, closest.position.y, 0])

    position = boat.odom.position[0:2]
    yaw = quat_to_rotvec(boat.odom.orientation)[2]
    heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])

    vec2 = numpy.array([heading[0], heading[1]])
    vec1 = numpy.array([closest.position.x, closest.position.y])


    numerator = numpy.dot(vec1, vec2)
    one = numpy.linalg.norm(vec1)
    two =  numpy.linalg.norm(vec2)
    cosine = numerator / (one * two)
    # Angle betwen the boat and the shore
    angle_to_move = math.atan(cosine)
    print angle_to_move

    boat.move.yaw_left(-angle_to_move).go()
    
    yield boat.move.set_position(point).go()
    yield boat.move.turn_left_deg(-20).go()


    for i in xrange(4):
        yield boat.move.forward(3).go()
        yield boat.move.turn_left_deg(90).go()
    def update(self, dt, desired, current):
        ((desired_p, desired_o), (desired_p_dot, desired_o_dot),
         (desired_p_dotdot,
          desired_o_dotdot)), ((p, o), (p_dot, o_dot)) = desired, current

        world_from_body = transformations.quaternion_matrix(o)[:3, :3]
        x_dot = numpy.concatenate([
            world_from_body.dot(p_dot),
            world_from_body.dot(o_dot),
        ])

        world_from_desiredbody = transformations.quaternion_matrix(
            desired_o)[:3, :3]
        desired_x_dot = numpy.concatenate([
            world_from_body.dot(desired_p_dot),
            world_from_body.dot(desired_o_dot),
        ])
        desired_x_dotdot = numpy.concatenate([
            world_from_body.dot(desired_p_dotdot),
            world_from_body.dot(desired_o_dotdot),
        ])

        error_position_world = numpy.concatenate([
            desired_p - p,
            quat_to_rotvec(
                transformations.quaternion_multiply(
                    desired_o,
                    transformations.quaternion_inverse(o),
                )),
        ])
        if self.config['two_d_mode']:
            error_position_world = error_position_world * [1, 1, 0, 0, 0, 1]

        world_from_body2 = numpy.zeros((6, 6))
        world_from_body2[:3, :3] = world_from_body2[3:, 3:] = world_from_body
        body_gain = lambda x: world_from_body2.dot(x).dot(world_from_body2.T)

        error_velocity_world = (desired_x_dot + body_gain(
            numpy.diag(self.config['k'])).dot(error_position_world)) - x_dot
        if self.config['two_d_mode']:
            error_velocity_world = error_velocity_world * [1, 1, 0, 0, 0, 1]

        pd_output = body_gain(numpy.diag(
            self.config['ks'])).dot(error_velocity_world)

        output = pd_output
        if self.config['use_rise']:
            rise_term_int = body_gain(numpy.diag(self.config['ks'] * self.config['alpha'])).dot(error_velocity_world) + \
                            body_gain(numpy.diag(self.config['beta'])).dot(numpy.sign(error_velocity_world))

            self._rise_term = self._rise_term + dt / 2 * (
                rise_term_int + self._rise_term_int_prev)
            self._rise_term_int_prev = rise_term_int

            output = output + self._rise_term
        else:
            # zero rise term so it doesn't wind up over time
            self._rise_term = numpy.zeros(6)
            self._rise_term_int_prev = numpy.zeros(6)
        output = output + body_gain(
            numpy.diag(self.config['accel_feedforward'])).dot(desired_x_dotdot)
        output = output + body_gain(numpy.diag(
            self.config['vel_feedforward'])).dot(desired_x_dot)

        wrench_from_vec = lambda output: (world_from_body.T.dot(output[0:3]),
                                          world_from_body.T.dot(output[3:6]))
        return wrench_from_vec(pd_output), wrench_from_vec(output)
예제 #8
0
파일: tools.py 프로젝트: uf-mil/PropaGator
def orientation_from_pose(p):
    return quat_to_rotvec(xyzw_array(p.orientation))
예제 #9
0
def main(nh):

    while not rospy.is_shutdown():

        boat = yield boat_scripting.get_boat(nh)
        #boat.pan_lidar(min_angle=2.7, max_angle=3.14, freq=.75)
        #boat.float_on()
        #gate_viz_pub = rospy.Publisher('gates_viz', Marker, queue_size = 10)
        #rospy.init_node("sdfhjhsdfjhgsdf")
        angle_to_move = 0

        hold = []
        x_mean = 0
        y_mean = 0
        numerator = 0
        denom = 0
        pointcloud_base = []

        # loops until lidar gets a good filtered lidar reading
        while len(hold) <= 0:
            pointcloud = yield boat.get_pointcloud()
            yield util.sleep(.1) # sleep to avoid tooPast errors
            pointcloud_base = yield boat.to_baselink(pointcloud)
            yield util.sleep(.1) # sleep to avoid tooPast errors

            pointcloud_enu = []

            for p in pc2.read_points(pointcloud, field_names=("x", "y", "z"), skip_nans=False, uvs=[]):
                pointcloud_enu.append((p[0], p[1], p[2]))

            yield util.sleep(.1) # sleep to avoid tooPast errors

            point_in_base = []
            hold = []

            # Filter lidar data to only data right in front of the boat
            print "Filtering lidar data"
            for i in range(0, len(pointcloud_base)):
                temp = pointcloud_base[i]
                if abs(temp[1]) < .3:
                    hold.append(pointcloud_enu[i])

        for i in hold:
            x_mean += i[0]
            y_mean += i[1]

        y_mean = y_mean/len(hold)
        x_mean = x_mean/len(hold)

        for i in hold:
            numerator += (i[0] - x_mean)*(i[1] - y_mean)
            denom += (i[0] - x_mean)*(i[0] - x_mean)

        if denom == 0: denom = 1
        m = numerator/denom
        b = y_mean - (m * x_mean)

        x1 = 1
        x2 = 100

        par_vect = m*1 + b
        point2 = m*x2 + b

        position = boat.odom.position[0:2]
        yaw = quat_to_rotvec(boat.odom.orientation)[2]
        heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])

        # Line we want to be orthagonal to
        position1 = to_vector(x1, par_vect)
        position3 = to_vector((heading[0]) + boat.odom.position[0], (heading[1]) + boat.odom.position[1])
        position4 = to_vector((heading[0]+ .01) + boat.odom.position[0], (heading[1]+.1) + boat.odom.position[1])
        
        #Vector to be parrallel to 
        vec1 = numpy.array([x1, par_vect, 0])
        vec2 = ([0,0,0])

        angle_to_move = 2
        move = 0
        angle_to_move = 1

        #def find_angle():
        # Calculte angle_to_move and position to move to get in front of object
        position = boat.odom.position[0:2]
        yaw = quat_to_rotvec(boat.odom.orientation)[2]
        heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
        vec2 = numpy.array([(heading[0]), (heading[1]), 0])

        numerator = numpy.dot(vec1, vec2)
        one = numpy.linalg.norm(vec1)
        two =  numpy.linalg.norm(vec2)
        cosine = numerator / (one * two)
        angle_to_move = math.acos(cosine)



        #find_angle()

        distances = yield boat.get_distance_from_object(.05)
        theda_one = .9 - angle_to_move
        L1 = distances[0]
        distance_to_move = math.sin(theda_one) * L1

        veccc = to_vector(x1,par_vect)
        vecc = to_vector(x2, point2)

        '''

        m = Marker()
        m.header = Header(frame_id = '/enu', stamp = rospy.Time.now())
        m.ns = 'gates'
        m.id = 1
        m.type = Marker.LINE_STRIP
        m.action = Marker.ADD
        m.scale.x = 0.1
        m.color.r = 0.5
        m.color.b = 0.75
        m.color.g = 0.1
        m.color.a = 1.0
        m.points.append(veccc)
        m.points.append(vecc)
        gate_viz_pub.publish(m)

        '''


        if vec2[0] < 0:
            print "Yawing left ", angle_to_move
            print "Moving right ", distance_to_move 
            raw_input("Press enter to continue...")
            boat.move.turn_left(angle_to_move).go()
            yield boat.move.left(-distance_to_move-2).go()
        if vec2[0] > 0:
            print "Yawing right ", angle_to_move
            print "Moving left ", distance_to_move 
            raw_input("Press enter to continue...")
            boat.move.turn_left(angle_to_move).go()
            yield boat.move.left(distance_to_move-2).go()
예제 #10
0
def main(nh):
    # #Print mission start msg
    #print 'Finding start gate with laser'
    boat = yield boat_scripting.get_boat(nh)

    try:
        yaw = quat_to_rotvec(boat.odom.orientation)[2]
        #print 'Pan the lidar between the maximum angle and slightly above horizontal'
        boat.pan_lidar(max_angle=3.264, min_angle=3.15, freq=0.5)

        # How many gates we've gone through
        gates_passed = 0

        have_gate = False
        last_gate_pos = None
        move = None

        while gates_passed < 2:
            #print 'Get gates'
            gates = yield boat.get_gates()

            (gate_pos, gate_orientation) = (None, None)
            if gates_passed == 0:
                (gate_pos,
                 gate_orientation) = filter_gates(boat, gates, yaw, False)
            else:
                (gate_pos,
                 gate_orientation) = filter_gates(boat, gates, yaw, True)

            # Check if valid gate found
            if gate_pos is not None:
                have_gate = True
                # Check if we previously found a gate
                if last_gate_pos is not None:
                    # Check if the gate center has drifted
                    # Don't go to a different gate (dis < 5)
                    dis = numpy.linalg.norm(last_gate_pos - gate_pos)
                    #print 'Distance: ', dis
                    if dis >= 0.1 and dis < 3:
                        print 'Gate drifted re-placing goal point'
                        #move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
                        if gates_passed == 0:
                            move = move_on_line.main(nh, gate_pos,
                                                     gate_orientation,
                                                     -GATE_DISTANCE)
                        else:
                            move = move_on_line.main(nh, gate_pos,
                                                     gate_orientation,
                                                     GATE_DISTANCE)
                    else:
                        #print 'Still happy with my current goal'
                        pass
                else:
                    # Just found a gate
                    print 'Moving to gate ' + str(gates_passed + 1)
                    #move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
                    if gates_passed == 0:
                        #yield boat.move.yaw_left_deg(30).go()
                        move = move_on_line.main(nh, gate_pos,
                                                 gate_orientation,
                                                 -GATE_DISTANCE)
                        print 'zzz'
                        yield util.sleep(1.0)
                    else:
                        move = move_on_line.main(nh, gate_pos,
                                                 gate_orientation,
                                                 GATE_DISTANCE)

                # Set last gate pos
                last_gate_pos = gate_pos

            else:
                if have_gate is False:
                    print 'No gate found moving forward 1m'
                    yield boat.move.forward(1).go()
                else:
                    print 'Lost sight of gate; Continuing to last known position'

            # Check if task complete
            if have_gate and move.called:
                print 'Move complete'
                #yield boat.move.forward(3).go()
                yaw = quat_to_rotvec(boat.odom.orientation)[2]
                have_gate = False
                last_gate_pos = None
                gates_passed = gates_passed + 1

    finally:
        boat.default_state()
예제 #11
0
    def main_loop(self, event):

        self.lock.acquire()

        #print 'desired state', desired_state
        #print 'current_state', state
        def smallest_coterminal_angle(x):
            return (x + math.pi) % (2 * math.pi) - math.pi

        # sub pd-controller sans rise
        rot = None
        # Get tf from /enu to /base_link
        #   Since we are dealing with differences and not absolute positions not translation is required
        try:
            (_,
             rot) = self.tf_listener.lookupTransform('/base_link', '/enu',
                                                     rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException) as e:
            rospy.logwarn('Tf exception: ' + str(e))

        if rot is not None:
            # Convert to euler angle ( we only care about rotation about z)
            theta = quat_to_rotvec(numpy.array(rot))[2]

            # Difference in /enu frame
            to_desired_state = self.desired_state[0:2] - self.state[0:2]

            # Convert to base_link
            s = numpy.sin(theta)
            c = numpy.cos(theta)
            rot_matrix_enu_base = numpy.array([[c, -s], [s, c]])
            to_desired_state = rot_matrix_enu_base.dot(to_desired_state)
            to_desired_state = numpy.insert(to_desired_state, 2,
                                            0)  # Append a zero for z

            #                                    Note: Angular differences are the same in base_link as enu so no tf required
            e = numpy.concatenate([
                to_desired_state,
                map(smallest_coterminal_angle,
                    self.desired_state[3:6] - self.state[3:6])
            ])  # e_1 in paper

            # Convert desired state dot into base_link
            #   angular velocities do not rquire transformation as they are differences (rendering them frameless)
            #   To convert velocites from enu + desired_orientation to base link:
            #       transform to enu: rotate the velocity vector by desired_orientation
            #       transform to base link: rotate the velocity vector by the angle between enu and base_link
            vels = self.desired_state_dot[0:2]
            theta2 = self.desired_state[5]
            s = numpy.sin(theta2)
            c = numpy.cos(theta2)
            rot_matrix_desired_enu = numpy.array([[c, -s], [s, c]])
            vels = rot_matrix_desired_enu.dot(vels)
            vels = rot_matrix_enu_base.dot(vels)

            vels = numpy.insert(vels, 2, 0)

            desired_state_dot = numpy.concatenate(
                [vels, self.desired_state_dot[3:6]])

            #print 'Desired_state tf: ', desired_state_dot
            e_dot = desired_state_dot - self.state_dot
            output = self.K_p.dot(e) + self.K_d.dot(e_dot)

            # Apply simple moving average filter
            new = output / self.num_to_avg
            old = (self.outputs[0] / self.num_to_avg)
            self.last_average = self.last_average - old + new
            self.outputs.popleft()
            self.outputs.append(output)

            # Resuse output var
            #print 'Outputs: ', self.outputs
            output = self.last_average

            #print 'Last Average: ', output
            self.lock.release()

            self.x_error = e[0]
            self.y_error = e[1]
            self.z_error = e[5]

            self.dx_error = e_dot[0]
            self.dy_error = e_dot[1]
            self.dz_error = e_dot[5]

            #self.to_terminal()

            if (not (self.odom_active)):
                output = [0, 0, 0, 0, 0, 0]
            if (self.enable & self.killed == False):
                self.controller_wrench.publish(
                    WrenchStamped(header=Header(
                        stamp=rospy.Time.now(),
                        frame_id="/base_link",
                    ),
                                  wrench=Wrench(
                                      force=Vector3(x=output[0],
                                                    y=output[1],
                                                    z=0),
                                      torque=Vector3(x=0, y=0, z=output[5]),
                                  )))

                if ((self.x_error < 1) & (self.y_error < 1) &
                    (self.z_error < 1)):
                    self.waypoint_progress.publish(True)

            if (self.killed == True):
                rospy.logwarn('PD_Controller KILLED: %s' %
                              self.kill_listener.get_kills())
                self.controller_wrench.publish(
                    WrenchStamped(header=Header(
                        stamp=rospy.Time.now(),
                        frame_id="/base_link",
                    ),
                                  wrench=Wrench(
                                      force=Vector3(x=0, y=0, z=0),
                                      torque=Vector3(x=0, y=0, z=0),
                                  )))

        else:
            self.lock.release()
예제 #12
0
def main(nh):
    # #Print mission start msg
    #print 'Finding start gate with laser'
    boat = yield boat_scripting.get_boat(nh)

    try:
        yaw = quat_to_rotvec(boat.odom.orientation)[2]
        #print 'Pan the lidar between the maximum angle and slightly above horizontal'
        boat.pan_lidar(max_angle=3.264, min_angle=3.15, freq=0.5)
        
        # How many gates we've gone through
        gates_passed = 0

        have_gate = False
        last_gate_pos = None
        move = None

        while gates_passed < 2:
            #print 'Get gates'    
            gates = yield boat.get_gates()

            (gate_pos, gate_orientation) = (None, None) 
            if gates_passed == 0:
                (gate_pos, gate_orientation) = filter_gates(boat, gates, yaw, False)
            else:
                (gate_pos, gate_orientation) = filter_gates(boat, gates, yaw, True)

            # Check if valid gate found
            if gate_pos is not None:
                have_gate = True
                # Check if we previously found a gate
                if last_gate_pos is not None:
                    # Check if the gate center has drifted
                    # Don't go to a different gate (dis < 5)
                    dis = numpy.linalg.norm(last_gate_pos - gate_pos)
                    #print 'Distance: ', dis
                    if dis >= 0.1 and dis < 3:
                        print 'Gate drifted re-placing goal point'
                        #move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
                        if gates_passed == 0:
                            move = move_on_line.main(nh, gate_pos, gate_orientation, -GATE_DISTANCE)
                        else:
                            move = move_on_line.main(nh, gate_pos, gate_orientation, GATE_DISTANCE)
                    else:
                        #print 'Still happy with my current goal'
                        pass
                else:
                    # Just found a gate    
                    print 'Moving to gate ' + str(gates_passed + 1)
                    #move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
                    if gates_passed == 0:
                        #yield boat.move.yaw_left_deg(30).go()
                        move = move_on_line.main(nh, gate_pos, gate_orientation, -GATE_DISTANCE)
                        print 'zzz'
                        yield util.sleep(1.0)
                    else:
                        move = move_on_line.main(nh, gate_pos, gate_orientation, GATE_DISTANCE)

                # Set last gate pos
                last_gate_pos = gate_pos

            else:
                if have_gate is False:
                    print 'No gate found moving forward 1m'
                    yield boat.move.forward(1).go()
                else:
                    print 'Lost sight of gate; Continuing to last known position'

            # Check if task complete
            if have_gate and move.called:
                print 'Move complete'
                #yield boat.move.forward(3).go()
                yaw = quat_to_rotvec(boat.odom.orientation)[2]
                have_gate = False
                last_gate_pos = None
                gates_passed = gates_passed + 1
        
    finally:
        boat.default_state()
예제 #13
0
def orient(boat):

    angle_to_move = 0
    final_cloud = []
    x_mean = 0
    y_mean = 0
    numerator = 0
    denom = 0
    pointcloud_base = []

    # loops until lidar gets a good filtered lidar reading
    while len(final_cloud) <= 0:
        pointcloud = yield boat.get_pointcloud()
        yield util.sleep(.1)  # sleep to avoid tooPast errors
        pointcloud_base = yield boat.to_baselink(pointcloud)
        yield util.sleep(.1)  # sleep to avoid tooPast errors

        pointcloud_enu = []

        # Append pointcloud data to enu
        for p in pc2.read_points(pointcloud,
                                 field_names=("x", "y", "z"),
                                 skip_nans=False,
                                 uvs=[]):
            pointcloud_enu.append((p[0], p[1], p[2]))
        yield util.sleep(.1)  # sleep to avoid tooPast errors

        final_cloud = []

        # Filter lidar data to only data right in front of the boat
        # Since both pointcloud have the sane index for the same points,
        # we then use the baselink pointcloud data to fiter enu points in front of the boat

        print "Filtering lidar data"
        for i in range(0, len(pointcloud_base)):
            temp = pointcloud_base[i]
            if abs(temp[1]) < .3:
                final_cloud.append(pointcloud_enu[i])

    # Use final point cloud to get x and y mean to be used in line generation
    for i in final_cloud:
        x_mean += i[0]
        y_mean += i[1]

    # Find x-mean and y-mean
    y_mean = y_mean / len(final_cloud)
    x_mean = x_mean / len(final_cloud)

    for i in final_cloud:
        numerator += (i[0] - x_mean) * (i[1] - y_mean)
        denom += (i[0] - x_mean) * (i[0] - x_mean)

    if denom == 0: denom = 1
    m = numerator / denom
    b = y_mean - (m * x_mean)

    x1 = 1
    x2 = 100

    # Vector we want to be parallel to
    parallel_vector = m * 1 + b

    # Gets heading of the boat as unit vector
    position = boat.odom.position[0:2]
    yaw = quat_to_rotvec(boat.odom.orientation)[2]
    heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])

    # Vector to be parrallel to
    vec1 = numpy.array([x1, parallel_vector, 0])
    # Vector of the boat
    vec2 = numpy.array([(heading[0]), (heading[1]), 0])

    angle_to_move = 2
    move = 0
    angle_to_move = 1

    # Finds angle between boat unit vector and dock
    numerator = numpy.dot(vec1, vec2)
    one = numpy.linalg.norm(vec1)
    two = numpy.linalg.norm(vec2)
    cosine = numerator / (one * two)
    # Angle betwen the boat and the shore
    angle_to_move = math.acos(cosine)

    # Scan lidar to get the distances from the shore to the boat for use in creating triangle
    distances = yield boat.get_distance_from_object(.05)

    # Caluculate distance to move to be directly in front of point
    theda_one = .9 - angle_to_move
    L1 = distances[0]
    distance_to_move = math.sin(theda_one) * L1

    if vec2[0] < 0:
        #print "Yawing left ", angle_to_move
        print "Moving right ", distance_to_move
        #boat.move.turn_left(angle_to_move).go()
        left = boat.move.left(-distance_to_move - 2).go()
        forward = boat.move.forward(distance_to_move - 3).go()
        yield left, forward

    if vec2[0] > 0:
        #print "Yawing right ", angle_to_move
        print "Moving left ", distance_to_move
        #boat.move.turn_left(angle_to_move).go()
        left = boat.move.left(distance_to_move - 2).go()
        forward = boat.move.forward(distance_to_move - 3).go()
        yield left, forward
예제 #14
0
    def main_loop(self, event):
        
        self.lock.acquire()
        #print 'desired state', desired_state
        #print 'current_state', state
        def smallest_coterminal_angle(x):
            return (x + math.pi) % (2*math.pi) - math.pi

        

        # sub pd-controller sans rise
        rot = None
        # Get tf from /enu to /base_link
        #   Since we are dealing with differences and not absolute positions not translation is required
        try:
            (_, rot) = self.tf_listener.lookupTransform('/base_link', '/enu', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
            rospy.logwarn('Tf exception: ' + str(e))

        if rot is not None:
            # Convert to euler angle ( we only care about rotation about z)
            theta = quat_to_rotvec(numpy.array(rot))[2] 

            # Difference in /enu frame
            to_desired_state = self.desired_state[0:2] - self.state[0:2]

            # Convert to base_link
            s = numpy.sin(theta)
            c = numpy.cos(theta)
            rot_matrix_enu_base = numpy.array([[c, -s],
                                               [s, c]])
            to_desired_state = rot_matrix_enu_base.dot(to_desired_state)
            to_desired_state = numpy.insert(to_desired_state, 2, 0) # Append a zero for z


            #                                    Note: Angular differences are the same in base_link as enu so no tf required
            e = numpy.concatenate([to_desired_state, map(smallest_coterminal_angle, self.desired_state[3:6] - self.state[3:6])]) # e_1 in paper

            # Convert desired state dot into base_link
            #   angular velocities do not rquire transformation as they are differences (rendering them frameless)
            #   To convert velocites from enu + desired_orientation to base link:
            #       transform to enu: rotate the velocity vector by desired_orientation
            #       transform to base link: rotate the velocity vector by the angle between enu and base_link
            vels = self.desired_state_dot[0:2]
            theta2 = self.desired_state[5]
            s = numpy.sin(theta2)
            c = numpy.cos(theta2)
            rot_matrix_desired_enu = numpy.array([[c, -s],
                                                  [s, c]])
            vels = rot_matrix_desired_enu.dot(vels)
            vels = rot_matrix_enu_base.dot(vels)

            vels = numpy.insert(vels, 2, 0)

            desired_state_dot = numpy.concatenate([vels, self.desired_state_dot[3:6]])

            #print 'Desired_state tf: ', desired_state_dot
            e_dot = desired_state_dot - self.state_dot
            output = self.K_p.dot(e) + self.K_d.dot(e_dot)

            # Apply simple moving average filter
            new = output / self.num_to_avg
            old = (self.outputs[0] / self.num_to_avg) 
            self.last_average = self.last_average - old + new
            self.outputs.popleft()
            self.outputs.append(output)

            # Resuse output var
            #print 'Outputs: ', self.outputs
            output = self.last_average

            #print 'Last Average: ', output
            self.lock.release()

            self.x_error = e[0]
            self.y_error = e[1]
            self.z_error = e[5]

            self.dx_error = e_dot[0]
            self.dy_error = e_dot[1]
            self.dz_error = e_dot[5]

            #self.to_terminal()            
            
            if (not(self.odom_active)):
                output = [0,0,0,0,0,0]
            if (self.enable & self.killed==False):
                self.controller_wrench.publish(WrenchStamped(
                    header = Header(
                        stamp=rospy.Time.now(),
                        frame_id="/base_link",
                        ),
                    wrench=Wrench(
                        force = Vector3(x= output[0],y= output[1],z= 0),
                        torque = Vector3(x=0,y= 0,z= output[5]),
                        ))
                        
                        )

                if((self.x_error < 1) & (self.y_error < 1) & (self.z_error < 1)):
                        self.waypoint_progress.publish(True)

            if (self.killed == True):
                rospy.logwarn('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
                self.controller_wrench.publish(WrenchStamped(
                        header = Header(
                            stamp=rospy.Time.now(),
                            frame_id="/base_link",
                            ),
                        wrench=Wrench(
                            force = Vector3(x= 0,y= 0,z= 0),
                            torque = Vector3(x=0,y= 0,z= 0),
                            ))
                            
                            )

        else:
            self.lock.release()
예제 #15
0
파일: tools.py 프로젝트: NAJILUC/PropaGator
def orientation_from_pose(p):
	return quat_to_rotvec(xyzw_array(p.orientation))