def sendInitMessage(self, pos, yaw): init = indexed_measurement_t() init.utime = getUtime() init.state_utime = init.utime init.measured_dim = 4 init.z_effective = [ pos[0], pos[1], pos[2] , yaw ] init.z_indices = [9, 10, 11, 8] init.measured_cov_dim = init.measured_dim*init.measured_dim init.R_effective= [0] * init.measured_cov_dim init.R_effective[0] = 0.25 init.R_effective[5] = 0.25 init.R_effective[10] = 0.25 init.R_effective[15] = math.pow( 50*math.pi/180 , 2 ) lcmUtils.publish('MAV_STATE_EST_VIEWER_MEASUREMENT', init)
def sendInitMessage(self, pos, yaw): init = indexed_measurement_t() init.utime = getUtime() init.state_utime = init.utime init.measured_dim = 4 init.z_effective = [pos[0], pos[1], pos[2], yaw] init.z_indices = [9, 10, 11, 8] init.measured_cov_dim = init.measured_dim * init.measured_dim init.R_effective = [0] * init.measured_cov_dim init.R_effective[0] = 0.25 init.R_effective[5] = 0.25 init.R_effective[10] = 0.25 init.R_effective[15] = math.pow(50 * math.pi / 180, 2) lcmUtils.publish('MAV_STATE_EST_VIEWER_MEASUREMENT', init)
######################################################################################## def timestamp_now (): return int (time.time () * 1000000) lc = lcm.LCM() print "0 started" o = utime_t() o.utime = timestamp_now () lc.publish("STATE_EST_RESTART",o.encode()) print '1 sent STATE_EST_RESTART' time.sleep(1) # Time in seconds. #################################################################### o = utime_t() o.utime = timestamp_now () lc.publish("STATE_EST_READY",o.encode()) print '2 sent STATE_EST_READY' time.sleep(1) # Time in seconds. #################################################################### m = indexed_measurement_t() m.utime = timestamp_now () m.state_utime = m.utime m.measured_dim = 4 m.z_effective = [0,0,0.85,0] m.z_indices = [9, 10, 11, 8] m.measured_cov_dim = 16 m.R_effective = [0.25, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0.75] lc.publish("MAV_STATE_EST_VIEWER_MEASUREMENT",m.encode()) print '3 sent MAV_STATE_EST_VIEWER_MEASUREMENT'
return int(time.time() * 1000000) lc = lcm.LCM() print "0 started" o = utime_t() o.utime = timestamp_now() lc.publish("STATE_EST_RESTART", o.encode()) print '1 sent STATE_EST_RESTART' time.sleep(1) # Time in seconds. #################################################################### o = utime_t() o.utime = timestamp_now() lc.publish("STATE_EST_READY", o.encode()) print '2 sent STATE_EST_READY' time.sleep(1) # Time in seconds. #################################################################### m = indexed_measurement_t() m.utime = timestamp_now() m.state_utime = m.utime m.measured_dim = 4 m.z_effective = [0, 0, 0.85, 0] m.z_indices = [9, 10, 11, 8] m.measured_cov_dim = 16 m.R_effective = [0.25, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0.75] lc.publish("MAV_STATE_EST_VIEWER_MEASUREMENT", m.encode()) print '3 sent MAV_STATE_EST_VIEWER_MEASUREMENT'