#include <MotionModel.h>
Public Member Functions | |
| void | applyOdomTransform (Particles &particles, const tf::Transform &odomTransform) |
| apply odomTransform to all particles (with random noise) | |
| bool | applyOdomTransformTemporal (Particles &particles, const ros::Time &t, double dt) |
| tf::Transform | computeOdomTransform (const tf::Transform currentPose) const |
| bool | getLastOdomPose (tf::Stamped< tf::Pose > &lastOdomPose) const |
| bool | lookupLocalTransform (const std::string &targetFrame, const ros::Time &t, tf::StampedTransform &localTransform) const |
| lookup the local target frame in the base frame (local transform) | |
| bool | lookupOdomPose (const ros::Time &t, tf::Stamped< tf::Pose > &odomPose) const |
| look up the odom pose at a certain time through tf | |
| bool | lookupOdomTransform (const ros::Time &t, tf::Transform &odomTransform) const |
| looks up the odometry pose at time t and then calls computeOdomTransform() | |
| bool | lookupPoseHeight (const ros::Time &t, double &poseHeight) const |
| lookup the height of the torso, based on tf between the base and footprint frames | |
| MotionModel (ros::NodeHandle *nh, EngineT *rngEngine, tf::TransformListener *tf, const std::string &odomFrameId, const std::string &baseFrameId) | |
| void | reset () |
| void | storeOdomPose (const tf::Stamped< tf::Pose > &odomPose) |
| store odomPose as m_lastOdomPose to compute future odom transforms | |
| virtual | ~MotionModel () |
Protected Member Functions | |
| tf::Transform | calibrateOdometry (const tf::Transform &odomTransform) const |
| calibrary odometry transform w.r.t. 2D drift (pos. + orientation) | |
| tf::Transform | odomTransformNoise (const tf::Transform &odomTransform) |
| void | transformPose (tf::Pose &particlePose, const tf::Transform &odomTransform) |
| Transform a particle's pose with the relative odomTransform with added random noise. | |
Protected Attributes | |
| std::string | m_baseFrameId |
| bool | m_firstOdometryReceived |
| std::string | m_footprintFrameId |
| tf::Stamped< tf::Pose > | m_lastOdomPose |
| Vector6f | m_motionNoise |
| Eigen::Matrix3d | m_odomCalibration2D |
| std::string | m_odomFrameId |
| NormalGeneratorT | m_rngNormal |
| UniformGeneratorT | m_rngUniform |
| tf::TransformListener * | m_tfListener |
Definition at line 35 of file MotionModel.h.
| humanoid_localization::MotionModel::MotionModel | ( | ros::NodeHandle * | nh, | |
| EngineT * | rngEngine, | |||
| tf::TransformListener * | tf, | |||
| const std::string & | odomFrameId, | |||
| const std::string & | baseFrameId | |||
| ) |
Definition at line 31 of file MotionModel.cpp.
| humanoid_localization::MotionModel::~MotionModel | ( | ) | [virtual] |
Definition at line 82 of file MotionModel.cpp.
| void humanoid_localization::MotionModel::applyOdomTransform | ( | Particles & | particles, | |
| const tf::Transform & | odomTransform | |||
| ) |
apply odomTransform to all particles (with random noise)
Definition at line 120 of file MotionModel.cpp.
| bool humanoid_localization::MotionModel::applyOdomTransformTemporal | ( | Particles & | particles, | |
| const ros::Time & | t, | |||
| double | dt | |||
| ) |
apply odomTransform to all particles (noisy), with temporal sampling over the range of dt. Times are sampled in an interval +-dt/2 around t, iff dt > 0.0.
Definition at line 128 of file MotionModel.cpp.
| tf::Transform humanoid_localization::MotionModel::calibrateOdometry | ( | const tf::Transform & | odomTransform | ) | const [protected] |
calibrary odometry transform w.r.t. 2D drift (pos. + orientation)
Definition at line 167 of file MotionModel.cpp.
| tf::Transform humanoid_localization::MotionModel::computeOdomTransform | ( | const tf::Transform | currentPose | ) | const |
computes the odometry transform from m_lastOdomPose to currentPose as relative 6D rigid body transform (=identity if m_lastOdomPose not available)
Definition at line 198 of file MotionModel.cpp.
| bool humanoid_localization::MotionModel::getLastOdomPose | ( | tf::Stamped< tf::Pose > & | lastOdomPose | ) | const |
get the last stored odomPose returns false when there is no valid previous pose stored
Definition at line 253 of file MotionModel.cpp.
| bool humanoid_localization::MotionModel::lookupLocalTransform | ( | const std::string & | targetFrame, | |
| const ros::Time & | t, | |||
| tf::StampedTransform & | localTransform | |||
| ) | const |
lookup the local target frame in the base frame (local transform)
Definition at line 237 of file MotionModel.cpp.
| bool humanoid_localization::MotionModel::lookupOdomPose | ( | const ros::Time & | t, | |
| tf::Stamped< tf::Pose > & | odomPose | |||
| ) | const |
look up the odom pose at a certain time through tf
Definition at line 218 of file MotionModel.cpp.
| bool humanoid_localization::MotionModel::lookupOdomTransform | ( | const ros::Time & | t, | |
| tf::Transform & | odomTransform | |||
| ) | const |
looks up the odometry pose at time t and then calls computeOdomTransform()
Definition at line 183 of file MotionModel.cpp.
| bool humanoid_localization::MotionModel::lookupPoseHeight | ( | const ros::Time & | t, | |
| double & | poseHeight | |||
| ) | const |
lookup the height of the torso, based on tf between the base and footprint frames
| tf::Transform humanoid_localization::MotionModel::odomTransformNoise | ( | const tf::Transform & | odomTransform | ) | [protected] |
Generates motion noise corresponding to odomTransform May not be called in parallel, accesses the random generator m_rngNormal
Definition at line 102 of file MotionModel.cpp.
| void humanoid_localization::MotionModel::reset | ( | ) |
Definition at line 116 of file MotionModel.cpp.
| void humanoid_localization::MotionModel::storeOdomPose | ( | const tf::Stamped< tf::Pose > & | odomPose | ) |
store odomPose as m_lastOdomPose to compute future odom transforms
Definition at line 207 of file MotionModel.cpp.
| void humanoid_localization::MotionModel::transformPose | ( | tf::Pose & | particlePose, | |
| const tf::Transform & | odomTransform | |||
| ) | [protected] |
Transform a particle's pose with the relative odomTransform with added random noise.
Definition at line 86 of file MotionModel.cpp.
std::string humanoid_localization::MotionModel::m_baseFrameId [protected] |
Definition at line 99 of file MotionModel.h.
bool humanoid_localization::MotionModel::m_firstOdometryReceived [protected] |
Definition at line 102 of file MotionModel.h.
std::string humanoid_localization::MotionModel::m_footprintFrameId [protected] |
Definition at line 100 of file MotionModel.h.
Definition at line 103 of file MotionModel.h.
Definition at line 93 of file MotionModel.h.
Eigen::Matrix3d humanoid_localization::MotionModel::m_odomCalibration2D [protected] |
Definition at line 94 of file MotionModel.h.
std::string humanoid_localization::MotionModel::m_odomFrameId [protected] |
Definition at line 98 of file MotionModel.h.
Definition at line 89 of file MotionModel.h.
Definition at line 90 of file MotionModel.h.
Definition at line 87 of file MotionModel.h.
