amcl.cc

机器人仿真软件

    }

    // Resample the particles
    pf_update_resample(this->pf);

    // Read out the current hypotheses
    double max_weight = 0.0;
    pf_vector_t max_weight_pose;
    this->hyp_count = 0;
    for (i = 0; (size_t) i < sizeof(this->hyps) / sizeof(this->hyps[0]); i++)
    {
      if (!pf_get_cluster_stats(this->pf, i, &weight, &pose_mean, &pose_cov))
        break;

      //pf_vector_fprintf(pose_mean, stdout, "%.3f");

      hyp = this->hyps + this->hyp_count++;
      hyp->weight = weight;
      hyp->pf_pose_mean = pose_mean;
      hyp->pf_pose_cov = pose_cov;

      if(hyp->weight > max_weight)
      {
        max_weight = hyp->weight;
        max_weight_pose = hyp->pf_pose_mean;
      }
    }

    if(max_weight > 0.0)
    {
      pthread_mutex_lock(&this->best_hyp_lock);
      this->best_hyp = max_weight_pose;
      pthread_mutex_unlock(&this->best_hyp_lock);
    }

#ifdef INCLUDE_RTKGUI
    // Update the GUI
    if (this->enable_gui)
      this->UpdateGUI();
#endif

    // Encode data to send to server
    this->PutDataLocalize(ts);
    this->PutDataPosition(delta);

    return true;
  }
  else
  {
    // Process the remaining sensor data up to the next action data
    while (1)
    {
      data = this->Peek();
      if (data == NULL)
        break;
      if (data->sensor->is_action)
        break;
      data = this->Pop();
      assert(data);
      delete data; data = NULL;
    }

    // Encode data to send to server; only the position interface
    // gets updated every cycle
    this->PutDataPosition(delta);

    return false;
  }
}

// this function will be passed to qsort(3) to sort the hypoths before
// sending them out.  the idea is to sort them in descending order of
// weight.
int
hypoth_compare(const void* h1, const void* h2)
{
  const player_localize_hypoth_t* hyp1 = (const player_localize_hypoth_t*)h1;
  const player_localize_hypoth_t* hyp2 = (const player_localize_hypoth_t*)h2;
  if(hyp1->alpha < hyp2->alpha)
    return(1);
  else if(hyp1->alpha == hyp2->alpha)
    return(0);
  else
    return(-1);
}


////////////////////////////////////////////////////////////////////////////////
// Output data on the localize interface
void AdaptiveMCL::PutDataLocalize(double time)
{
  int i;
  amcl_hyp_t *hyp;
  pf_vector_t pose;
  pf_matrix_t pose_cov;
  size_t datalen;
  player_localize_data_t data;

  // Record the number of pending observations
  data.pending_count = 0;
  data.pending_time = 0.0;

  // Encode the hypotheses
  data.hypoths_count = this->hyp_count;
  for (i = 0; i < this->hyp_count; i++)
  {
    hyp = this->hyps + i;

    // Get the current estimate
    pose = hyp->pf_pose_mean;
    pose_cov = hyp->pf_pose_cov;

    // Check for bad values
    if (!pf_vector_finite(pose))
    {
      pf_vector_fprintf(pose, stderr, "%e");
      assert(0);
    }
    if (!pf_matrix_finite(pose_cov))
    {
      pf_matrix_fprintf(pose_cov, stderr, "%e");
      assert(0);
    }

    data.hypoths[i].alpha = hyp->weight;

    data.hypoths[i].mean.px = pose.v[0];
    data.hypoths[i].mean.py = pose.v[1];
    data.hypoths[i].mean.pa = pose.v[2];

    data.hypoths[i].cov[0] = pose_cov.m[0][0];
    data.hypoths[i].cov[1] = pose_cov.m[1][1];
    data.hypoths[i].cov[2] = pose_cov.m[2][2];
  }

  // sort according to weight
  qsort((void*)data.hypoths,data.hypoths_count,
        sizeof(player_localize_hypoth_t),&hypoth_compare);

  // Compute the length of the data packet
  datalen = (sizeof(data) - sizeof(data.hypoths) +
             data.hypoths_count * sizeof(data.hypoths[0]));

  // Push data out
  this->Publish(this->localize_addr, NULL,
                PLAYER_MSGTYPE_DATA,
                PLAYER_LOCALIZE_DATA_HYPOTHS,
                (void*)&data,datalen,NULL);
}


////////////////////////////////////////////////////////////////////////////////
// Output data on the position interface
void AdaptiveMCL::PutDataPosition(pf_vector_t delta)
{
  pf_vector_t pose;
  player_position2d_data_t data;

  memset(&data,0,sizeof(data));

  // Get the current estimate
  pthread_mutex_lock(&this->best_hyp_lock);
  pose = this->best_hyp;
  pthread_mutex_unlock(&this->best_hyp_lock);

  // Add the accumulated odometric change
  pose = pf_vector_coord_add(delta, pose);

  data.pos.px = pose.v[0];
  data.pos.py = pose.v[1];
  data.pos.pa = pose.v[2];

  // Push data out
  this->Publish(this->position_addr, NULL,
                PLAYER_MSGTYPE_DATA,
                PLAYER_POSITION2D_DATA_STATE,
                (void*)&data,sizeof(data),NULL);
}

// MessageHandler
int
AdaptiveMCL::ProcessMessage(MessageQueue * resp_queue,
                            player_msghdr * hdr,
                            void * data)
{
  player_localize_set_pose_t* setposereq;

  // Is it a request to set the filter's pose?
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                           PLAYER_LOCALIZE_REQ_SET_POSE,
                           this->localize_addr))
  {
    if(hdr->size != sizeof(player_localize_set_pose_t))
    {
      PLAYER_ERROR2("request is wrong length (%d != %d); ignoring",
                    hdr->size, sizeof(player_localize_set_pose_t));
      return(-1);
    }
    setposereq = (player_localize_set_pose_t*)data;

    pf_vector_t pose;
    pf_matrix_t cov;

    pose.v[0] = setposereq->mean.px;
    pose.v[1] = setposereq->mean.py;
    pose.v[2] = setposereq->mean.pa;

    cov = pf_matrix_zero();
    cov.m[0][0] = setposereq->cov[0];
    cov.m[1][1] = setposereq->cov[1];
    cov.m[2][2] = setposereq->cov[2];

    // Re-initialize the filter
    this->pf_init_pose_mean = pose;
    this->pf_init_pose_cov = cov;
    this->pf_init = false;

    // Send an ACK
    this->Publish(this->localize_addr, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK,
                  PLAYER_LOCALIZE_REQ_SET_POSE);
    return(0);
  }
  // Is it a request for the current particle set?
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                PLAYER_LOCALIZE_REQ_GET_PARTICLES,
                                this->localize_addr))
  {
    if(hdr->size != 0)
    {
      PLAYER_ERROR2("request is wrong length (%d != %d); ignoring",
                    hdr->size, 0);
      return(PLAYER_MSGTYPE_RESP_NACK);
    }


    pf_vector_t mean;
    double var;
    player_localize_get_particles_t resp;
    pf_sample_set_t *set;
    pf_sample_t *sample;
    size_t i;

    pf_get_cep_stats(this->pf, &mean, &var);

    resp.mean.px = mean.v[0];
    resp.mean.py = mean.v[1];
    resp.mean.pa = mean.v[2];
    resp.variance = var;

    set = this->pf->sets + this->pf->current_set;

    resp.particles_count =
            MIN(set->sample_count,PLAYER_LOCALIZE_PARTICLES_MAX);

    // TODO: pick representative particles
    for(i=0;i<resp.particles_count;i++)
    {
      sample = set->samples + i;
      resp.particles[i].pose.px = sample->pose.v[0];
      resp.particles[i].pose.py = sample->pose.v[1];
      resp.particles[i].pose.pa = sample->pose.v[2];
      resp.particles[i].alpha = sample->weight;
    }

    this->Publish(this->localize_addr, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK,
                  PLAYER_LOCALIZE_REQ_GET_PARTICLES,
                  (void*)&resp, sizeof(resp), NULL);
    return(0);
  } else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                  PLAYER_POSITION2D_REQ_GET_GEOM, device_addr))
  {
    assert(hdr->size == 0);
    ProcessGeom(resp_queue, hdr);
    return(0);
  }

  // pass on the rest of the messages to the sensors
  for (int i = 0; i < this->sensor_count; i++)
  {
    int ret = this->sensors[i]->ProcessMessage(resp_queue, hdr, data);
    if (ret >= 0)
    	return ret;
  }

  return(-1);
}

void
AdaptiveMCL::ProcessGeom(MessageQueue* resp_queue, player_msghdr_t* hdr)
{
  player_position2d_geom_t geom;
  // just return a point so we don't get errors from playerv
  geom.pose.px = 0;
  geom.pose.py = 0;
  geom.pose.pa = 0;
  geom.size.sl = 0.01;
  geom.size.sw = 0.01;

  Publish(this->position_addr, resp_queue,
          PLAYER_MSGTYPE_RESP_ACK,
          PLAYER_POSITION2D_REQ_GET_GEOM,
          &geom, sizeof(geom), NULL);
}

#ifdef INCLUDE_RTKGUI

////////////////////////////////////////////////////////////////////////////////
// Set up the GUI
int AdaptiveMCL::SetupGUI(void)
{
  int i;

  // Initialize RTK
  rtk_init(NULL, NULL);

  this->app = rtk_app_create();

  this->canvas = rtk_canvas_create(this->app);
  rtk_canvas_title(this->canvas, "AdaptiveMCL");

  /* TODO: fix
  if (this->map != NULL)
  {
    rtk_canvas_size(this->canvas, this->map->size_x, this->map->size_y);
    rtk_canvas_scale(this->canvas, this->map->scale, this->map->scale);
  }
  else
  */
  {
    rtk_canvas_size(this->canvas, 400, 400);
    rtk_canvas_scale(this->canvas, 0.1, 0.1);
  }

  this->map_fig = rtk_fig_create(this->canvas, NULL, -1);
  this->pf_fig = rtk_fig_create(this->canvas, this->map_fig, 5);

  /* FIX
  // Draw the map
  if (this->map != NULL)
  {
    map_draw_occ(this->map, this->map_fig);
    //map_draw_cspace(this->map, this->map_fig);

    // HACK; testing
    map_draw_wifi(this->map, this->map_fig, 0);
  }
  */

  rtk_fig_line(this->map_fig, 0, -100, 0, +100);
  rtk_fig_line(this->map_fig, -100, 0, +100, 0);

  // Best guess figure
  this->robot_fig = rtk_fig_create(this->canvas, NULL, 9);

  // Draw the robot
  rtk_fig_color(this->robot_fig, 0.7, 0, 0);
  rtk_fig_rectangle(this->robot_fig, 0, 0, 0, 0.40, 0.20, 0);

  // TESTING
  //rtk_fig_movemask(this->robot_fig, RTK_MOVE_TRANS | RTK_MOVE_ROT);

  // Initialize the sensor GUI's
  for (i = 0; i < this->sensor_count; i++)
    this->sensors[i]->SetupGUI(this->canvas, this->robot_fig);

  rtk_app_main_init(this->app);

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shut down the GUI
int AdaptiveMCL::ShutdownGUI(void)
{
  int i;

  // Finalize the sensor GUI's
  for (i = 0; i < this->sensor_count; i++)
    this->sensors[i]->ShutdownGUI(this->canvas, this->robot_fig);

  rtk_fig_destroy(this->robot_fig);
  rtk_fig_destroy(this->pf_fig);
  rtk_fig_destroy(this->map_fig);

  rtk_canvas_destroy(this->canvas);
  rtk_app_destroy(this->app);

  rtk_app_main_term(this->app);

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Update the GUI
void AdaptiveMCL::UpdateGUI(void)
{
  rtk_fig_clear(this->pf_fig);
  rtk_fig_color(this->pf_fig, 0, 0, 1);
  pf_draw_samples(this->pf, this->pf_fig, 1000);
  pf_draw_cep_stats(this->pf, this->pf_fig);
  //rtk_fig_color(this->pf_fig, 0, 1, 0);
  //pf_draw_stats(this->pf, this->pf_fig);
  //pf_draw_hist(this->pf, this->pf_fig);

  // Draw the best pose estimate
  this->DrawPoseEst();

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Draw the current best pose estimate
void AdaptiveMCL::DrawPoseEst()
{
  int i;
  double max_weight;
  amcl_hyp_t *hyp;
  pf_vector_t pose;

  this->Lock();

  max_weight = -1;
  for (i = 0; i < this->hyp_count; i++)
  {
    hyp = this->hyps + i;

    if (hyp->weight > max_weight)
    {
      max_weight = hyp->weight;
      pose = hyp->pf_pose_mean;
    }
  }

  this->Unlock();

  if (max_weight < 0.0)
    return;

  // Shift the robot figure
  rtk_fig_origin(this->robot_fig, pose.v[0], pose.v[1], pose.v[2]);

  return;
}

#endif