00001 /* 00002 * ViconHandler.hpp 00003 * 00004 * Created on: Jun 19, 2012 00005 * Author: rspica 00006 */ 00007 00008 #ifndef VICONHANDLER_HPP_ 00009 #define VICONHANDLER_HPP_ 00010 00011 00012 #include <telekyb_defines/telekyb_defines.hpp> 00013 #include <telekyb_base/Options.hpp> 00014 #include <StateEstimators/SensorHandler.hpp> 00015 00016 #include <geometry_msgs/TransformStamped.h> 00017 00018 using namespace TELEKYB_NAMESPACE; 00019 00020 namespace telekyb_state { 00021 00022 class ViconHandlerOption : public OptionContainer { 00023 public: 00024 //Option<std::string>* viconTopic; 00025 00026 Option<Eigen::Matrix<double,6,6>>* vicCov; 00027 00028 Option<Eigen::Vector3d>* vicPosition; 00029 Option<Eigen::Quaterniond>* vicOrientation; 00030 00031 Option<std::string>* vicTopic; 00032 ViconHandlerOption(); 00033 }; 00034 00035 class ViconHandler: public SensorHandler{ 00036 00037 protected: 00038 ViconHandlerOption options; 00039 00040 void measureCallback(const geometry_msgs::TransformStamped::ConstPtr& msg); 00041 Eigen::Vector3d vicPosition; 00042 Eigen::Quaterniond vicOrientation; 00043 00044 public: 00045 // //Core functions 00046 void update(StateBufferElement& state, const geometry_msgs::TransformStamped& z); 00047 void initialize(); 00048 void destroy(); 00049 00050 MeasureBufferElement lastMeasure; 00051 00052 }; 00053 00054 00055 } /* namespace telekyb_state */ 00056 #endif /* VICONHANDLER_HPP_ */
