gmcl_odom.h

Go to the documentation of this file.

//this package is based on amcl and has been modified to fit gmcl 
/* 
 * Author: Mhd Ali Alshikh Khalil
 * Date: 20 June 2021
 * 
*/
 
//amcl author clarification
/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey et al.
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library 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
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
//
// Desc: Odometry sensor model for AMCL
// Author: Andrew Howard
// Date: 17 Aug 2003
// CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $
//
 
#ifndef AMCL_ODOM_H
#define AMCL_ODOM_H
 
#include "gmcl_sensor.h"
#include "../pf/pf_pdf.h"
 
namespace gmcl
{
 
typedef enum
{
  ODOM_MODEL_DIFF,
  ODOM_MODEL_OMNI,
  ODOM_MODEL_DIFF_CORRECTED,
  ODOM_MODEL_OMNI_CORRECTED
} odom_model_t;
 
 
 
// Odometric sensor data
class GMCLOdomData : public GMCLSensorData
{
 
  // Odometric pose
  public: pf_vector_t pose;
 
  // Change in odometric pose
  public: pf_vector_t delta;
};
// Odometric sensor model
class GMCLOdom : public GMCLSensor
{
  // Default constructor
  public: GMCLOdom();
 
  public: void SetModelDiff(double alpha1, 
                            double alpha2, 
                            double alpha3, 
                            double alpha4);
 
  public: void SetModelOmni(double alpha1, 
                            double alpha2, 
                            double alpha3, 
                            double alpha4,
                            double alpha5);
 
  public: void SetModel( odom_model_t type,
                         double alpha1,
                         double alpha2,
                         double alpha3,
                         double alpha4,
                         double alpha5 = 0 );
 
  // Update the filter based on the action model.  Returns true if the filter
  // has been updated.
  public: virtual bool UpdateAction(pf_t *pf, GMCLSensorData *data);
 
  // Current data timestamp
  private: double time;
  
  // Model type
  private: odom_model_t model_type;
 
  // Drift parameters
  private: double alpha1, alpha2, alpha3, alpha4, alpha5;
  
  private: static void OmniModel(GMCLOdomData *data,pf_sample_set_t* set);
  private: static void DiffModel(GMCLOdomData *data,pf_sample_set_t* set);
  private: static void OmniCorrectedModel(GMCLOdomData *data,pf_sample_set_t* set);
  private: static void DiffCorrectedModel(GMCLOdomData *data,pf_sample_set_t* set);
};
 
 
 
 
 
}
 
#endif