amcl.cc

机器人仿真平台,和stage配合运行

  data.pending_time_sec = htonl(data.pending_time_sec);
  data.pending_time_usec = htonl(data.pending_time_usec);

  // Byte-swap
  for (i = 0; i < data.hypoth_count; i++)
  {
    for (j = 0; j < 3; j++)
    {
      data.hypoths[i].mean[j] = htonl(data.hypoths[i].mean[j]);
      for (k = 0; k < 3; k++)
        data.hypoths[i].cov[j][k] = htonll(data.hypoths[i].cov[j][k]);
    }
    data.hypoths[i].alpha = htonl(data.hypoths[i].alpha);
  }
  data.hypoth_count = htonl(data.hypoth_count);

  // Copy data to server
  assert(len >= datalen);
  memcpy(dest, &data, datalen);

  // Set the timestamp
  if (time_sec)
    *time_sec = sdata.odom_time_sec;
  if (time_usec)
    *time_usec = sdata.odom_time_usec;

  return datalen;
}


////////////////////////////////////////////////////////////////////////////////
// Push data onto the filter queue.
void AdaptiveMCL::Push(amcl_sensor_data_t *data)
{
  int i;
  
  if (this->q_len >= this->q_size)
  {
    PLAYER_ERROR("queue overflow");
    return;
  }

  i = (this->q_start + this->q_len++) % this->q_size;
  this->q_data[i] = *data;
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Pop data from the filter queue
int AdaptiveMCL::Pop(amcl_sensor_data_t *data)
{
  int i;
  
  if (this->q_len == 0)
    return 0;

  PLAYER_TRACE1("q len %d\n", this->q_len);
  
  i = this->q_start++ % this->q_size;
  this->q_len--;
  *data = this->q_data[i];
  
  return 1;
}


////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void AdaptiveMCL::Main(void) 
{  
  struct timespec sleeptime;
  amcl_sensor_data_t data;
  
  // WARNING: this only works for Linux
  // Run at a lower priority
  nice(10);

  // Initialize the filter
  this->InitFilter(this->pf_init_pose_mean, this->pf_init_pose_cov);
  
  while (true)
  {
#ifdef INCLUDE_RTKGUI
    if (this->enable_gui)
    {
      rtk_canvas_render(this->canvas);
      rtk_app_main_loop(this->app);
    }
#endif

    // Sleep for 1ms (will actually take longer than this).
    sleeptime.tv_sec = 0;
    sleeptime.tv_nsec = 1000000L;
    nanosleep(&sleeptime, NULL);

    // Test if we are supposed to cancel this thread.
    pthread_testcancel();

    // Process any pending requests.
    this->HandleRequests();

    // Process any queued data
    if (this->Pop(&data))
      this->UpdateFilter(&data);
  }

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Initialize the filter
void AdaptiveMCL::InitFilter(pf_vector_t pose_mean, pf_matrix_t pose_cov)
{
  int i;
  double weight;
  amcl_hyp_t *hyp;
  
  // Initialize the odometric model
  odometry_init_init(this->odom_model, pose_mean, pose_cov);
  
  // Draw samples from the odometric distribution
  pf_init(this->pf, (pf_init_model_fn_t) odometry_init_model, this->odom_model);
  
  odometry_init_term(this->odom_model);

  this->Lock();

  // Get the hypotheses
  this->hyp_count = 0;
  for (i = 0; i < sizeof(this->hyps) / sizeof(this->hyps[0]); i++)
  {
    if (!pf_get_cluster_stats(this->pf, i, &weight, &pose_mean, &pose_cov))
      break;

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

  this->Unlock();
  
#ifdef INCLUDE_RTKGUI
  // Draw the samples
  if (this->enable_gui)
  {
    DrawPoseEst();
    
    rtk_fig_clear(this->pf_fig);
    rtk_fig_color(this->pf_fig, 0, 0, 1);
    pf_draw_samples(this->pf, this->pf_fig, 1000);
    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);
  }
#endif

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Update the filter with new sensor data
void AdaptiveMCL::UpdateFilter(amcl_sensor_data_t *data)
{
  int i;
  double weight;
  pf_vector_t pose_mean;
  pf_matrix_t pose_cov;
  amcl_hyp_t *hyp;

  // Update the odometry sensor model with the latest odometry measurements
  odometry_action_init(this->odom_model, this->pf_odom_pose, data->odom_pose);
  odometry_sensor_init(this->odom_model);

  // Apply the odometry action model
  pf_update_action(this->pf, (pf_action_model_fn_t) odometry_action_model, this->odom_model);

  // Apply the odometry sensor model
  pf_update_sensor(this->pf, (pf_sensor_model_fn_t) odometry_sensor_model, this->odom_model);

  odometry_sensor_term(this->odom_model);
  odometry_action_term(this->odom_model);

  // Update the sonar sensor model with the latest sonar measurements
  sonar_clear_ranges(this->sonar_model);
  for (i = 0; i < data->srange_count; i++)
    sonar_add_range(this->sonar_model, data->sranges[i]);

  // Apply the sonar sensor model
  pf_update_sensor(this->pf, (pf_sensor_model_fn_t) sonar_sensor_model, this->sonar_model);  
  
  // Update the laser sensor model with the latest laser measurements
  laser_clear_ranges(this->laser_model);
  for (i = 0; i < data->range_count; i += data->range_count / this->laser_max_samples)
    laser_add_range(this->laser_model, data->ranges[i][0], data->ranges[i][1]);

  // Apply the laser sensor model
  pf_update_sensor(this->pf, (pf_sensor_model_fn_t) laser_sensor_model, this->laser_model);  

  // Resample
  pf_update_resample(this->pf);
  
  this->Lock();

  this->pf_odom_pose = data->odom_pose;
  this->pf_odom_time_sec = data->odom_time_sec;
  this->pf_odom_time_usec = data->odom_time_usec;

  // Get the hypotheses
  this->hyp_count = 0;
  for (i = 0; i < sizeof(this->hyps) / sizeof(this->hyps[0]); i++)
  {
    if (!pf_get_cluster_stats(this->pf, i, &weight, &pose_mean, &pose_cov))
      break;

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

  this->Unlock();


#ifdef INCLUDE_RTKGUI
  // Draw the samples
  if (this->enable_gui)
  {
    DrawPoseEst();
    DrawSonarData(data);
    
    rtk_fig_clear(this->pf_fig);
    rtk_fig_color(this->pf_fig, 0, 0, 1);
    pf_draw_samples(this->pf, this->pf_fig, 1000);
    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);
  }
#endif

  return;
}


#ifdef INCLUDE_RTKGUI

////////////////////////////////////////////////////////////////////////////////
// Draw the current best pose estimate
void AdaptiveMCL::DrawPoseEst()
{
  int i;
  amcl_hyp_t *hyp;
  
  this->Lock();

  for (i = 0; i < this->hyp_count; i++)
  {
    hyp = this->hyps + i;
    rtk_fig_origin(this->robot_fig, hyp->pf_pose_mean.v[0],
                   hyp->pf_pose_mean.v[1], hyp->pf_pose_mean.v[2]);
  }
  
  this->Unlock();
  
  return;
}

////////////////////////////////////////////////////////////////////////////////
// Draw the sonar values
void AdaptiveMCL::DrawSonarData(amcl_sensor_data_t *data)
{
  int i;
  double r, b, ax, ay, bx, by;
  
  rtk_fig_clear(this->sonar_fig);
  rtk_fig_color_rgb32(this->sonar_fig, 0xC0C080);
  
  for (i = 0; i < data->srange_count; i++)
  {
    r = data->sranges[i];
    b = this->sonar_poses[i].v[2];

    ax = this->sonar_poses[i].v[0];
    ay = this->sonar_poses[i].v[1];

    bx = ax + r * cos(b);
    by = ay + r * sin(b);
    
    rtk_fig_line(this->sonar_fig, ax, ay, bx, by);
  }
  return;
}

#endif


////////////////////////////////////////////////////////////////////////////////
// Process requests.  Returns 1 if the configuration has changed.
int AdaptiveMCL::HandleRequests(void)
{
  int len;
  void *client;
  char request[PLAYER_MAX_REQREP_SIZE];
  
  while ((len = GetConfig(&client, &request, sizeof(request))) > 0)
  {
    switch (request[0])
    {
      case PLAYER_LOCALIZE_SET_POSE_REQ:
        HandleSetPose(client, request, len);
        break;
      case PLAYER_LOCALIZE_GET_MAP_INFO_REQ:
        HandleGetMapInfo(client, request, len);
        break;
      case PLAYER_LOCALIZE_GET_MAP_DATA_REQ:
        HandleGetMapData(client, request, len);
        break;
      default:
        if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
          PLAYER_ERROR("PutReply() failed");
        break;
    }
  }
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Handle the set pose request
void AdaptiveMCL::HandleSetPose(void *client, void *request, int len)
{
  size_t reqlen;
  player_localize_set_pose_t req;
  pf_vector_t pose;
  pf_matrix_t cov;

  // Expected length of request
  reqlen = sizeof(req);
  
  // check if the config request is valid
  if (len != reqlen)
  {
    PLAYER_ERROR2("config request len is invalid (%d != %d)", len, reqlen);
    if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
    return;
  }

  req = *((player_localize_set_pose_t*) request);

  pose.v[0] = ((int32_t) ntohl(req.mean[0])) / 1000.0;
  pose.v[1] = ((int32_t) ntohl(req.mean[1])) / 1000.0;
  pose.v[2] = ((int32_t) ntohl(req.mean[2])) / 3600.0 * M_PI / 180;
    
  cov = pf_matrix_zero();
  cov.m[0][0] = ((int64_t) ntohll(req.cov[0][0])) / 1e6;
  cov.m[0][1] = ((int64_t) ntohll(req.cov[0][1])) / 1e6;
  cov.m[1][0] = ((int64_t) ntohll(req.cov[1][0])) / 1e6;
  cov.m[1][1] = ((int64_t) ntohll(req.cov[1][1])) / 1e6;
  cov.m[2][2] = ((int64_t) ntohll(req.cov[2][2])) / (3600.0 * 3600.0) * (M_PI / 180 * M_PI / 180);

  // Initialize the filter
  this->InitFilter(pose, cov);

  // Give them an ack
  if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0) != 0)
    PLAYER_ERROR("PutReply() failed");

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Handle map info request
void AdaptiveMCL::HandleGetMapInfo(void *client, void *request, int len)
{
  size_t reqlen;
  player_localize_map_info_t info;

  // Expected length of request
  reqlen = sizeof(info.subtype);
  
  // check if the config request is valid
  if (len != reqlen)
  {
    PLAYER_ERROR2("config request len is invalid (%d != %d)", len, reqlen);
    if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
    return;
  }

  PLAYER_TRACE4("%d %d %f %d\n", this->map->size_x, this->map->size_y,
                this->map->scale, ntohl(info.scale));
  
  info.scale = htonl((int32_t) (1000.0 / this->map->scale + 0.5));
  info.width = htonl((int32_t) (this->map->size_x));
  info.height = htonl((int32_t) (this->map->size_y));

  // Send map info to the client
  if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &info, sizeof(info)) != 0)
    PLAYER_ERROR("PutReply() failed");
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Handle map data request
void AdaptiveMCL::HandleGetMapData(void *client, void *request, int len)
{
  int i, j;
  int oi, oj, si, sj;
  size_t reqlen;
  map_cell_t *cell;
  player_localize_map_data_t data;

  // Expected length of request
  reqlen = sizeof(data) - sizeof(data.data);

  // check if the config request is valid
  if (len != reqlen)
  {
    PLAYER_ERROR2("config request len is invalid (%d != %d)", len, reqlen);
    if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
    return;
  }

  // Construct reply
  memcpy(&data, request, len);

  oi = ntohl(data.col);
  oj = ntohl(data.row);
  si = ntohl(data.width);
  sj = ntohl(data.height);

  PLAYER_TRACE4("%d %d %d %d\n", oi, oj, si, sj);

  // Grab the pixels from the map
  for (j = 0; j < sj; j++)
  {
    for (i = 0; i < si; i++)
    {
      if (MAP_VALID(this->map, i + oi, j + oj))
      {
        cell = this->map->cells + MAP_INDEX(this->map, i + oi, j + oj);
        data.data[i + j * si] = cell->occ_state;
      }
      else
        data.data[i + j * si] = 0;
    }
  }
    
  // Send map info to the client
  if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &data, sizeof(data)) != 0)
    PLAYER_ERROR("PutReply() failed");
  
  return;
}