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)