amcl.cc

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

////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int AdaptiveMCL::Shutdown(void)
{
  // Stop the driver thread.
  this->StopThread();
    
#ifdef INCLUDE_RTKGUI
  // Stop the GUI
  if (this->enable_gui)
    this->ShutdownGUI();
#endif

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

  // Delete the odometry model
  odometry_free(this->odom_model);
  this->odom_model = NULL;

  // Delete the sonar model
  sonar_free(this->sonar_model);
  this->sonar_model = NULL;

  // Delete the laser model
  laser_free(this->laser_model);
  this->laser_model = NULL;
  
  // Delete the map
  map_free(this->map);
  this->map = NULL;
  
  // Stop the laser
  this->ShutdownLaser();

  // Stop the sonar
  this->ShutdownSonar();

  // Stop the odom device
  this->ShutdownOdom();

  PLAYER_TRACE0("shutdown");
  return 0;
}


#ifdef INCLUDE_RTKGUI

////////////////////////////////////////////////////////////////////////////////
// Set up the GUI
int AdaptiveMCL::SetupGUI(void)
{
  // Initialize RTK
  rtk_init(NULL, NULL);

  this->app = rtk_app_create();

  this->canvas = rtk_canvas_create(this->app);
  rtk_canvas_title(this->canvas, "AdaptiveMCL");
  rtk_canvas_size(this->canvas, this->map->size_x, this->map->size_y);
  rtk_canvas_scale(this->canvas, this->map->scale, this->map->scale);

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

  // Draw the map
  map_draw_occ(this->map, this->map_fig);
  //map_draw_cspace(this->map, this->map_fig);

  this->robot_fig = rtk_fig_create(this->canvas, NULL, 9);
  this->sonar_fig = rtk_fig_create(this->canvas, this->robot_fig, 10);

  // 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);

  rtk_app_main_init(this->app);

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shut down the GUI
int AdaptiveMCL::ShutdownGUI(void)
{
  rtk_fig_destroy(this->sonar_fig);
  rtk_fig_destroy(this->robot_fig);
  rtk_fig_destroy(this->map_fig);
  rtk_fig_destroy(this->pf_fig);
  rtk_canvas_destroy(this->canvas);
  rtk_app_main_term(this->app);
  rtk_app_destroy(this->app);
  
  return 0;
}
  
#endif


////////////////////////////////////////////////////////////////////////////////
// Set up the underlying odom device.
int AdaptiveMCL::SetupOdom(void)
{
  player_device_id_t id;
    
  id.code = PLAYER_POSITION_CODE;
  id.index = this->odom_index;

  this->odom = deviceTable->GetDevice(id);
  if (!this->odom)
  {
    PLAYER_ERROR("unable to locate suitable position device");
    return -1;
  }
  
  if (this->odom->Subscribe(this) != 0)
  {
    PLAYER_ERROR("unable to subscribe to position device");
    return -1;
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the underlying odom device.
int AdaptiveMCL::ShutdownOdom(void)
{
  this->odom->Unsubscribe(this);
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Get the current odometry reading
void AdaptiveMCL::GetOdomData(amcl_sensor_data_t *data)
{
  size_t size;
  player_position_data_t ndata;

  // Get the odom device data.
  size = this->odom->GetData(this, (uint8_t*) &ndata, sizeof(ndata),
                             &data->odom_time_sec, &data->odom_time_usec);

  // Byte swap
  ndata.xpos = ntohl(ndata.xpos);
  ndata.ypos = ntohl(ndata.ypos);
  ndata.yaw = ntohl(ndata.yaw);

  data->odom_pose.v[0] = (double) ((int32_t) ndata.xpos) / 1000.0;
  data->odom_pose.v[1] = (double) ((int32_t) ndata.ypos) / 1000.0;
  data->odom_pose.v[2] = (double) ((int32_t) ndata.yaw) * M_PI / 180;

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the sonar
int AdaptiveMCL::SetupSonar(void)
{
  int i;
  uint8_t req;
  uint16_t reptype;
  player_device_id_t id;
  player_sonar_geom_t geom;
  struct timeval tv;

  // If there is no sonar device...
  if (this->sonar_index < 0)
    return 0;
  
  id.code = PLAYER_SONAR_CODE;
  id.index = this->sonar_index;

  this->sonar = deviceTable->GetDevice(id);
  if (!this->sonar)
  {
    PLAYER_ERROR("unable to locate suitable sonar device");
    return -1;
  }
  if (this->sonar->Subscribe(this) != 0)
  {
    PLAYER_ERROR("unable to subscribe to sonar device");
    return -1;
  }

  // Get the sonar geometry
  req = PLAYER_SONAR_GET_GEOM_REQ;
  if (this->sonar->Request(&id, this, &req, 1, &reptype, &tv, &geom, sizeof(geom)) < 0)
  {
    PLAYER_ERROR("unable to get sonar geometry");
    return -1;
  }

  this->sonar_pose_count = (int16_t) ntohs(geom.pose_count);
  assert(this->sonar_pose_count < sizeof(this->sonar_poses) / sizeof(this->sonar_poses[0]));
  
  for (i = 0; i < this->sonar_pose_count; i++)
  {
    this->sonar_poses[i].v[0] = ((int16_t) ntohs(geom.poses[i][0])) / 1000.0;
    this->sonar_poses[i].v[1] = ((int16_t) ntohs(geom.poses[i][1])) / 1000.0;
    this->sonar_poses[i].v[2] = ((int16_t) ntohs(geom.poses[i][2])) * M_PI / 180.0;
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shut down the sonar
int AdaptiveMCL::ShutdownSonar(void)
{
  // If there is no sonar device...
  if (this->sonar_index < 0)
    return 0;

  this->sonar->Unsubscribe(this);
  this->sonar = NULL;
  
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Check for new sonar data
void AdaptiveMCL::GetSonarData(amcl_sensor_data_t *data)
{
  int i;
  size_t size;
  player_sonar_data_t ndata;
  double r; //b, db;

  // If there is no sonar device...
  if (this->sonar_index < 0)
  {
    data->srange_count = 0;
    return;
  }
  
  // Get the sonar device data.
  size = this->sonar->GetData(this, (uint8_t*) &ndata, sizeof(ndata), NULL, NULL);

  data->srange_count = ntohs(ndata.range_count);
  assert(data->srange_count < sizeof(data->sranges) / sizeof(data->sranges[0]));

  // Read and byteswap the range data
  for (i = 0; i < data->srange_count; i++)
  {
    r = ((int16_t) ntohs(ndata.ranges[i])) / 1000.0;
    data->sranges[i] = r;
  }

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the laser
int AdaptiveMCL::SetupLaser(void)
{
  uint8_t req;
  uint16_t reptype;
  player_device_id_t id;
  player_laser_geom_t geom;
  struct timeval tv;

  // If there is no laser device...
  if (this->laser_index < 0)
    return 0;

  id.code = PLAYER_LASER_CODE;
  id.index = this->laser_index;

  this->laser = deviceTable->GetDevice(id);
  if (!this->laser)
  {
    PLAYER_ERROR("unable to locate suitable laser device");
    return -1;
  }
  if (this->laser->Subscribe(this) != 0)
  {
    PLAYER_ERROR("unable to subscribe to laser device");
    return -1;
  }

  // Get the laser geometry
  req = PLAYER_LASER_GET_GEOM;
  if (this->laser->Request(&id, this, &req, 1, &reptype, &tv, &geom, sizeof(geom)) < 0)
  {
    PLAYER_ERROR("unable to get laser geometry");
    return -1;
  }

  this->laser_pose.v[0] = ((int16_t) ntohl(geom.pose[0])) / 1000.0;
  this->laser_pose.v[1] = ((int16_t) ntohl(geom.pose[1])) / 1000.0;
  this->laser_pose.v[2] = ((int16_t) ntohl(geom.pose[2])) * M_PI / 180.0;

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shut down the laser
int AdaptiveMCL::ShutdownLaser(void)
{
  // If there is no laser device...
  if (this->laser_index < 0)
    return 0;
  
  this->laser->Unsubscribe(this);
  this->laser = NULL;
  
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Check for new laser data
void AdaptiveMCL::GetLaserData(amcl_sensor_data_t *data)
{
  int i;
  size_t size;
  player_laser_data_t ndata;
  double r, b, db;
  
  // If there is no laser device...
  if (this->laser_index < 0)
  {
    data->range_count = 0;
    return;
  }

  // Get the laser device data.
  size = this->laser->GetData(this, (uint8_t*) &ndata, sizeof(ndata), NULL, NULL);

  b = ((int16_t) ntohs(ndata.min_angle)) / 100.0 * M_PI / 180.0;
  db = ((int16_t) ntohs(ndata.resolution)) / 100.0 * M_PI / 180.0;

  data->range_count = ntohs(ndata.range_count);
  assert(data->range_count < sizeof(data->ranges) / sizeof(data->ranges[0]));

  // Read and byteswap the range data
  for (i = 0; i < data->range_count; i++)
  {
    r = ((int16_t) ntohs(ndata.ranges[i])) / 1000.0;
    data->ranges[i][0] = r;
    data->ranges[i][1] = b;
    b += db;
  }
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Get the current pose.  This function is called by the server thread.
size_t AdaptiveMCL::GetData(void* client, unsigned char* dest, size_t len,
                            uint32_t* time_sec, uint32_t* time_usec)
{
  int i, j, k;
  int datalen;
  player_localize_data_t data;
  pf_vector_t odom_pose, odom_diff;
  pf_vector_t pose;
  pf_matrix_t pose_cov;
  amcl_sensor_data_t sdata;
  amcl_hyp_t *hyp;
  double scale[3];
  
  this->Lock();

  // See if there is new odometry data.  If there is, push it and all
  // the rest of the sensor data onto the sensor queue.
  this->GetOdomData(&sdata);

  // See how far the robot has moved
  odom_pose = sdata.odom_pose;
  odom_diff = pf_vector_coord_sub(odom_pose, this->odom_pose);

  // Make sure we have moved a reasonable distance
  if (fabs(odom_diff.v[0]) > 0.20 ||
      fabs(odom_diff.v[1]) > 0.20 ||
      fabs(odom_diff.v[2]) > M_PI / 6)
  {
    this->odom_pose = sdata.odom_pose;

    // Get the current sonar data; we assume it is new data
    this->GetSonarData(&sdata);

    // Get the current laser data; we assume it is new data
    this->GetLaserData(&sdata);

    // Push the data
    this->Push(&sdata);
  }
  
  // Compute the change in odometric pose
  odom_diff = pf_vector_coord_sub(odom_pose, this->pf_odom_pose);

  // Record the number of pending observations
  data.pending_count = this->q_len;
  data.pending_time_sec = this->pf_odom_time_sec;
  data.pending_time_usec = this->pf_odom_time_usec;
  
  // Encode the hypotheses
  data.hypoth_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;

    // Translate/rotate the hypotheses to take account of latency in filter
    pose = pf_vector_coord_add(odom_diff, pose);

    // 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);
    }

    scale[0] = 1000;
    scale[1] = 1000;
    scale[2] = 3600 * 180 / M_PI;
    
    data.hypoths[i].alpha = (uint32_t) (hyp->weight * 1e6);
        
    data.hypoths[i].mean[0] = (int32_t) (pose.v[0] * scale[0]);
    data.hypoths[i].mean[1] = (int32_t) (pose.v[1] * scale[1]);
    data.hypoths[i].mean[2] = (int32_t) (pose.v[2] * scale[2]);
  
    data.hypoths[i].cov[0][0] = (int64_t) (pose_cov.m[0][0] * scale[0] * scale[0]);
    data.hypoths[i].cov[0][1] = (int64_t) (pose_cov.m[0][1] * scale[1] * scale[1]);
    data.hypoths[i].cov[0][2] = 0;
  
    data.hypoths[i].cov[1][0] = (int64_t) (pose_cov.m[1][0] * scale[0] * scale[0]);
    data.hypoths[i].cov[1][1] = (int64_t) (pose_cov.m[1][1] * scale[1] * scale[1]);
    data.hypoths[i].cov[1][2] = 0;

    data.hypoths[i].cov[2][0] = 0;
    data.hypoths[i].cov[2][1] = 0;
    data.hypoths[i].cov[2][2] = (int64_t) (pose_cov.m[2][2] * scale[2] * scale[2]);
  }

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

  // Byte-swap
  data.pending_count = htons(data.pending_count);