00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef DETECTOR_FILTER_H_
00038 #define DETECTOR_FILTER_H_
00039
00040 #include <ros/ros.h>
00041 #include <tf/transform_listener.h>
00042 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00043 #include <boost/thread/mutex.hpp>
00044 #include <filter/extendedkalmanfilter.h>
00045 #include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
00046 #include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
00047 #include <pdf/linearanalyticconditionalgaussian.h>
00048 #include <std_srvs/Empty.h>
00049
00050
00051 namespace detector
00052 {
00053
00054 class DetectorFilter
00055 {
00056 public:
00057 DetectorFilter();
00058 ~DetectorFilter();
00059
00060 bool getPose(geometry_msgs::PoseWithCovarianceStamped& pose);
00061
00062 private:
00063 void poseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose);
00064 void initialize(const geometry_msgs::PoseWithCovarianceStamped& pose);
00065 void decomposeTransform(const geometry_msgs::PoseWithCovarianceStamped& pose,
00066 MatrixWrapper::ColumnVector& vector);
00067 void composeTransform(const MatrixWrapper::ColumnVector& vector,
00068 geometry_msgs::PoseWithCovarianceStamped& pose);
00069
00070 bool resetState(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res );
00071
00072 ros::Subscriber pose_sub_;
00073 ros::ServiceServer reset_srv_;
00074 std::string fixed_frame_;
00075 bool initialized_;
00076 tf::TransformListener tf_;
00077
00078
00079 BFL::LinearAnalyticConditionalGaussian* sys_pdf_;
00080 BFL::LinearAnalyticSystemModelGaussianUncertainty* sys_model_;
00081 BFL::LinearAnalyticConditionalGaussian* meas_pdf_;
00082 BFL::LinearAnalyticMeasurementModelGaussianUncertainty* meas_model_;
00083 BFL::Gaussian* prior_;
00084 BFL::ExtendedKalmanFilter* filter_;
00085 ros::Time filter_time_;
00086
00087
00088 };
00089 }
00090 #endif