def on_pose_body(channel, data):
global atlas_state, joint_names
if (atlas_state.num_joints==0):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
o.num_joints = atlas_state.num_joints
#o.joint_name = ["" for x in range(o.num_joints)]
o.joint_name = joint_name_list
o.joint_position = atlas_state.joint_position
o.joint_velocity = atlas_state.joint_velocity
o.joint_effort = atlas_state.joint_effort
o.pose.translation.x =m.pos[0];
o.pose.translation.y =m.pos[1];
o.pose.translation.z =m.pos[2];
o.pose.rotation.w = m.orientation[0];
o.pose.rotation.x = m.orientation[1];
o.pose.rotation.y = m.orientation[2];
o.pose.rotation.z = m.orientation[3];
lc.publish("EST_ROBOT_STATE",o.encode())
def on_pose_body(channel, data):
global atlas_state, joint_names
if (atlas_state.num_joints == 0):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
o.num_joints = atlas_state.num_joints
#o.joint_name = ["" for x in range(o.num_joints)]
o.joint_name = joint_name_list
o.joint_position = atlas_state.joint_position
o.joint_velocity = atlas_state.joint_velocity
o.joint_effort = atlas_state.joint_effort
o.pose.translation.x = m.pos[0]
o.pose.translation.y = m.pos[1]
o.pose.translation.z = m.pos[2]
o.pose.rotation.w = m.orientation[0]
o.pose.rotation.x = m.orientation[1]
o.pose.rotation.y = m.orientation[2]
o.pose.rotation.z = m.orientation[3]
lc.publish("EST_ROBOT_STATE", o.encode())
def on_pose(channel, data):
m = pose_t.decode(data)
if (mode is 0):
print m.utime * 1E-6, " vic"
else:
print m.utime * 1E-6, ",0,0"
def on_pb(channel, data):
# record a queue of messages and compute the largest translational error between them, display that.
global correctionTime, initialPoseBodyMsg, counterPosesAfterInitial
msg = pose_t.decode(data)
# Get time start
global utimeStart
if utimeStart == None:
utimeStart = msg.utime
# if there is a correction, record the current pose body we know that the next
# measurement will be a correction, therefore we need to store this pose, in
# order to estimate the trajectory
if msg.utime > correctionTime and correctionTime != None:
if initialPoseBodyMsg == None:
initialPoseBodyMsg = msg
return
# once we have received a correction - don't record the first pose body
correctionTime = None
# now check if we have received a pose body initially, only then compute the
# difference and publish it
if initialPoseBodyMsg != None:
# compute and publish the diff
counterPosesAfterInitial = counterPosesAfterInitial + 1
publish_error(initialPoseBodyMsg, msg)
def on_pose(channel, data):
m = pose_t.decode(data)
if (mode is 0):
print m.utime, " bdi"
else:
print m.utime, ",0,0"
def on_pose_body(channel, data):
if (skip_joint_angles is False):
if (received_joint_state is False):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
o.num_joints = joint_state.num_joints
o.joint_name = joint_state.joint_name
o.joint_position = joint_state.joint_position
o.joint_velocity = joint_state.joint_velocity
o.joint_effort = joint_state.joint_effort
o.pose.translation.x =m.pos[0];
o.pose.translation.y =m.pos[1];
o.pose.translation.z =m.pos[2];
o.pose.rotation.w = m.orientation[0];
o.pose.rotation.x = m.orientation[1];
o.pose.rotation.y = m.orientation[2];
o.pose.rotation.z = m.orientation[3];
lc.publish("EST_ROBOT_STATE",o.encode())
def on_pose(channel, data):
m = pose_t.decode(data)
if (mode is 0):
print m.utime, " bdi"
else:
print m.utime,",0,0"
def on_pose(channel, data):
global utime_last_print, str_alt, str_body, str_vicon
m = pose_t.decode(data)
rpy = botpy.quat_to_euler(m.orientation)
str_out = channel + ", " + str(m.utime) + ", "
str_out = str_out + str(m.pos[0]) + ", " + str(m.pos[1]) + ", " + str(m.pos[2]) + ", "
str_out = str_out + str(m.orientation[0]) + ", " + str(m.orientation[1]) + ", " + str(m.orientation[2]) + ", " + str(m.orientation[3]) + ", "
str_out = str_out + str(rpy[0]*180.0/math.pi) + ", " + str(rpy[1]*180.0/math.pi) + ", " + str(rpy[2]*180.0/math.pi)
if (channel == "POSE_BODY_ALT"):
str_alt = str_out
elif (channel == "POSE_BODY"):
str_pose = str_out
elif (channel == "POSE_VICON"):
str_vicon = str_out
if (m.utime > utime_last_print + 1E6):
if (channel == "POSE_BODY"):
if (str_alt is None):
return
if (str_pose is None):
return
if (str_vicon is None):
return
utime_last_print = m.utime
print str_pose
print str_alt
print str_vicon
def on_pose(channel, data):
m = pose_t.decode(data)
if (mode is 0):
print m.utime*1E-6, " vic"
else:
print m.utime*1E-6,",0,0"
def on_pose(channel, data):
global utime_last_print, str_alt, str_body, str_vicon
m = pose_t.decode(data)
rpy = botpy.quat_to_euler(m.orientation)
str_out = channel + ", " + str(m.utime) + ", "
str_out = str_out + str(m.pos[0]) + ", " + str(m.pos[1]) + ", " + str(
m.pos[2]) + ", "
str_out = str_out + str(m.orientation[0]) + ", " + str(
m.orientation[1]) + ", " + str(m.orientation[2]) + ", " + str(
m.orientation[3]) + ", "
str_out = str_out + str(rpy[0] * 180.0 / math.pi) + ", " + str(
rpy[1] * 180.0 / math.pi) + ", " + str(rpy[2] * 180.0 / math.pi)
if (channel == "POSE_BODY_ALT"):
str_alt = str_out
elif (channel == "POSE_BODY"):
str_pose = str_out
elif (channel == "POSE_VICON"):
str_vicon = str_out
if (m.utime > utime_last_print + 1E6):
if (channel == "POSE_BODY"):
if (str_alt is None):
return
if (str_pose is None):
return
if (str_vicon is None):
return
utime_last_print = m.utime
print str_pose
print str_alt
print str_vicon
def on_pose(channel, data):
m = pose_t.decode(data)
delta_time = (m.utime - s.prev_utime) / 1E6
if (delta_time < 1):
return
rpy = quat_to_euler(m.orientation)
xyz = m.pos
measure_drift(delta_time, m.utime, xyz, rpy)
def on_pose(channel, data):
m = pose_t.decode(data)
delta_time = (m.utime - s.prev_utime)/1E6
if (delta_time < 1):
return
rpy = quat_to_euler(m.orientation )
xyz = m.pos
measure_drift(delta_time, m.utime, xyz, rpy)
def on_pose(channel, data):
m = pose_t.decode(data)
rpy = botpy.quat_to_euler(m.orientation)
print rpy[2] * 180.0 / math.pi
rpy[2] = rpy[2] #- 20.0*math.pi/180.0
print rpy[2] * 180.0 / math.pi
m.orientation = botpy.euler_to_quat(rpy)
pos = list(m.pos)
pos[0] = pos[0] - 0.05
print pos[0]
m.pos = pos
lc.publish("POSE_BODY_CORRECTED", m.encode())
quit()
def on_pose_body(channel, data):
m = pose_t.decode(data)
out = indexed_measurement_t()
out.utime = m.utime;
out.state_utime = m.utime;
out.measured_dim = 4;
out.z_indices = [8,9,10,11] # yaw, x, y, z
#msg->state[8]
# out.z_effective = { rpyl[2] , msg->state[9], msg->state[10], msg->state[11]};
out.z_effective = [0.1,0,0,0]# msg->state[8] , msg->state[9], msg->state[10], msg->state[11]};
out.measured_cov_dim = 16;
out.R_effective = [0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0]
out.R_effective[0] = 1000#cl_cfg_.yaw_cov; // yaw
out.R_effective[5] = 1000#cl_cfg_.xyz_cov; // x
out.R_effective[10] = 1000#cl_cfg_.xyz_cov; // y
out.R_effective[15] = 1000#cl_cfg_.xyz_cov; // z
lc.publish("GPF_MEASUREMENT_QUICK_LOCK", out.encode())
def on_pose_body(channel, data):
global core_robot_state, joint_names
if (core_robot_state.num_joints==0):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
o.num_joints = core_robot_state.num_joints
o.joint_name = core_robot_state.joint_name
o.joint_position = core_robot_state.joint_position
o.joint_velocity = core_robot_state.joint_velocity
o.joint_effort = core_robot_state.joint_effort
nrot = quaternion_t()
nvec = vector_3d_t()
p = position_3d_t()
p.rotation = nrot
p.translation = nvec
o.pose = p
t = twist_t()
t.linear_velocity = nvec
t.angular_velocity = nvec
o.twist = t
o.pose.translation.x =m.pos[0];
o.pose.translation.y =m.pos[1];
o.pose.translation.z =m.pos[2];
o.pose.rotation.w = m.orientation[0];
o.pose.rotation.x = m.orientation[1];
o.pose.rotation.y = m.orientation[2];
o.pose.rotation.z = m.orientation[3];
ft = force_torque_t()
o.force_torque = ft
lc.publish("EST_ROBOT_STATE",o.encode())
def on_pose_gt(channel, data):
m = pose_t.decode(data)
rpy = botpy.quat_to_euler(m.orientation)
# pre-init:
if (s.last_est.utime < 0):
return
# after the reliable time
s.counter = s.counter + 1
if (s.counter % 10 == 1):
string1 = ''
string1 = string1 + str(m.utime) +", "+ str(m.pos[0]) +", "+ str(m.pos[1]) +", "+ str(m.pos[2])
string1 = string1+ ", " +str(m.orientation[0]) +", "+ str(m.orientation[1]) +", "+ str(m.orientation[2]) +", "+ str(m.orientation[3])
d= s.last_est
string1 = string1 + ", " +str(d.utime) +", "+ str(d.pos[0]) +", "+ str(d.pos[1]) +", "+ str(d.pos[2])
string1 = string1 + ", " +str(d.orientation[0]) +", "+ str(d.orientation[1]) +", "+ str(m.orientation[2]) +", "+ str(d.orientation[3])
print string1
def on_pose_body(channel, data):
global core_robot_state, joint_names
if (core_robot_state.num_joints == 0):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
o.num_joints = core_robot_state.num_joints
o.joint_name = core_robot_state.joint_name
o.joint_position = core_robot_state.joint_position
o.joint_velocity = core_robot_state.joint_velocity
o.joint_effort = core_robot_state.joint_effort
nrot = quaternion_t()
nvec = vector_3d_t()
p = position_3d_t()
p.rotation = nrot
p.translation = nvec
o.pose = p
t = twist_t()
t.linear_velocity = nvec
t.angular_velocity = nvec
o.twist = t
o.pose.translation.x = m.pos[0]
o.pose.translation.y = m.pos[1]
o.pose.translation.z = m.pos[2]
o.pose.rotation.w = m.orientation[0]
o.pose.rotation.x = m.orientation[1]
o.pose.rotation.y = m.orientation[2]
o.pose.rotation.z = m.orientation[3]
ft = force_torque_t()
o.force_torque = ft
lc.publish("EST_ROBOT_STATE", o.encode())
def on_pose_body(channel, data):
m = pose_t.decode(data)
out = indexed_measurement_t()
out.utime = m.utime
out.state_utime = m.utime
out.measured_dim = 4
out.z_indices = [8, 9, 10, 11] # yaw, x, y, z
#msg->state[8]
# out.z_effective = { rpyl[2] , msg->state[9], msg->state[10], msg->state[11]};
out.z_effective = [
0.1, 0, 0, 0
] # msg->state[8] , msg->state[9], msg->state[10], msg->state[11]};
out.measured_cov_dim = 16
out.R_effective = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
out.R_effective[0] = 1000 #cl_cfg_.yaw_cov; // yaw
out.R_effective[5] = 1000 #cl_cfg_.xyz_cov; // x
out.R_effective[10] = 1000 #cl_cfg_.xyz_cov; // y
out.R_effective[15] = 1000 #cl_cfg_.xyz_cov; // z
lc.publish("GPF_MEASUREMENT_QUICK_LOCK", out.encode())
def on_pose_body(channel, data):
m = pose_t.decode(data)
if (state.got_pose_body == False): print "Received", channel
state.got_pose_body = True
state.pose_body = getBotCorePose3dAsBotTrans(m)
def on_pose(channel, data): m = pose_t.decode(data) print m.utime, " bdi"
def on_pose(channel, data):
estimatedPose = pose_t.decode(data)
if viconPose != 0:
errors = double_array_t()
errors.utime = estimatedPose.utime
errors.num_values = 2
errors.values = [0] * errors.num_values
# Compute 3D translation error
transErr = math.sqrt(
pow(viconPose.pos[0] - estimatedPose.pos[0], 2.0) +
pow(viconPose.pos[1] - estimatedPose.pos[1], 2.0) +
pow(viconPose.pos[2] - estimatedPose.pos[2], 2.0))
# Compute 3D rotation error
viconRot = quat_to_rot(viconPose.orientation)
estimatedRot = quat_to_rot(estimatedPose.orientation)
try:
viconRotInverse = np.linalg.inv(viconRot)
except np.linalg.LinAlgError:
# Not invertible. Skip this one.
pass
deltaRot = np.dot(estimatedRot, viconRotInverse)
traceRot = deltaRot.trace()
rotErr = math.acos((traceRot - 1) / 2) * 180.0 / math.pi
# Set output variable
errors.values[0] = transErr
errors.values[1] = rotErr
# Get time in seconds
global index
if index == 0:
global utimeStart
utimeStart = errors.utime
utimeCurrent = (errors.utime - utimeStart)
timeCurrent = utimeCurrent * 1e-6
# Set updated correction flag
global updatedCorrection
updatedFlag = False
if updatedCorrection:
updatedFlag = True
updatedCorrection = False
# Write to file
global file
# rowIndex, time [s], translErr [m], rotErr [m], updatedCorrectionFlag
file.write('%d \t' % index)
file.write('%f \t' % timeCurrent)
file.write('%f \t' % transErr)
file.write('%f \t' % rotErr)
file.write('%i \n' % updatedFlag)
index = index + 1
# Publish to LCM
lc.publish("POSE_ERRORS", errors.encode())
statusMsg = ""
startTime = 0
imageChannels = []
lastPose =pose_t()
lastPose.utime =-1;
imageFnames = {}
imagePoses = {}
imageMeta = {}
imagesWritten =0
for e in log:
if msgCount == 0:
startTime = e.timestamp
if e.channel=="POSE":
lastPose = pose_t.decode(e.data);
elif lastPose.utime<0:
continue
msgCount = msgCount + 1
if (msgCount % 5000) == 0:
statusMsg = deleteStatusMsg(statusMsg)
statusMsg = "read % d messages, % d %% done, %d images written" % (msgCount, log.tell() / float(log.size())*100,imagesWritten)
sys.stderr.write(statusMsg)
sys.stderr.flush()
#if (msgCount>=100000):
#break
isImage = channels.match(e.channel)
if not isImage:
def on_pose_est(channel, data): m = pose_t.decode(data) s.last_est = m
def on_pose(channel, data):
errorTransform = pose_t.decode(data)
errorsToPub = double_array_t()
errorsToPub.utime = errorTransform.utime
errorsToPub.num_values = 8
errorsToPub.values = [0] * errorsToPub.num_values
# Compute 3D translation error
translErr = math.sqrt(
pow(errorTransform.pos[0], 2.0) + pow(errorTransform.pos[1], 2.0) +
pow(errorTransform.pos[2], 2.0))
# Compute 3D rotation error
errorRotMatrix = quat_to_rot(errorTransform.orientation)
traceRot = errorRotMatrix.trace()
rotErr = 180.0 - (math.acos((traceRot - 1) / 2) * 180.0 / math.pi)
# Decomposed errors
xErr = errorTransform.pos[0]
yErr = errorTransform.pos[1]
zErr = errorTransform.pos[2]
yawErr = math.atan(errorRotMatrix[1, 0] / errorRotMatrix[0, 0])
pitchErr = math.atan(-errorRotMatrix[2, 0] / math.sqrt(
pow(errorRotMatrix[2, 1], 2.0) + pow(errorRotMatrix[2, 2], 2.0)))
rollErr = math.atan(errorRotMatrix[2, 1] / errorRotMatrix[2, 2])
# Set output variable
errorsToPub.values[0] = translErr
errorsToPub.values[1] = rotErr
errorsToPub.values[2] = xErr
errorsToPub.values[3] = yErr
errorsToPub.values[4] = zErr
errorsToPub.values[5] = yawErr
errorsToPub.values[6] = pitchErr
errorsToPub.values[7] = rollErr
# Get time in seconds
global index
if index == 0:
global utimeStart
utimeStart = errorsToPub.utime
utimeCurrent = (errorsToPub.utime - utimeStart)
timeCurrent = utimeCurrent * 1e-6
# Set updated correction flag
global updatedCorrection
updatedFlag = False
if updatedCorrection:
updatedFlag = True
updatedCorrection = False
# Write to file
global file
# rowIndex, time [s], translErr [m], rotErr [m], updatedCorrectionFlag
file.write('%d \t' % index)
file.write('%f \t' % timeCurrent)
file.write('%f \t' % translErr)
file.write('%f \t' % rotErr)
file.write('%i \n' % updatedFlag)
index = index + 1
# Publish to LCM
lc.publish("POSE_ERRORS", errorsToPub.encode())
def on_pose_est(channel, data): m = pose_t.decode(data) #print '%.6f' % (m.utime*1e-6) s.most_recent_est = m
def on_pose(channel, data):
m = pose_t.decode(data)
print m.utime, " bdi"
statusMsg = ""
startTime = 0
imageChannels = []
lastPose = pose_t()
lastPose.utime = -1
imageFnames = {}
imagePoses = {}
imageMeta = {}
imagesWritten = 0
for e in log:
if msgCount == 0:
startTime = e.timestamp
if e.channel == "POSE":
lastPose = pose_t.decode(e.data)
elif lastPose.utime < 0:
continue
msgCount = msgCount + 1
if (msgCount % 5000) == 0:
statusMsg = deleteStatusMsg(statusMsg)
statusMsg = "read % d messages, % d %% done, %d images written" % (
msgCount, log.tell() / float(log.size()) * 100, imagesWritten)
sys.stderr.write(statusMsg)
sys.stderr.flush()
#if (msgCount>=100000):
#break
isImage = channels.match(e.channel)
def on_correction(channel, data):
global correctionTime
# record the timestamp at which we have received the message
msg = pose_t.decode(data)
correctionTime = msg.utime
def on_pose(channel, data):
m = pose_t.decode(data)
#print "p %s" % (p.utime)
dt = float(timestamp_now() - m.utime) / 1000000
def on_vicon(channel, data):
global viconPose
viconPose = pose_t.decode(data)
def pose_handler(self, channel, data):
msg = pose_t.decode(data)
#gobject.idle_add(self.callback, "turn_right")
t = threading.Thread( target=functools.partial(self.callback , "turn_right"))
t.start()
def on_pose_gt(channel, data):
m = pose_t.decode(data)
#print ' %.6f' % (m.utime*1e-6)
rpy = botpy.quat_to_euler(m.orientation)
# pre-init:
if (s.most_recent_est.utime < 0):
return
if (s.last.utime < 0):
s.last = m
s.last_est = s.most_recent_est
return
# after the reliable time
#if (m.utime - s.last.utime > 1e6):
if decide_new_measurement(m):
#print '%.6f' % (s.last.utime*1e-6)
gt_a = poseToBotTrans(s.last)
gt_b = poseToBotTrans(m)
gt_ab = botpy.transform_relative(gt_a, gt_b)
se_a = poseToBotTrans(s.last_est)
se_b = poseToBotTrans(s.most_recent_est)
se_ab = botpy.transform_relative(se_a, se_b)
#print "==========="
#print gt_ab.trans_vec
#print se_ab.trans_vec
distanceTravelled = np.linalg.norm(np.array(m.pos) - np.array(s.last.pos))
drift_xyz = np.array(se_ab.trans_vec) - np.array(gt_ab.trans_vec)
drift = np.linalg.norm(np.array(se_ab.trans_vec) - np.array(gt_ab.trans_vec))
percentDriftPerDistanceTravelled = 100*drift/distanceTravelled
# yaw is postive anti clockwise, zero facing towards x
gt_ab_yaw = botpy.quat_to_euler(gt_ab.rot_quat)[2]*180.0/np.pi
se_ab_yaw = botpy.quat_to_euler(se_ab.rot_quat)[2]*180.0/np.pi
yaw_error = se_ab_yaw - gt_ab_yaw
#print "======================"
#print "======================"
#print "yaw gt pre: ", botpy.quat_to_euler(s.last.orientation)[2]*180.0/np.pi
#print "yaw gt now: ", botpy.quat_to_euler(m.orientation)[2]*180.0/np.pi
#print "yaw se pre: ", botpy.quat_to_euler(s.last_est.orientation)[2]*180.0/np.pi
#print "yaw se now: ", botpy.quat_to_euler(s.most_recent_est.orientation)[2]*180.0/np.pi
#print "======================"
#print gt_ab_yaw
#print se_ab_yaw
print yaw_error, "deg yaw drift |", drift_xyz, "or" , drift, "m drift in", distanceTravelled , "m travelled, thus" , percentDriftPerDistanceTravelled, "%DDT"
em = error_metrics_t()
em.utime = m.utime
em.pos_error = drift_xyz
em.pos_error_norm = drift
em.rpy_error = [0,0,yaw_error]
em.distance_travelled = distanceTravelled
em.time_elapsed = (s.last.utime - m.utime)*1e-6
em.percent_ddt = percentDriftPerDistanceTravelled
lc.publish("PRONTO_ERROR", em.encode())
s.last = m
s.last_est = s.most_recent_est
return
def on_pose_body(channel, data): m = pose_t.decode(data) if (state.got_pose_body == False): print "Received",channel state.got_pose_body = True state.pose_body = getBotCorePose3dAsBotTrans(m)
def on_pose(channel, data): m = pose_t.decode(data) #print "p %s" % (p.utime) dt = float( timestamp_now() - m.utime )/1000000
