MotionModel.h
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
00003 
00004 /*
00005  * 6D localization for humanoid robots
00006  *
00007  * Copyright 2009-2012 Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/humanoid_localization
00009  *
00010  *
00011  * This program is free software: you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation, version 3.
00014  *
00015  * This program is distributed in the hope that it will be useful,
00016  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  * GNU General Public License for more details.
00019  *
00020  * You should have received a copy of the GNU General Public License
00021  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00022  */
00023 
00024 #ifndef HUMANOID_LOCALIZATION_MOTIONMODEL_H_
00025 #define HUMANOID_LOCALIZATION_MOTIONMODEL_H_
00026 
00027 #include <ros/ros.h>
00028 #include <tf/transform_listener.h>
00029 #include <tf/transform_datatypes.h>
00030 #include <Eigen/Cholesky>
00031 
00032 #include <humanoid_localization/humanoid_localization_defs.h>
00033 
00034 namespace humanoid_localization{
00035 class MotionModel {
00036 public:
00037   MotionModel(ros::NodeHandle* nh, EngineT* rngEngine, tf::TransformListener* tf, const std::string& odomFrameId, const std::string& baseFrameId);
00038   virtual ~MotionModel();
00039   void reset();
00041   bool lookupOdomPose(const ros::Time& t, tf::Stamped<tf::Pose>& odomPose) const;
00042 
00044   bool lookupOdomTransform(const ros::Time& t, tf::Transform& odomTransform) const;
00045 
00048   tf::Transform computeOdomTransform(const tf::Transform currentPose) const;
00049 
00051   bool lookupLocalTransform(const std::string& targetFrame, const ros::Time& t, tf::StampedTransform& localTransform) const;
00052 
00054   bool lookupPoseHeight(const ros::Time& t, double& poseHeight) const;
00055 
00057   void applyOdomTransform(Particles& particles, const tf::Transform& odomTransform);
00058 
00061   bool applyOdomTransformTemporal(Particles& particles, const ros::Time& t, double dt);
00062 
00064   void storeOdomPose(const tf::Stamped<tf::Pose>& odomPose);
00065 
00068   bool getLastOdomPose(tf::Stamped<tf::Pose>& lastOdomPose) const;
00069 
00070 
00071   // aligns fixed-size members in memory
00072   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00073 
00074 protected:
00076   void applyOdomTransform(tf::Pose& particlePose, const tf::Transform& odomTransform);
00077 
00079   void transformPose(tf::Pose& particlePose, const tf::Transform& odomTransform);
00080 
00083   tf::Transform odomTransformNoise(const tf::Transform& odomTransform);
00084 
00086   tf::Transform calibrateOdometry(const tf::Transform& odomTransform) const;
00087 
00088   tf::TransformListener* m_tfListener;
00089 
00090   NormalGeneratorT m_rngNormal; // standard normal-distributed noise
00091   UniformGeneratorT m_rngUniform;
00092   // parameters:
00094   Eigen::Matrix3d m_odomNoise2D;
00096   Eigen::Matrix3d m_odomCalibration2D;
00098   double m_odomNoiseZ;
00100   double m_odomNoiseRoll;
00102   double m_odomNoisePitch;
00103 
00104 
00105   std::string m_odomFrameId;
00106   std::string m_baseFrameId;
00107   std::string m_footprintFrameId;
00108 
00109   bool m_firstOdometryReceived;
00110   tf::Stamped<tf::Pose> m_lastOdomPose;
00111 
00112 
00113 };
00114 }
00115 
00116 #endif


humanoid_localization
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Wed Aug 26 2015 11:54:40