def slampose_feedback_handler(self, channel, data):
"""
Feedback Handler for slam pose
this is run when a feedback message is recieved
"""
msg = pose_xyt_t.decode(data)
self.slam_pose = (msg.x, msg.y, msg.theta)
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)
data = {
'slam_pose': {
'x': [],
'y': [],
'th': [],
'time': []
},
'odometry': {
'x': [],
'y': [],
'th': [],
'time': []
}
}
for event in log:
if event.channel == "SLAM_POSE":
msg = pose_xyt_t.decode(event.data)
data['slam_pose']['x'].append(msg.x)
data['slam_pose']['y'].append(msg.y)
data['slam_pose']['th'].append(msg.theta)
data['slam_pose']['time'].append(msg.utime)
if event.channel == "ODOMETRY":
msg = pose_xyt_t.decode(event.data)
data['odometry']['x'].append(msg.x)
data['odometry']['y'].append(msg.y)
data['odometry']['th'].append(msg.theta)
data['odometry']['time'].append(msg.utime)
pickle.dump(data, open("team_5_data.p", "wb"))
def slam_pose_handler(self, channel, data):
msg = pose_xyt_t.decode(data)
self.cur_pose.x = msg.x
self.cur_pose.y = msg.y
self.cur_pose.theta = msg.theta
# put lcm types you need here
# run make once to generate python lcm types
from lcmtypes import pose_xyt_t
if len(sys.argv) < 2:
sys.stderr.write("usage: decode-log <logfile>\n")
sys.exit(1)
log = lcm.EventLog(sys.argv[1], "r")
f = open("TvsS.csv", "w")
for event in log:
if event.channel == "TRUE_POSE":
msgT = pose_xyt_t.decode(event.data)
# haveTrue = 1
# print("TruePose:")
# print("timestamp= %d" % msg.utime)
# print("pose: (%f, %f, %f)" % (msg.x, msg.y, msg.theta))
if event.channel == "SLAM_POSE":
msgS = pose_xyt_t.decode(event.data)
# haveSlam = 1
# print("SlamPose:")
# print("timestamp= %d" % msgS.utime)
# print("pose: (%f, %f, %f)" % (msgS.x, msgS.y, msgS.theta))
# print("%d , %f , %f , %f , %f , %f , %f" % (msgT.utime, msgT.x, msgT.y, msgT.theta, msgS.x, msgS.y, msgS.theta))
f.write("%d , %f , %f , %f , %f , %f , %f \n" %
(msgT.utime, msgT.x, msgT.y, msgT.theta, msgS.x, msgS.y,
msgS.theta))