ethzasl-msf - Modular Sensor Fusion
Time delay compensated single and multi sensor fusion framework based on an EKF
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
List of all members | Public Types | Public Member Functions | Private Types | Private Member Functions | Private Attributes | Friends
msf_pose_sensor::PoseSensorManager Class Reference

#include <pose_sensormanager.h>

Inheritance diagram for msf_pose_sensor::PoseSensorManager:
msf_core::MSF_SensorManagerROS< msf_updates::EKFState > msf_core::MSF_SensorManager< msf_updates::EKFState > msf_core::StateVisitor< msf_updates::EKFState >

Public Types

typedef msf_updates::EKFState EKFState_T
typedef EKFState_T::StateSequence_T StateSequence_T
typedef
EKFState_T::StateDefinition_T 
StateDefinition_T

Public Member Functions

 PoseSensorManager (ros::NodeHandle pnh=ros::NodeHandle("~/pose_sensor"))
virtual ~PoseSensorManager ()
virtual const Config_Tgetcfg ()
- Public Member Functions inherited from msf_core::MSF_SensorManagerROS< msf_updates::EKFState >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MSF_SensorManagerROS (ros::NodeHandle pnh=ros::NodeHandle("~core"))
virtual ~MSF_SensorManagerROS ()
virtual void coreConfig (msf_core::MSF_CoreConfig &config, uint32_t level)
 Gets called by the internal callback caller.
void Config (msf_core::MSF_CoreConfig &config, uint32_t level)
 Gets called by dynamic reconfigure and calls all registered callbacks in callbacks_.
void setHLStateBuffer (const sensor_fusion_comm::ExtEkf &msg)
virtual bool getParam_fixed_bias () const
virtual double getParam_noise_acc () const
virtual double getParam_noise_accbias () const
virtual double getParam_noise_gyr () const
virtual double getParam_noise_gyrbias () const
virtual double getParam_fuzzythres () const
virtual void publishStateInitial (const shared_ptr< msf_updates::EKFState > &state) const
virtual void publishStateAfterPropagation (const shared_ptr< msf_updates::EKFState > &state) const
virtual void publishStateAfterUpdate (const shared_ptr< msf_updates::EKFState > &state) const
- Public Member Functions inherited from msf_core::MSF_SensorManager< msf_updates::EKFState >
 MSF_SensorManager ()
bool data_playback ()
virtual ~MSF_SensorManager ()
void addHandler (shared_ptr< SensorHandler< msf_updates::EKFState > > handler)
virtual void init (double scale) const =0
virtual void initState (msf_updates::EKFState &state) const =0
virtual void calculateQAuxiliaryStates (msf_updates::EKFState &UNUSEDPARAM(state), double UNUSEDPARAM(dt)) const =0
virtual void setP (Eigen::Matrix< double, msf_updates::EKFState::nErrorStatesAtCompileTime, msf_updates::EKFState::nErrorStatesAtCompileTime > &P) const =0
virtual void augmentCorrectionVector (Eigen::Matrix< double, msf_updates::EKFState::nErrorStatesAtCompileTime, 1 > &UNUSEDPARAM(correction)) const =0
virtual void sanityCheckCorrection (msf_updates::EKFState &UNUSEDPARAM(delaystate), const msf_updates::EKFState &UNUSEDPARAM(buffstate), Eigen::Matrix< double, msf_updates::EKFState::nErrorStatesAtCompileTime, 1 > &UNUSEDPARAM(correction)) const =0
- Public Member Functions inherited from msf_core::StateVisitor< msf_updates::EKFState >
virtual void resetState (msf_updates::EKFState &state) const =0
 The state is set to zero/identity, this method will be called to give the user the possibility to change the reset values of some states.
virtual ~StateVisitor ()

Private Types

typedef PoseSensorHandler
< msf_updates::pose_measurement::PoseMeasurement,
PoseSensorManager
PoseSensorHandler_T

Private Member Functions

virtual void config (Config_T &config, uint32_t level)
 Dynamic reconfigure callback.
void init (double scale) const
virtual void resetState (EKFState_T &state) const
virtual void initState (EKFState_T &state) const
virtual void calculateQAuxiliaryStates (EKFState_T &state, double dt) const
virtual void setP (Eigen::Matrix< double, EKFState_T::nErrorStatesAtCompileTime, EKFState_T::nErrorStatesAtCompileTime > &P) const
virtual void augmentCorrectionVector (Eigen::Matrix< double, EKFState_T::nErrorStatesAtCompileTime, 1 > &correction) const
virtual void sanityCheckCorrection (EKFState_T &delaystate, const EKFState_T &buffstate, Eigen::Matrix< double, EKFState_T::nErrorStatesAtCompileTime, 1 > &correction) const

Private Attributes

shared_ptr
< msf_core::IMUHandler_ROS
< msf_updates::EKFState > > 
imu_handler_
shared_ptr< PoseSensorHandler_Tpose_handler_
Config_T config_
ReconfigureServerPtr reconf_server_

Friends

class PoseSensorHandler< msf_updates::pose_measurement::PoseMeasurement, PoseSensorManager >

Additional Inherited Members

- Public Attributes inherited from msf_core::MSF_SensorManager< msf_updates::EKFState >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
shared_ptr< MSF_Core
< msf_updates::EKFState > > 
msf_core_
 The ekf core instance.
- Protected Types inherited from msf_core::MSF_SensorManager< msf_updates::EKFState >
typedef std::vector
< shared_ptr< SensorHandler
< msf_updates::EKFState > > > 
Handlers
- Protected Attributes inherited from msf_core::MSF_SensorManagerROS< msf_updates::EKFState >
msf_core::MSF_CoreConfig config_
 Dynamic reconfigure config.

Member Typedef Documentation

typedef PoseSensorHandler<msf_updates::pose_measurement::PoseMeasurement, PoseSensorManager> msf_pose_sensor::PoseSensorManager::PoseSensorHandler_T
private

Constructor & Destructor Documentation

msf_pose_sensor::PoseSensorManager::PoseSensorManager ( ros::NodeHandle  pnh = ros::NodeHandle("~/pose_sensor"))
inline

< Distort the pose measurements.

virtual msf_pose_sensor::PoseSensorManager::~PoseSensorManager ( )
inlinevirtual

Member Function Documentation

virtual void msf_pose_sensor::PoseSensorManager::augmentCorrectionVector ( Eigen::Matrix< double, EKFState_T::nErrorStatesAtCompileTime, 1 > &  correction) const
inlineprivatevirtual
virtual void msf_pose_sensor::PoseSensorManager::calculateQAuxiliaryStates ( EKFState_T state,
double  dt 
) const
inlineprivatevirtual
virtual void msf_pose_sensor::PoseSensorManager::config ( Config_T config,
uint32_t  level 
)
inlineprivatevirtual
virtual const Config_T& msf_pose_sensor::PoseSensorManager::getcfg ( )
inlinevirtual
void msf_pose_sensor::PoseSensorManager::init ( double  scale) const
inlineprivate

Gravity.

Bias gyroscopes.

Bias accelerometer.

Robot velocity (IMU centered).

Initial angular velocity.

Initial acceleration.

virtual void msf_pose_sensor::PoseSensorManager::initState ( EKFState_T state) const
inlineprivatevirtual
virtual void msf_pose_sensor::PoseSensorManager::resetState ( EKFState_T state) const
inlineprivatevirtual
virtual void msf_pose_sensor::PoseSensorManager::sanityCheckCorrection ( EKFState_T delaystate,
const EKFState_T buffstate,
Eigen::Matrix< double, EKFState_T::nErrorStatesAtCompileTime, 1 > &  correction 
) const
inlineprivatevirtual
virtual void msf_pose_sensor::PoseSensorManager::setP ( Eigen::Matrix< double, EKFState_T::nErrorStatesAtCompileTime, EKFState_T::nErrorStatesAtCompileTime > &  P) const
inlineprivatevirtual

Friends And Related Function Documentation

friend class PoseSensorHandler< msf_updates::pose_measurement::PoseMeasurement,PoseSensorManager >
friend

Member Data Documentation

Config_T msf_pose_sensor::PoseSensorManager::config_
private
shared_ptr<msf_core::IMUHandler_ROS<msf_updates::EKFState> > msf_pose_sensor::PoseSensorManager::imu_handler_
private
shared_ptr<PoseSensorHandler_T> msf_pose_sensor::PoseSensorManager::pose_handler_
private
ReconfigureServerPtr msf_pose_sensor::PoseSensorManager::reconf_server_
private

The documentation for this class was generated from the following file: