Can't record GPS and IMU data of UAV in a certain frequency?

[KermitLee]

Can't record GPS and IMU data of UAV in a certain frequency?

Question

My question

How to record GPS and IMU data of UAV in a certain frequency?

Include context on what I am trying to achieve

I coded the code to record the status information of the UAV and the code to control the flight of the UAV according to the path point in Python language.And use threads to run two part of the code, but

state = client. GetMultirotorState ()
While state. Landed_state = = airsim. LandedState. Landed: 

is always an error.

Context details

OS:win10, AirSim Current version, Python:3.7, Unreal version 4.25

Include details of what you already did to find answers

No solution has been found so far.

[jonyMarino]

Hi @KermitLee! Which error are you getting?

[KermitLee]

Hi @KermitLee! Which error are you getting?

This is my code.

    state = client.getMultirotorState()
    if state.landed_state == airsim.LandedState.Landed:
        print("take off failed...")
        sys.exit(1)
    state = client.getMultirotorState()
    s = pprint.pformat(state)
    print("state: %s" % s)
    try:
        _thread.start_new_thread(log_with_time, (trace_ty,))
    except:
        print("thread error")

    client.moveOnPathAsync(v_path_list, uav_speed).join()

And The part that reports the error is in the function 'log_with_time'.

def log_with_time(trace_ty=None):
    # print(client)
    # state = client.getMultirotorState()
    # s = pprint.pformat(state)
    # print("state: %s" % s)
    state = client.getMultirotorState()
    while state.landed_state != airsim.LandedState.Landed:
        write_file(client.getImuData(), client.getGpsData(),
                   client.getMagnetometerData().magnetic_field_body, trace_ty)
        time.sleep(2)

[KermitLee]

Hi @KermitLee! Which error are you getting?
Thank you for reply!

And

I want to know how to record the motion state of the UAV according to a certain frequency, and I need the UAV to fly along the route specified by me.
Regardless of the above code, I just want to specify the route and get the data.
Thanks you again!

[KermitLee]

I streamlined my code. It still doesn't work.

"""
airsim  get_uav_fly_data
"""
import airsim
# import numpy as np
# import math
import time
# import pprint
import sys
# import threading
import _thread


def write_file(IMU_data, GPS_data, mag_data):

    file_name = 'fly_data' + ".txt"
    all_data = dict()
    all_data['time'] = time.time()
    all_data['angular_velocity'] = [IMU_data.angular_velocity.x_val, IMU_data.angular_velocity.y_val,
                                    IMU_data.angular_velocity.z_val]
    all_data['linear_acceleration'] = [IMU_data.linear_acceleration.x_val, IMU_data.linear_acceleration.y_val,
                                       IMU_data.linear_acceleration.z_val]
    all_data['altitude'] = GPS_data.gnss.geo_point.altitude
    all_data['longitude'] = GPS_data.gnss.geo_point.longitude
    all_data['latitude'] = GPS_data.gnss.geo_point.latitude
    all_data['magnetometer'] = [mag_data.x_val, mag_data.y_val, mag_data.z_val]
    with open(file_name, 'a+', encoding='utf-8') as f:
        data_str = ''
        for d_key, d_val in all_data.items():
            if d_key is 'angular_velocity':
                data_str += ','.join(list(map(str, d_val))) + ','
            elif d_key is 'linear_acceleration':
                data_str += ','.join(list(map(str, d_val))) + ','
            elif d_key is 'magnetometer':
                data_str += ','.join(list(map(str, d_val)))
            else:
                data_str += str(d_val) + ','
        f.write(data_str + '\n')


def log_with_time(sleep_time):
    state = client.getMultirotorState()
    while state.landed_state != airsim.LandedState.Landed:
        write_file(client.getImuData(), client.getGpsData(),
                   client.getMagnetometerData().magnetic_field_body)
        time.sleep(sleep_time)


def fly_by_path():
    z = -40
    path_list = list()
    # fly straight
    for i in range(400):
        path_list.append([i * 0.125, 0, z])
    v_path_list = list()
    origin_z = path_list[-1][-1]
    for p in path_list:
        v_path_list.append(airsim.Vector3r(p[0], p[1], p[2]))
    del path_list

    state = client.getMultirotorState()
    if state.landed_state == airsim.LandedState.Landed:
        print("taking off...")
        client.takeoffAsync().join()
    else:
        client.hoverAsync()

    time.sleep(0.2)

    state = client.getMultirotorState()
    if state.landed_state == airsim.LandedState.Landed:
        print("take off failed...")
        sys.exit(1)

    try:
        _thread.start_new_thread(log_with_time, (0.2))
    except:
        print("thread error")

    client.moveOnPathAsync(v_path_list, 2).join()
    client.moveToPositionAsync(0, 0, origin_z, 2).join()
    # land
    client.landAsync().join()


# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)

fly_by_path()

client.armDisarm(False)
client.reset()
client.enableApiControl(False)

[KermitLee]

I thought of an idea. I can use the native logging API. Record it in file C:\Users<User>\Documents\AirSim\2021-04-17-XX.
Like this.

This sample can record relative coordinates and attitude information.
But it still can't record information from other sensors like GPS and IMU.
I checked the source code XXXX\AirSim\Unreal\Environments\Blocks\Plugins\AirSim\Source\PawnSimApi.cpp.
Find it.

I think I can add some code here.
But how do I call the function that gets the GPS and IMU?
I don't know how to change this file; I'm afraid of wrecking the project.
Could you give me some help?Thank you very much.

[rajat2004]

Hi, try adding a method in MultirotorPawnSimApi.cpp similar to the one in CarPawnSimApi.cpp -

AirSim/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp

Line 36 in d566869

std::string CarPawnSimApi::getRecordFileLine(bool is_header_line) const

You should be able to get the sensor data using something like - getVehicleApiBase()->getImuData(), haven't tested it. Referring to your comment below, the method PawnSimApi::getVehicleApiBase() returns a nullptr since it's a base class and there's no vehicle currently, the actual vehicles are in CarPawnSimApi.cpp/.h, etc. For e.g. see

AirSim/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h

Line 45 in d566869

virtual msr::airlib::VehicleApiBase* getVehicleApiBase() const override

[KermitLee]

Hi, try adding a method in MultirotorPawnSimApi.cpp similar to the one in CarPawnSimApi.cpp -

AirSim/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp

Line 36 in d566869

std::string CarPawnSimApi::getRecordFileLine(bool is_header_line) const

You should be able to get the sensor data using something like - getVehicleApiBase()->getImuData(), haven't tested it. Referring to your comment below, the method PawnSimApi::getVehicleApiBase() returns a nullptr since it's a base class and there's no vehicle currently, the actual vehicles are in CarPawnSimApi.cpp/.h, etc. For e.g. see

AirSim/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h

Line 45 in d566869

virtual msr::airlib::VehicleApiBase* getVehicleApiBase() const override

Hi, @rajat2004. Thank you so much! I realized my idea in your way.I have added a function(getRecordFileLine) in MultirotorPawnSimapi.cpp.

std::string MultirotorPawnSimApi::getRecordFileLine(bool is_header_line) const
{
    std::string common_line = PawnSimApi::getRecordFileLine(is_header_line);
    if (is_header_line) {
        return common_line +
            "altitude\tlatitude\tlongitude\tBarometer\tMagetometer\t";
    }

    const auto& state = vehicle_api_->getMultirotorState();
    const auto& bar_data = vehicle_api_->getBarometerData("");
    const auto& mag_data = vehicle_api_->getMagnetometerData("");

    std::ostringstream ss;
    ss << common_line;
    ss << state.gps_location.altitude << "\t" << state.gps_location.latitude << "\t"
        << state.gps_location.longitude << "\t";

    ss << bar_data.pressure << "\t";

    ss << mag_data.magnetic_field_body.x() << "\t" << mag_data.magnetic_field_body.y() << "\t"
        << mag_data.magnetic_field_body.z() << "\t";
    return ss.str();
}

Thank you again for helping me.

[KermitLee]

Is it not finished yet?I seem to need to keep working on this function to get what I want.

[zimmy87]

Hi @KermitLee, I ran your script on my machine locally, and got a exception on ln 77: "2nd arg must be a tuple", which I was able to fix by changing _thread.start_new_thread(log_with_time, (0.2)) to _thread.start_new_thread(log_with_time, (0.2,)). However I'm now getting an exception from ln 43: "RuntimeError: IOLoop is already running"; have you run into this yourself or know of a work-around?

[KermitLee]

Hi @KermitLee, I ran your script on my machine locally, and got a exception on ln 77: "2nd arg must be a tuple", which I was able to fix by changing _thread.start_new_thread(log_with_time, (0.2)) to _thread.start_new_thread(log_with_time, (0.2,)). However I'm now getting an exception from ln 43: "RuntimeError: IOLoop is already running"; have you run into this yourself or know of a work-around?

Hi @zimmy87, sorry, I can't help you. I have the same problem, so I have put forward this discussion.

[thomasTUK]

@KermitLee were you able to write your GPS and IMU data to a txt file? I want to achieve the same and was wondering you'd done it now.

[KermitLee]

Yes, I can.
You can see the reply #3624 (reply in thread) .

I accomplished my goal through that reply.

[Rick-Weng]

U also can realize the function with " client.moveOnPathAsync(v_path_list, 20, 20000) ", if u are not add ".join()" , the drone will fly and code will continue execute, and then u can set a for loop to recording.

[KermitLee]

U also can realize the function with " client.moveOnPathAsync(v_path_list, 20, 20000) ", if u are not add ".join()" , the drone will fly and code will continue execute, and then u can set a for loop to recording.

Yes, thank you for the method. I have solved my problem.

[tanyaspaul]

Hello there I tried adding this to the MultirotorPawnSimapi.cpp however it still seems to not work. Any advice on how to do it? Did you do any additional edit to the PawnSimApi.cpp doc so that it dismisses the default recording file?

std::string MultirotorPawnSimApi::getRecordFileLine(bool is_header_line) const
{
std::string common_line = PawnSimApi::getRecordFileLine(is_header_line);
if (is_header_line) {
return common_line +
"altitude\tlatitude\tlongitude\tBarometer\tMagetometer\t";
}

const auto& state = vehicle_api_->getMultirotorState();
const auto& bar_data = vehicle_api_->getBarometerData("");
const auto& mag_data = vehicle_api_->getMagnetometerData("");

std::ostringstream ss;
ss << common_line;
ss << state.gps_location.altitude << "\t" << state.gps_location.latitude << "\t"
    << state.gps_location.longitude << "\t";

ss << bar_data.pressure << "\t";

ss << mag_data.magnetic_field_body.x() << "\t" << mag_data.magnetic_field_body.y() << "\t"
    << mag_data.magnetic_field_body.z() << "\t";
return ss.str();

}
Thanks for your help