def simple_imu_handler(channel, data): global target msg = mbot_imu_t.decode(data) if False: # and frame_count % 5 == 0: print(msg.accel) target.write(str(msg.utime) + ',' + str(msg.gyro[0]) + ',' + str(msg.gyro[1]) + ',' + str(msg.gyro[2])) target.write(',' + str(msg.accel[0]) + ',' + str(msg.accel[1]) + ',' + str(msg.accel[2]) + '\n')
def convert(in_filename, out_filename): log = lcm.EventLog(in_filename, "r") seq = 0 prev_time = None with rosbag.Bag(out_filename, 'w') as bag: for event in log: ros_msg = None channel = None if event.channel == "ODOMETRY": lcm_msg = odometry_t.decode(event.data) ros_msg = convert_pose(lcm_msg) channel = "odometry" elif event.channel == "SLAM_POSE": lcm_msg = pose_xyt_t.decode(event.data) ros_msg = convert_pose(lcm_msg) channel = "slam_pose" elif event.channel == "SLAM_MAP": lcm_msg = occupancy_grid_t.decode(event.data) ros_msg = convert_map(lcm_msg, prev_time) channel = "map" elif event.channel == "MBOT_IMU": lcm_msg = mbot_imu_t.decode(event.data) ros_msg = convert_imu(lcm_msg) channel = "imu" else: continue ros_msg.header.seq = seq seq += 1 prev_time = ros_msg.header.stamp bag.write(channel, ros_msg, t=ros_msg.header.stamp)
def simple_imu_handler(channel, data): msg = mbot_imu_t.decode(data) if False: # and frame_count % 5 == 0: print(msg.accel)