def main():
    measurement = np.zeros((2,1)) # Measurement vector
    state = np.zeros((2,1))       # Initial state vector [x,y,vx,vy]
    pose  = Pose2D()
    kalman = Kalman()
    navdata = navdata_info()

    rospy.init_node('filtered_tag_data')
    pub_kalman = rospy.Publisher('/kalman', Pose2D, queue_size=100)

    rate = rospy.Rate(200) # 200Hz

    while not rospy.is_shutdown():
        # measurement[0, 0] = navdata.norm_tag_x
        # measurement[1, 0] = navdata.norm_tag_y
        # measurement[2, 0] = navdata.tag_vx
        # measurement[3, 0] = navdata.tag_vy

        measurement[0, 0] = navdata.tag_x
        measurement[1, 0] = navdata.tag_y

        kalman.state_callback()
        kalman.measurement_callback(measurement)

        state = kalman.x

        pose.x = state[0, 0]
        pose.y = state[1, 0]

        pub_kalman.publish(pose)

        rate.sleep()
示例#2
0
def main():
    measurement = np.zeros((2, 1))  # Measurement vector
    state = np.zeros((2, 1))  # Initial state vector [x,y,vx,vy]
    pose = Pose2D()
    kalman = Kalman()
    navdata = navdata_info()

    rospy.init_node('filtered_tag_data')
    pub_kalman = rospy.Publisher('/kalman', Pose2D, queue_size=100)

    rate = rospy.Rate(200)  # 200Hz

    while not rospy.is_shutdown():
        # measurement[0, 0] = navdata.norm_tag_x
        # measurement[1, 0] = navdata.norm_tag_y
        # measurement[2, 0] = navdata.tag_vx
        # measurement[3, 0] = navdata.tag_vy

        measurement[0, 0] = navdata.tag_x
        measurement[1, 0] = navdata.tag_y

        kalman.state_callback()
        kalman.measurement_callback(measurement)

        state = kalman.x

        pose.x = state[0, 0]
        pose.y = state[1, 0]

        pub_kalman.publish(pose)

        rate.sleep()
示例#3
0
def main():
    measurement = np.zeros((2,1)) # Measurement vector
    state = np.zeros((2,1))       # Initial state vector [x,y,vx,vy]
    pose  = Pose2D()
    kalman = Kalman()
    navdata = navdata_info()

    # conversions
    x_conversion = 0.64
    y_conversion = 0.36

    rospy.init_node('filtered_tag_data')
    pub_kalman = rospy.Publisher('/filtered_tag_pose', Pose2D, queue_size=100)

    rate = rospy.Rate(200) # 200Hz

    while not rospy.is_shutdown():
        measurement[0, 0] = navdata.tag_x * x_conversion
        measurement[1, 0] = navdata.tag_y * y_conversion

        kalman.state_callback()
        kalman.measurement_callback(measurement)

        state = kalman.x

        pose.x = state[0, 0]
        pose.y = state[1, 0]

        pub_kalman.publish(pose)

        rate.sleep()
示例#4
0
tag_theta_workbook_sheet.write('B1', vertical + 'Theta')

tag_x_workbook_sheet.write('C1', 'Altitude')
tag_x_workbook_sheet.write('D1', 'Roll')
tag_x_workbook_sheet.write('E1', 'Pitch')

tag_y_workbook_sheet.write('C1', 'Altitude')
tag_y_workbook_sheet.write('D1', 'Roll')
tag_y_workbook_sheet.write('E1', 'Pitch')

# ROS initializations
rospy.init_node('test_data_saver')
rate = rospy.Rate(100)

# So we can get the data from the bag
nd = navdata_info()

# To keep track of what row we're in (start in row 2)
i = 2

start_time = 0
current_time = 0
time_set = False

while not rospy.is_shutdown():
    # Division by 1,000,000 to convert from microseconds to seconds
    current_time = nd.navdata.tm/1000000

    # so that the time that's saved starts at 0 and not the QC's system time
    # if you have issues, you can remove this without issue
    if (not time_set and (current_time > 0)):
def main():
    rospy.init_node('follow_mode_test')
    rate     = rospy.Rate(200) # 200Hz
    pub_ctrl = rospy.Publisher('cmd_vel', Twist, queue_size=100)

    qc      = Twist()
    navdata = navdata_info()
    ctrl    = Controller()

    ########################
    # Set the bounding box #
    ########################
    # X is in front and behind QC [0, 360] pixels
    # Y is left and right of QC   [0, 640] pixels
    # For pixels
    bbx_max = 625
    bbx_min = 375
    bby_max = 625
    bby_min = 375

    # For normalized distances
    # bbx_max = -1.0
    # bbx_min =  1.0
    # bby_max = -1.0
    # bby_min =  1.0
    yaw_max = 350
    yaw_min = 10

    ####################################
    # Setup the individual controllers #
    ####################################
    # Note: in order to change the values for integrator max and min, you need
    # to go into Controller.py and adjust the values there
    # Set yaw controller
    # NOTE: this doesn't seem to be working correctly...
    ctrl.pid_theta.setKp(1/260.0)
    # ctrl.pid_theta.setKp(0.0)
    ctrl.pid_theta.setKi(0.0)
    ctrl.pid_theta.setKd(0.0)
    ctrl.pid_theta.setPoint(180.0)
    default = 10
    # ctrl.pid_theta.setIntegrator(100)
    # ctrl.pid_theta.setDerivator(100)

    # Set the x (forward/backward) controller
    # ctrl.pid_x.setKp(5)
    ctrl.pid_x.setKp(0.01)
    ctrl.pid_x.setKi(0.005)
    ctrl.pid_x.setKd(0.03)
    # ctrl.pid_x.setKd(0.0)
    ctrl.pid_x.setPoint(500.0)
    # ctrl.pid_x.setPoint(0.0)
    #ctrl.pid_x.setIntegrator(5000.0)
    #ctrl.pid_x.setDerivator(5000.0)

    # Set the y (left/right) controller
    # ctrl.pid_y.setKp(5)
    ctrl.pid_y.setKp(0.3)
    ctrl.pid_y.setKi(0.005)
    ctrl.pid_y.setKd(0.1)
    ctrl.pid_y.setPoint(500.0)
    # ctrl.pid_y.setPoint(0.0)
    # ctrl.pid_y.setIntegrator(5000)
    # ctrl.pid_y.setDerivator(5000)

    # Set the z (altitude) controller
    ctrl.pid_z.setKp(0.0)
    ctrl.pid_z.setKi(0.0)
    ctrl.pid_z.setKd(0.0)
    ctrl.pid_z.setPoint(0.0)
    # ctrl.pid_z.setIntegrator(500)
    # ctrl.pid_z.setDerivator(500)

    # Disable hover mode
    qc.angular.x = 0.5
    qc.angular.y = 0.5

    # controller update values
    yaw_update = 0
    x_update   = 0
    y_update   = 0
    z_update   = 0

    # i = 0

    while not rospy.is_shutdown():
        # always update the altitude
        z_update = ctrl.pid_z.update(z_update)
        # print("Theta %.2f"  % navdata.theta)
        # print("(%d, %d)"  % (navdata.tag_x, navdata.tag_y))

        if navdata.tag_acquired:
            # print("Tag acquired %d" % i)
            # i += 1
            # If 10 < theta < 350 then let's rotate
            if ((yaw_min < navdata.theta) and (navdata.theta < yaw_max)):
                # We need to make sure that we have an offset so that the QC
                # rotates correctly
                # if((180 < navdata.theta) and (navdata.theta < yaw_max)):
                #     navdata.theta -= 360
                yaw_update  = ctrl.pid_theta.update(navdata.theta)
            else:
                yaw_update = 0

            # is_in_box(minimum, maximum, position)
            # if (is_in_box(bbx_min, bbx_max, navdata.tag_y) and is_in_box(bby_min, bby_max, navdata.tag_x)):
            # kalman filter testing
            if (is_in_box(bbx_min, bbx_max, pose.y) and is_in_box(bby_min, bby_max, pose.x)):
                x_update = 0
                y_update = 0
                qc.angular.x = 0.0
                qc.angular.y = 0.0
                # print("In the Box")
            # It's not in the bounding box therefore we should update the PIDs
            # and disable Hover mode
            else:
                # x_update = navdata.tag_norm_x
                # y_update = navdata.tag_norm_y
                # x_update  = ctrl.pid_x.update(x_update)
                # y_update  = ctrl.pid_y.update(y_update)
                # x_update  = ctrl.pid_x.update(navdata.tag_x) / 15
                # y_update  = ctrl.pid_y.update(navdata.tag_y) / 15
                x_update  = ctrl.pid_x.update(pose.x) / 15
                y_update  = ctrl.pid_y.update(pose.y) / 15
                qc.angular.x = 0.5
                qc.angular.y = 0.5
                # print("%.3f" % ctrl.pid_x.getError())

        # Make sure that we're not making any drastic updates
        # qc.angular.z = ctrl.pid_theta.avoid_drastic_corrections(yaw_update)
        # qc.linear.x  = ctrl.pid_x.avoid_drastic_corrections(x_update)
        # qc.linear.y  = ctrl.pid_y.avoid_drastic_corrections(y_update)
        # qc.linear.z  = ctrl.pid_z.avoid_drastic_corrections(z_update)

        qc.angular.z = yaw_update
        qc.linear.x  = x_update
        qc.linear.y  = y_update
        # qc.linear.z  = z_update

        pub_ctrl.publish(qc)
        rate.sleep()
示例#6
0
tag_y_workbook_sheet.write('D1', 'Roll')
tag_y_workbook_sheet.write('E1', 'Pitch')

#Battery
tag_x_workbook_sheet.write('F1', 'Battery')

#Magnetometer
tag_x_workbook_sheet.write('G1', 'magX')
tag_x_workbook_sheet.write('H1', 'magY')

# ROS initializations
rospy.init_node('test_data_saver')
rate = rospy.Rate(100)

# So we can get the data from the bag
nd = navdata_info()

# To keep track of what row we're in (start in row 2)
i = 2

start_time = 0
current_time = 0
time_set = False

while not rospy.is_shutdown():
    # Division by 1,000,000 to convert from microseconds to seconds
    current_time = nd.navdata.tm / 1000000

    # so that the time that's saved starts at 0 and not the QC's system time
    # if you have issues, you can remove this without issue
    if (not time_set and (current_time > 0)):