amcl.cc

机器人仿真软件

  this->pf_init_pose_cov.m[1][1] = u[1] * u[1];
  this->pf_init_pose_cov.m[2][2] = u[2] * u[2];

  // Update distances
  this->min_dr = cf->ReadTupleLength(section, "update_thresh", 0, 0.20);
  this->min_da = cf->ReadTupleAngle(section, "update_thresh", 1, M_PI/6);

  // Initial hypothesis list
  this->hyp_count = 0;
  pthread_mutex_init(&this->best_hyp_lock,NULL);

#ifdef INCLUDE_RTKGUI
  // Enable debug gui
  this->enable_gui = cf->ReadInt(section, "enable_gui", 0);
#endif


  return;
}


////////////////////////////////////////////////////////////////////////////////
// Destructor
AdaptiveMCL::~AdaptiveMCL(void)
{
  int i;

  // Delete sensor data queue
  delete[] this->q_data;

  // Delete sensors
  for (i = 0; i < this->sensor_count; i++)
  {
    this->sensors[i]->Unload();
    delete this->sensors[i];
  }
  this->sensor_count = 0;
  pthread_mutex_destroy(&this->best_hyp_lock);

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int AdaptiveMCL::Setup(void)
{
  PLAYER_MSG0(2, "setup");

  // Create the particle filter
  assert(this->pf == NULL);
  this->pf = pf_alloc(this->pf_min_samples, this->pf_max_samples);
  this->pf->pop_err = this->pf_err;
  this->pf->pop_z = this->pf_z;

  // Start sensors
  for (int i = 0; i < this->sensor_count; i++)
    if (this->sensors[i]->Setup() < 0)
    {
      PLAYER_ERROR1 ("failed to setup sensor %d", i);
      return -1;
    }


  // Start the driver thread.
  PLAYER_MSG0(2, "running");
  this->StartThread();

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int AdaptiveMCL::Shutdown(void)
{
  int i;

  PLAYER_MSG0(2, "shutting down");

  // Stop the driver thread.
  this->StopThread();

  // Stop sensors
  for (i = 0; i < this->sensor_count; i++)
    this->sensors[i]->Shutdown();

  // Delete the particle filter
  pf_free(this->pf);
  this->pf = NULL;

  PLAYER_MSG0(2, "shutdown done");
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Check for updated sensor data
void AdaptiveMCL::UpdateSensorData(void)
{
/*
  int i;
  AMCLSensorData *data;
  pf_vector_t pose, delta;

  assert(this->action_sensor >= 0 && this->action_sensor < this->sensor_count);

  // Check the sensors for new data
  for (i = 0; i < this->sensor_count; i++)
  {
    data = this->sensors[i]->GetData();
    if (data != NULL)
    {
      this->Push(data);
      if(this->sensors[i]->is_action)
      {
        if (this->pf_init)
        {
          // HACK: Assume that the action sensor is odometry
          pose = ((AMCLOdomData*)data)->pose;

          // Compute change in pose
          delta = pf_vector_coord_sub(pose, this->pf_odom_pose);

          // Publish new pose estimate
          this->PutDataPosition(delta);
        }
      }
    }
  }
  return;*/
}


////////////////////////////////////////////////////////////////////////////////
// Push data onto the filter queue.
void AdaptiveMCL::Push(AMCLSensorData *data)
{
  int i;

  this->Lock();

  if (this->q_len >= this->q_size)
  {
    this->Unlock();
    PLAYER_ERROR("queue overflow");
    return;
  }
  i = (this->q_start + this->q_len++) % this->q_size;

  this->q_data[i] = data;

  this->Unlock();
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Take a peek at the queue
AMCLSensorData *AdaptiveMCL::Peek(void)
{
  int i;

  this->Lock();

  if (this->q_len == 0)
  {
    this->Unlock();
    return NULL;
  }
  i = this->q_start % this->q_size;

  this->Unlock();
  return this->q_data[i];
}


////////////////////////////////////////////////////////////////////////////////
// Pop data from the filter queue
AMCLSensorData *AdaptiveMCL::Pop(void)
{
  int i;

  this->Lock();

  if (this->q_len == 0)
  {
    this->Unlock();
    return NULL;
  }
  i = this->q_start++ % this->q_size;
  this->q_len--;

  this->Unlock();
  return this->q_data[i];
}


////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void AdaptiveMCL::Main(void)
{
  this->q_len = 0;

  // No data has yet been pushed, and the
  // filter has not yet been initialized
  this->pf_init = false;

  // Initial hypothesis list
  this->hyp_count = 0;

  // WARNING: this only works for Linux
  // Run at a lower priority
  nice(10);

#ifdef INCLUDE_RTKGUI
  pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);

  // Start the GUI
  if (this->enable_gui)
    this->SetupGUI();
#endif

#ifdef INCLUDE_OUTFILE
  // Open file for logging results
  this->outfile = fopen("amcl.out", "w+");
#endif

  while (true)
  {
#ifdef INCLUDE_RTKGUI
    if (this->enable_gui)
    {
      rtk_canvas_render(this->canvas);
      rtk_app_main_loop(this->app);
    }
    pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
#endif

    // Sleep for 1ms (will actually take longer than this).
    const struct timespec sleeptime = {0, 1000000L};
    nanosleep(&sleeptime, NULL);

    // Test if we are supposed to cancel this thread.  This is the
    // only place we can cancel from if we are running the GUI.
    pthread_testcancel();

    // Process any pending messages
    this->ProcessMessages();
    //UpdateSensorData();

#ifdef INCLUDE_RTKGUI
    pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
#endif

    // Initialize the filter if we havent already done so
    if (!this->pf_init)
      this->InitFilter();

    // Update the filter
    if (this->UpdateFilter())
    {
#ifdef INCLUDE_OUTFILE
      // Save the error values
      pf_vector_t mean;
      double var;

      pf_get_cep_stats(pf, &mean, &var);

      printf("amcl %.3f %.3f %f\n", mean.v[0], mean.v[1], mean.v[2] * 180 / M_PI);

      fprintf(this->outfile, "%d.%03d unknown 6665 localize 01 %d.%03d ",
              0, 0, 0, 0);
      fprintf(this->outfile, "1.0 %e %e %e %e 0 0 0 0 0 0 0 0 \n",
              mean.v[0], mean.v[1], mean.v[2], var);
      fflush(this->outfile);
#endif
    }
  }
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Thread finalization
void AdaptiveMCL::MainQuit()
{
#ifdef INCLUDE_RTKGUI
  // Stop the GUI
  if (this->enable_gui)
    this->ShutdownGUI();
#endif

#ifdef INCLUDE_OUTFILE
  // Close the log file
  fclose(this->outfile);
#endif

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Initialize the filter
void AdaptiveMCL::InitFilter(void)
{
  ::pf_init(this->pf, this->pf_init_pose_mean, this->pf_init_pose_cov);

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Update the filter with new sensor data
bool AdaptiveMCL::UpdateFilter(void)
{
  int i;
  double ts;
  double weight;
  pf_vector_t pose, delta;
  pf_vector_t pose_mean;
  pf_matrix_t pose_cov;
  amcl_hyp_t *hyp;
  AMCLSensorData *data;
  bool update;

  //printf("q len %d\n", this->q_len);

  // Get the action data
  data = this->Pop();
  if (data == NULL)
    return false;
  if (!data->sensor->is_action)
  {
    delete data;
    return false;
  }

  // Use the action timestamp
  ts = data->time;

  // HACK
  pose = ((AMCLOdomData*) data)->pose;
  delta = pf_vector_zero();
  update = false;

  //printf("odom pose\n");
  //pf_vector_fprintf(pose, stdout, "%.3f");

  // Compute change in pose
  if (this->pf_init)
  {
    // Compute change in pose
    delta = pf_vector_coord_sub(pose, this->pf_odom_pose);

    // See if we should update the filter
    update = fabs(delta.v[0]) > this->min_dr ||
      fabs(delta.v[1]) > this->min_dr ||
      fabs(delta.v[2]) > this->min_da;
  }

  // If the filter has not been initialized
  if (!this->pf_init)
  {
    // Discard this action data
    delete data; data = NULL;

    // Pose at last filter update
    this->pf_odom_pose = pose;

    // Filter is now initialized
    this->pf_init = true;

    // Should update sensor data
    update = true;

    //printf("init\n");
    //pf_vector_fprintf(pose, stdout, "%.3f");
  }

  // If the robot has moved, update the filter
  else if (this->pf_init && update)
  {
    //printf("pose\n");
    //pf_vector_fprintf(pose, stdout, "%.3f");

    // HACK
    // Modify the delta in the action data so the filter gets
    // updated correctly
    ((AMCLOdomData*) data)->delta = delta;

    // Use the action data to update the filter
    data->sensor->UpdateAction(this->pf, data);
    delete data; data = NULL;

    // Pose at last filter update
    //this->pf_odom_pose = pose;
  }
  else
  {
    // Discard this action data
    delete data; data = NULL;
  }

  bool processed_first_sensor = false;
  // If the robot has moved, update the filter
  if (update)
  {
    // Process the remaining sensor data up to the next action data
    while (1)
    {
      data = this->Peek();
      if ((data == NULL ) || (data->sensor->is_action))
      {
        // HACK: Discard action data until we've processed at least one
        // sensor reading
        if(!processed_first_sensor)
        {
          if(data)
          {
            data = this->Pop();
            assert(data);
            delete data;
          }
          // avoid a busy loop while waiting for a sensor reading to
          // process
          //return false;
          usleep(1000);
          ProcessMessages();
          UpdateSensorData();
          continue;
        }
        else
          break;
      }
      data = this->Pop();
      assert(data);

      // Use the data to update the filter
      // HACK: only use the first sensor reading
      if(!processed_first_sensor)
      {
        data->sensor->UpdateSensor(this->pf, data);
        processed_first_sensor = true;
        this->pf_odom_pose = pose;
      }

#ifdef INCLUDE_RTKGUI
      // Draw the current sensor data
      if (this->enable_gui)
        data->sensor->UpdateGUI(this->canvas, this->robot_fig, data);
#endif

      // Discard data
      delete data; data = NULL;