Python indexed_measurement_t示例

示例#1

文件： atlasdriver.py 项目： jediofgever/director

    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)

示例#2

文件： atlasdriver.py 项目： RobotLocomotion/director

    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)

示例#3

文件： send_indexed_measurement.py 项目： iit-DLSLab/pronto

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())

示例#4

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())