#include <ViconImuStateEstimator.hpp>
Public Member Functions | |
virtual void | destroy () |
virtual std::string | getName () const |
virtual void | initialize () |
virtual void | willBecomeActive () |
virtual void | willBecomeInActive () |
Protected Member Functions | |
void | imuCallback (const sensor_msgs::Imu::ConstPtr &msg) |
void | initVelocityFilters () |
void | publishState () |
void | spin () |
void | ssxCallback (const telekyb_msgs::TKState::ConstPtr &stateMsg) |
void | viconCallback (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) |
Protected Attributes | |
Velocity3D | angVelocityNED |
boost::mutex | imuMutex |
ros::Subscriber | imuSub |
telekyb::Time | intTime |
telekyb::Timer | intTimer |
ros::NodeHandle | nodeHandle |
ViconImuStateEstimatorOptions | options |
Position3D | posNED |
Quaternion | quatNED |
telekyb::Timer | rtTimer |
IIRFilter * | smoothVelFilter [3] |
Velocity3D | smoothVelNED |
ros::Publisher | smoothVelPub |
bool | spinning |
boost::thread | thread |
boost::mutex | timeMutex |
IIRFilter * | velFilter [3] |
Velocity3D | velNED |
boost::mutex | viconMutex |
ros::Subscriber | viconSub |
Definition at line 60 of file ViconImuStateEstimator.hpp.
void telekyb_state::ViconImuStateEstimator::destroy | ( | ) | [virtual] |
Implements TELEKYB_NAMESPACE::StateEstimator.
Definition at line 138 of file ViconImuStateEstimator.cpp.
std::string telekyb_state::ViconImuStateEstimator::getName | ( | ) | const [virtual] |
Implements TELEKYB_NAMESPACE::StateEstimator.
Definition at line 149 of file ViconImuStateEstimator.cpp.
void telekyb_state::ViconImuStateEstimator::imuCallback | ( | const sensor_msgs::Imu::ConstPtr & | msg | ) | [protected] |
Definition at line 237 of file ViconImuStateEstimator.cpp.
void telekyb_state::ViconImuStateEstimator::initialize | ( | ) | [virtual] |
Implements TELEKYB_NAMESPACE::StateEstimator.
Definition at line 83 of file ViconImuStateEstimator.cpp.
void telekyb_state::ViconImuStateEstimator::initVelocityFilters | ( | ) | [protected] |
Definition at line 63 of file ViconImuStateEstimator.cpp.
void telekyb_state::ViconImuStateEstimator::publishState | ( | ) | [protected] |
Definition at line 181 of file ViconImuStateEstimator.cpp.
void telekyb_state::ViconImuStateEstimator::spin | ( | ) | [protected] |
Definition at line 94 of file ViconImuStateEstimator.cpp.
void telekyb_state::ViconImuStateEstimator::ssxCallback | ( | const telekyb_msgs::TKState::ConstPtr & | stateMsg | ) | [protected] |
void telekyb_state::ViconImuStateEstimator::viconCallback | ( | const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & | msg | ) | [protected] |
Definition at line 154 of file ViconImuStateEstimator.cpp.
void telekyb_state::ViconImuStateEstimator::willBecomeActive | ( | ) | [virtual] |
Implements TELEKYB_NAMESPACE::StateEstimator.
Definition at line 110 of file ViconImuStateEstimator.cpp.
void telekyb_state::ViconImuStateEstimator::willBecomeInActive | ( | ) | [virtual] |
Implements TELEKYB_NAMESPACE::StateEstimator.
Definition at line 128 of file ViconImuStateEstimator.cpp.
Definition at line 78 of file ViconImuStateEstimator.hpp.
boost::mutex telekyb_state::ViconImuStateEstimator::imuMutex [protected] |
Definition at line 89 of file ViconImuStateEstimator.hpp.
Definition at line 66 of file ViconImuStateEstimator.hpp.
telekyb::Time telekyb_state::ViconImuStateEstimator::intTime [protected] |
Definition at line 83 of file ViconImuStateEstimator.hpp.
telekyb::Timer telekyb_state::ViconImuStateEstimator::intTimer [protected] |
Definition at line 84 of file ViconImuStateEstimator.hpp.
Definition at line 64 of file ViconImuStateEstimator.hpp.
Definition at line 62 of file ViconImuStateEstimator.hpp.
Definition at line 80 of file ViconImuStateEstimator.hpp.
Definition at line 79 of file ViconImuStateEstimator.hpp.
telekyb::Timer telekyb_state::ViconImuStateEstimator::rtTimer [protected] |
Definition at line 92 of file ViconImuStateEstimator.hpp.
IIRFilter* telekyb_state::ViconImuStateEstimator::smoothVelFilter[3] [protected] |
Definition at line 76 of file ViconImuStateEstimator.hpp.
Definition at line 82 of file ViconImuStateEstimator.hpp.
Definition at line 67 of file ViconImuStateEstimator.hpp.
bool telekyb_state::ViconImuStateEstimator::spinning [protected] |
Definition at line 95 of file ViconImuStateEstimator.hpp.
boost::thread telekyb_state::ViconImuStateEstimator::thread [protected] |
Definition at line 87 of file ViconImuStateEstimator.hpp.
boost::mutex telekyb_state::ViconImuStateEstimator::timeMutex [protected] |
Definition at line 91 of file ViconImuStateEstimator.hpp.
IIRFilter* telekyb_state::ViconImuStateEstimator::velFilter[3] [protected] |
Definition at line 75 of file ViconImuStateEstimator.hpp.
Definition at line 81 of file ViconImuStateEstimator.hpp.
boost::mutex telekyb_state::ViconImuStateEstimator::viconMutex [protected] |
Definition at line 90 of file ViconImuStateEstimator.hpp.
Definition at line 65 of file ViconImuStateEstimator.hpp.