$search
00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/humanoid_localization/include/humanoid_localization/MotionModel.h $ 00002 // SVN $Id: MotionModel.h 3975 2013-02-26 10:38:51Z hornunga@informatik.uni-freiburg.de $ 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 00056 00057 00059 void applyOdomTransform(Particles& particles, const tf::Transform& odomTransform); 00060 00063 bool applyOdomTransformTemporal(Particles& particles, const ros::Time& t, double dt); 00064 00066 void storeOdomPose(const tf::Stamped<tf::Pose>& odomPose); 00067 00070 bool getLastOdomPose(tf::Stamped<tf::Pose>& lastOdomPose) const; 00071 00072 00073 // aligns fixed-size members in memory 00074 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00075 00076 protected: 00078 void transformPose(tf::Pose& particlePose, const tf::Transform& odomTransform); 00079 00082 tf::Transform odomTransformNoise(const tf::Transform& odomTransform); 00083 00085 tf::Transform calibrateOdometry(const tf::Transform& odomTransform) const; 00086 00087 tf::TransformListener* m_tfListener; 00088 00089 NormalGeneratorT m_rngNormal; // standard normal-distributed noise 00090 UniformGeneratorT m_rngUniform; 00091 // parameters: 00092 //Matrix6f m_motionNoiseL; 00093 Vector6f m_motionNoise; 00094 Eigen::Matrix3d m_odomCalibration2D; 00095 //NoiseParams m_motionNoise; 00096 00097 00098 std::string m_odomFrameId; 00099 std::string m_baseFrameId; 00100 std::string m_footprintFrameId; 00101 00102 bool m_firstOdometryReceived; 00103 tf::Stamped<tf::Pose> m_lastOdomPose; 00104 00105 00106 }; 00107 } 00108 00109 #endif