mobileroboticsskoltech / bandeja-wrapper Goto Github PK

import time
from src.RemoteControl import RemoteControl
from concurrent.futures import ThreadPoolExecutor
import subprocess
import rospy
from sensor_msgs.msg import Imu
import numpy as np
import pandas as pd
from io import StringIO
from src.TimeSync import TimeSync2

import matplotlib.pyplot as plt

HOST = '10.30.65.160'  # The smartphone's IP address

mcu_imu_time = []
mcu_imu_data = []


def callback(data):
    dat = data.header.stamp.secs + data.header.stamp.nsecs  / 1e9
    mcu_imu_time.append(dat)

    dat = data.angular_velocity
    mcu_imu_data.append([dat.x, dat.y, dat.z])

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("mcu_imu", Imu, callback)
    #rospy.spin()

def main():
    remote = RemoteControl(HOST)
    start_duration = 1
    main_duration = 4
    end_duration = 5

    with ThreadPoolExecutor(max_workers=1) as executor:
        print('imus gathering started')
        future = executor.submit(remote.get_imu, 1000 * (start_duration + main_duration + end_duration), False, True)
        listener()
        time.sleep(start_duration)
        print('start shaking')
        time.sleep(main_duration)
        print('put back')
        time.sleep(end_duration)
        rospy.signal_shutdown('it is enough')
        accel_data, gyro_data = future.result()
        print('stopped')


    mcu_gyro_data = np.array(mcu_imu_data) - np.array(mcu_imu_data)[:200].mean(axis=0)
    mcu_gyro_time = np.array(mcu_imu_time)
    print(gyro_data[:200])

    '''
    Some other code
    '''



if __name__ == '__main__':
    main()

imus gathering started
start shaking
put back
stopped
6.5392947,-7.188068,-3.8377817,68005547326543
-0.0012394455,-0.0012382239,-3.2620205E-4,68005549328543
-0.0012394455,-0.0012382239,-3.2620205E-4,68005551342543
-0.0012394455,-6.273586E-4,2.846632E-4,6
...