amcl.cc

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

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      [e-mail]    [e-mail]
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
///////////////////////////////////////////////////////////////////////////
//
// Desc: Adaptive Monte-Carlo localization
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl.cc,v 1.13.2.8 2003/05/23 20:57:24 inspectorg Exp $
//
// Theory of operation:
//  TODO
//
// Requires: position (odometry), laser, sonar
// Provides: localization
//
///////////////////////////////////////////////////////////////////////////

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <assert.h>
#include <errno.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>       // for atoi(3)
#include <sys/types.h>
#include <netinet/in.h>   // for htons(3)
#include <unistd.h>
#include <sys/time.h>

#define PLAYER_ENABLE_TRACE 0
#define PLAYER_ENABLE_MSG 1

#include "player.h"
#include "device.h"
#include "devicetable.h"
#include "drivertable.h"

#include "pf/pf.h"
#include "map/map.h"
#include "models/odometry.h"
#include "models/sonar.h"
#include "models/laser.h"

#ifdef INCLUDE_RTKGUI
#include "rtk.h"
#endif


// Combined sensor data
typedef struct
{
  // Data time-stamp (odometric)
  uint32_t odom_time_sec, odom_time_usec;
    
  // Odometric pose
  pf_vector_t odom_pose;

  // Sonar ranges
  int srange_count;
  double sranges[PLAYER_SONAR_MAX_SAMPLES];

  // Laser ranges
  int range_count;
  double ranges[PLAYER_LASER_MAX_SAMPLES][2];
  
} amcl_sensor_data_t;


// Pose hypothesis
typedef struct
{
  // Total weight (weights sum to 1)
  double weight;

  // Mean of pose esimate
  pf_vector_t pf_pose_mean;

  // Covariance of pose estimate
  pf_matrix_t pf_pose_cov;
  
} amcl_hyp_t;


// Incremental navigation driver
class AdaptiveMCL : public CDevice
{
  ///////////////////////////////////////////////////////////////////////////
  // Top half methods; these methods run in the server thread
  ///////////////////////////////////////////////////////////////////////////

  // Constructor
  public: AdaptiveMCL(char* interface, ConfigFile* cf, int section);

  // Destructor
  public: virtual ~AdaptiveMCL(void);

  // Setup/shutdown routines.
  public: virtual int Setup(void);
  public: virtual int Shutdown(void);

#ifdef INCLUDE_RTKGUI
  // Set up the GUI
  private: int SetupGUI(void);
  private: int ShutdownGUI(void);
#endif

  // Set up the odometry device
  private: int SetupOdom(void);
  private: int ShutdownOdom(void);

  // Get the current odometric pose
  private: void GetOdomData(amcl_sensor_data_t *data);

  // Set up the sonar device
  private: int SetupSonar(void);
  private: int ShutdownSonar(void);

  // Check for new sonar data
  private: void GetSonarData(amcl_sensor_data_t *data);

  // Set up the laser device
  private: int SetupLaser(void);
  private: int ShutdownLaser(void);

  // Check for new laser data
  private: void GetLaserData(amcl_sensor_data_t *data);

  // Get the current pose
  private: virtual size_t GetData(void* client, unsigned char* dest, size_t len,
                                  uint32_t* time_sec, uint32_t* time_usec);

  ///////////////////////////////////////////////////////////////////////////
  // Middle methods: these methods facilitate communication between the top
  // and bottom halfs.
  ///////////////////////////////////////////////////////////////////////////

  // Push data onto the filter queue
  private: void Push(amcl_sensor_data_t *data);

  // Pop data from the filter queue
  private: int Pop(amcl_sensor_data_t *data);

  ///////////////////////////////////////////////////////////////////////////
  // Bottom half methods; these methods run in the device thread
  ///////////////////////////////////////////////////////////////////////////
  
  // Main function for device thread.
  private: virtual void Main(void);

  // Initialize the filter
  private: void InitFilter(pf_vector_t pose_mean, pf_matrix_t pose_cov);

  // Update the filter with new sensor data
  private: void UpdateFilter(amcl_sensor_data_t *data);

#ifdef INCLUDE_RTKGUI
  // Draw the current best pose estimate
  private: void DrawPoseEst();

  // Draw the sonar values
  private: void DrawSonarData(amcl_sensor_data_t *data);
#endif

  // Process requests.  Returns 1 if the configuration has changed.
  private: int HandleRequests(void);

  // Handle geometry requests.
  private: void HandleGetGeom(void *client, void *request, int len);

  // Handle the set pose request
  private: void HandleSetPose(void *client, void *request, int len);

  // Handle map info request
  private: void HandleGetMapInfo(void *client, void *request, int len);

  // Handle map data request
  private: void HandleGetMapData(void *client, void *request, int len);

  ///////////////////////////////////////////////////////////////////////////
  // Properties
  ///////////////////////////////////////////////////////////////////////////

  // Odometry device info
  private: CDevice *odom;
  private: int odom_index;

  // Sonar device info
  private: CDevice *sonar;
  private: int sonar_index;

  // Sonar poses relative to robot
  private: int sonar_pose_count;
  private: pf_vector_t sonar_poses[PLAYER_SONAR_MAX_SAMPLES];
  
  // Laser device info
  private: CDevice *laser;
  private: int laser_index;

  // Laser pose relative to robot
  private: pf_vector_t laser_pose;

  // Effective robot radius (used for c-space tests)
  private: double robot_radius;

  // Occupancy map
  private: const char *map_file;
  private: double map_scale;
  private: int map_negate;
  private: map_t *map;

  // Odometry sensor/action model
  private: odometry_t *odom_model;

  // Sonar sensor model
  private: sonar_t *sonar_model;

  // Laser sensor model
  private: laser_t *laser_model;
  private: int laser_max_samples;
  private: double laser_map_err;

  // Odometric pose of last used sensor reading
  private: pf_vector_t odom_pose;

  // Sensor data queue
  private: int q_size, q_start, q_len;
  private: amcl_sensor_data_t *q_data;
  
  // Particle filter
  private: pf_t *pf;
  private: int pf_min_samples, pf_max_samples;
  private: double pf_err, pf_z;

  // Last odometric pose estimates used by filter
  private: pf_vector_t pf_odom_pose;
  private: uint32_t pf_odom_time_sec, pf_odom_time_usec;

  // Initial pose estimate
  private: pf_vector_t pf_init_pose_mean;
  private: pf_matrix_t pf_init_pose_cov;

  // Current particle filter pose estimates
  private: int hyp_count;
  private: amcl_hyp_t hyps[PLAYER_LOCALIZE_MAX_HYPOTHS];

#ifdef INCLUDE_RTKGUI
  // RTK stuff; for testing only
  private: int enable_gui;
  private: rtk_app_t *app;
  private: rtk_canvas_t *canvas;
  private: rtk_fig_t *map_fig;
  private: rtk_fig_t *pf_fig;
  private: rtk_fig_t *robot_fig;
  private: rtk_fig_t *sonar_fig;
#endif
};


// Initialization function
CDevice* AdaptiveMCL_Init(char* interface, ConfigFile* cf, int section)
{
  if (strcmp(interface, PLAYER_LOCALIZE_STRING) != 0)
  {
    PLAYER_ERROR1("driver \"amcl\" does not support interface \"%s\"\n", interface);
    return (NULL);
  }
  return ((CDevice*) (new AdaptiveMCL(interface, cf, section)));
}


// a driver registration function
void AdaptiveMCL_Register(DriverTable* table)
{
  table->AddDriver("amcl", PLAYER_ALL_MODE, AdaptiveMCL_Init);
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
AdaptiveMCL::AdaptiveMCL(char* interface, ConfigFile* cf, int section)
    : CDevice(sizeof(player_localize_data_t), 0, 100, 100)
{
  //double size;
  double u[3];
  
  this->odom = NULL;
  this->odom_index = cf->ReadInt(section, "position_index", 0);

  this->sonar = NULL;
  this->sonar_index = cf->ReadInt(section, "sonar_index", -1);

  this->laser = NULL;
  this->laser_index = cf->ReadInt(section, "laser_index", -1);

  // C-space info
  this->robot_radius = cf->ReadLength(section, "robot_radius", 0.20);
  
  // Get the map settings
  this->map_file = cf->ReadFilename(section, "map_file", NULL);
  this->map_scale = cf->ReadLength(section, "map_scale", 0.05);
  this->map_negate = cf->ReadInt(section, "map_negate", 0);
  this->map = NULL;
  
  // Odometry model settings
  this->odom_model = NULL;

  // Laser model settings
  this->laser_model = NULL;
  this->laser_max_samples = cf->ReadInt(section, "laser_max_samples", 5);
  this->laser_map_err = cf->ReadLength(section, "laser_map_err", 0.05);

  // Particle filter settings
  this->pf = NULL;
  this->pf_min_samples = cf->ReadInt(section, "pf_min_samples", 100);
  this->pf_max_samples = cf->ReadInt(section, "pf_max_samples", 10000);

  // Adaptive filter parameters
  this->pf_err = cf->ReadFloat(section, "pf_err", 0.01);
  this->pf_z = cf->ReadFloat(section, "pf_z", 3);
  
  // Initial pose estimate
  this->pf_init_pose_mean = pf_vector_zero();
  this->pf_init_pose_mean.v[0] = cf->ReadTupleLength(section, "init_pose", 0, 0);
  this->pf_init_pose_mean.v[1] = cf->ReadTupleLength(section, "init_pose", 1, 0);
  this->pf_init_pose_mean.v[2] = cf->ReadTupleAngle(section, "init_pose", 2, 0);

  // Initial pose covariance
  u[0] = cf->ReadTupleLength(section, "init_pose_var", 0, 1e3);
  u[1] = cf->ReadTupleLength(section, "init_pose_var", 1, 1e3);
  u[2] = cf->ReadTupleAngle(section, "init_pose_var", 2, 1e2);
  this->pf_init_pose_cov = pf_matrix_zero();
  this->pf_init_pose_cov.m[0][0] = u[0] * u[0];
  this->pf_init_pose_cov.m[1][1] = u[1] * u[1];
  this->pf_init_pose_cov.m[2][2] = u[2] * u[2];

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

  // Create the sensor queue
  this->q_len = 0;
  this->q_size = 100;
  this->q_data = new amcl_sensor_data_t[this->q_size];

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


////////////////////////////////////////////////////////////////////////////////
// Destructor
AdaptiveMCL::~AdaptiveMCL(void)
{
  delete[] this->q_data;
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int AdaptiveMCL::Setup(void)
{
  amcl_sensor_data_t sdata;
    
  PLAYER_TRACE0("setup");

  // Initialise the underlying position device
  if (this->SetupOdom() != 0)
    return -1;
  
  // Initialise the sonar
  if (this->SetupSonar() != 0)
    return -1;

  // Initialise the laser
  if (this->SetupLaser() != 0)
    return -1;

  // Create the map
  if (!this->map_file)
  {
    PLAYER_ERROR("map file not specified");
    return -1;
  }
  
  // Create the map
  assert(this->map == NULL);
  this->map = map_alloc(this->map_scale);
  
  // Load the map
  PLAYER_MSG1("loading map file [%s]", this->map_file);
  if (map_load_occ(this->map, this->map_file, this->map_negate) != 0)
    return -1;

  // Compute the c-space
  PLAYER_MSG0("computing cspace");
  map_update_cspace(this->map, 2 * this->robot_radius);
  
  // Create the odometry model
  this->odom_model = odometry_alloc(this->map, this->robot_radius);
  if (odometry_init_cspace(this->odom_model))
  {
    PLAYER_ERROR("error generating free space map (this could be a bad map)");
    return -1;
  }

  // Create the sonar model
  this->sonar_model = sonar_alloc(this->map, this->sonar_pose_count, this->sonar_poses);
  
  // Create the laser model
  this->laser_model = laser_alloc(this->map, this->laser_pose);
  this->laser_model->range_cov = this->laser_map_err * this->laser_map_err;

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

  // Set the initial odometric poses
  this->GetOdomData(&sdata);
  this->odom_pose = sdata.odom_pose;
  this->pf_odom_pose = sdata.odom_pose;
  this->pf_odom_time_sec = sdata.odom_time_sec;
  this->pf_odom_time_usec = sdata.odom_time_usec;

  // Initial hypothesis list
  this->hyp_count = 0;
  
#ifdef INCLUDE_RTKGUI
  // Start the GUI
  if (this->enable_gui)
    this->SetupGUI();
#endif

  // Start the driver thread.
  PLAYER_MSG0("running");
  this->StartThread();
  
  return 0;
}