def on_robot_state(channel, data):
global state
m = robot_state_t.decode(data)
state.out_angle = m.joint_position[18]
state.out_utime = m.utime
if (state.pot_angle is not None):
print state.out_utime, state.out_angle, state.pot_angle
def on_robot_state(channel, data):
global state
m = robot_state_t.decode(data)
state.out_angle = m.joint_position[18]
state.out_utime = m.utime
if (state.pot_angle is not None):
print state.out_utime, state.out_angle, state.pot_angle
def on_ers(channel, data):
m = robot_state_t.decode(data)
delta_time = (m.utime - s.prev_utime)/1E6
if (delta_time < 1):
return
rpy = quat_to_euler([m.pose.rotation.w, m.pose.rotation.x, m.pose.rotation.y, m.pose.rotation.z] )
xyz = [m.pose.translation.x, m.pose.translation.y, m.pose.translation.z]
measure_drift(delta_time, m.utime, xyz, rpy)
def on_msg1(channel, data):
m = robot_state_t.decode(data)
for i in range(len(m.joint_name)):
joint = m.joint_name[i]
joint_out = mappings.get(joint)
if (joint_out is not None):
m.joint_name[i] = joint_out
else:
print joint , " not found"
lc.publish('EST_ROBOT_STATE', m.encode() )
def on_ers(channel, data):
m = robot_state_t.decode(data)
delta_time = (m.utime - s.prev_utime) / 1E6
if (delta_time < 1):
return
rpy = quat_to_euler([
m.pose.rotation.w, m.pose.rotation.x, m.pose.rotation.y,
m.pose.rotation.z
])
xyz = [m.pose.translation.x, m.pose.translation.y, m.pose.translation.z]
measure_drift(delta_time, m.utime, xyz, rpy)
def on_msg1(channel, data):
global test_init, test_stop
m = robot_state_t.decode(data)
print "got ers"
neckmsg = neck_pitch_t()
neckmsg.utime = timestamp_now()
neckmsg.pitch = 0.43
lc.publish('DESIRED_NECK_PITCH', neckmsg.encode())
time.sleep(10) # for the robot to drop onto the ground
msg = utime_t()
msg.utime = timestamp_now()
lc.publish('START_MIT_STAND', msg.encode())
print "Sent START_MIT_STAND"
lc.unsubscribe(sub1)
def on_msg1(channel, data):
global test_init, test_stop
m = robot_state_t.decode(data)
print "got ers"
neckmsg = neck_pitch_t()
neckmsg.utime = timestamp_now()
neckmsg.pitch = 0.43
lc.publish('DESIRED_NECK_PITCH', neckmsg.encode() )
time.sleep(10) # for the robot to drop onto the ground
msg = utime_t()
msg.utime = timestamp_now()
lc.publish('START_MIT_STAND', msg.encode() )
print "Sent START_MIT_STAND"
lc.unsubscribe(sub1)
def on_est_robot_state(channel, data):
m = robot_state_t.decode(data)
s.last_utime = m.utime
s.status.utime = m.utime
if (s.manip_until_utime > m.utime):
# manip plan still being executed
s.status.execution_status = 0 # EXECUTION_STATUS_EXECUTING
s.status.plan_type = 8 # manip
time_remaining = (s.manip_until_utime - m.utime)*1E-6
print "manip, time remaining: ", time_remaining
elif (s.walk_until_utime > m.utime):
# manip plan still being executed
s.status.execution_status = 0 # EXECUTION_STATUS_EXECUTING
s.status.plan_type = 2 # walking
time_remaining = (s.walk_until_utime - m.utime)*1E-6
print "walking, time remaining: ", time_remaining
else:
# manip or walking plan not being executed
s.status.plan_type = 1 # standing
s.status.execution_status = 2 # NO PLAN
lc.publish("PLAN_EXECUTION_STATUS", s.status.encode())
def on_est_robot_state(channel, data):
m = robot_state_t.decode(data)
s.last_utime = m.utime
def on_est_robot_state(channel, data): m = robot_state_t.decode(data) print "3, ",m.utime
def on_robot_state(channel, data):
m = robot_state_t.decode(data)
if (mode is 0):
print " ", m.utime, " ERS"
else:
print "0,0,", m.utime
def on_robot_state(channel, data):
m = robot_state_t.decode(data)
if (mode is 0):
print " ",m.utime, " ERS"
else:
print "0,0,",m.utime
