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