MotionModel.h

Go to the documentation of this file.

// SVN $HeadURL$
// SVN $Id$

/*
 * 6D localization for humanoid robots
 *
 * Copyright 2009-2012 Armin Hornung, University of Freiburg
 * http://www.ros.org/wiki/humanoid_localization
 *
 *
 * 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, version 3.
 *
 * 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, see <http://www.gnu.org/licenses/>.
 */

#ifndef HUMANOID_LOCALIZATION_MOTIONMODEL_H_
#define HUMANOID_LOCALIZATION_MOTIONMODEL_H_

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <Eigen/Cholesky>

#include <humanoid_localization/humanoid_localization_defs.h>

namespace humanoid_localization{
class MotionModel {
public:
  MotionModel(ros::NodeHandle* nh, EngineT* rngEngine, tf::TransformListener* tf, const std::string& odomFrameId, const std::string& baseFrameId);
  virtual ~MotionModel();
  void reset();
  bool lookupOdomPose(const ros::Time& t, tf::Stamped<tf::Pose>& odomPose) const;

  bool lookupOdomTransform(const ros::Time& t, tf::Transform& odomTransform) const;

  tf::Transform computeOdomTransform(const tf::Transform currentPose) const;

  bool lookupLocalTransform(const std::string& targetFrame, const ros::Time& t, tf::StampedTransform& localTransform) const;

  bool lookupPoseHeight(const ros::Time& t, double& poseHeight) const;

  void applyOdomTransform(Particles& particles, const tf::Transform& odomTransform);

  bool applyOdomTransformTemporal(Particles& particles, const ros::Time& t, double dt);

  void storeOdomPose(const tf::Stamped<tf::Pose>& odomPose);

  bool getLastOdomPose(tf::Stamped<tf::Pose>& lastOdomPose) const;


  // aligns fixed-size members in memory
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:
  void applyOdomTransform(tf::Pose& particlePose, const tf::Transform& odomTransform);

  void transformPose(tf::Pose& particlePose, const tf::Transform& odomTransform);

  tf::Transform odomTransformNoise(const tf::Transform& odomTransform);

  tf::Transform calibrateOdometry(const tf::Transform& odomTransform) const;

  tf::TransformListener* m_tfListener;

  NormalGeneratorT m_rngNormal; // standard normal-distributed noise
  UniformGeneratorT m_rngUniform;
  // parameters:
  Eigen::Matrix3d m_odomNoise2D;
  Eigen::Matrix3d m_odomCalibration2D;
  double m_odomNoiseZ;
  double m_odomNoiseRoll;
  double m_odomNoisePitch;


  std::string m_odomFrameId;
  std::string m_baseFrameId;
  std::string m_footprintFrameId;

  bool m_firstOdometryReceived;
  tf::Stamped<tf::Pose> m_lastOdomPose;


};
}

#endif