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 #ifndef RobotLocalization_RosFilter_h
00034 #define RobotLocalization_RosFilter_h
00035
00036 #include "robot_localization/ros_filter_utilities.h"
00037 #include "robot_localization/filter_common.h"
00038 #include "robot_localization/filter_base.h"
00039 #include "robot_localization/SetPose.h"
00040
00041 #include <ros/ros.h>
00042 #include <std_msgs/String.h>
00043 #include <nav_msgs/Odometry.h>
00044 #include <sensor_msgs/Imu.h>
00045 #include <geometry_msgs/TwistWithCovarianceStamped.h>
00046 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00047 #include <tf/transform_listener.h>
00048 #include <tf/transform_broadcaster.h>
00049 #include <tf/message_filter.h>
00050 #include <message_filters/subscriber.h>
00051 #include <diagnostic_updater/diagnostic_updater.h>
00052 #include <diagnostic_updater/publisher.h>
00053 #include <diagnostic_msgs/DiagnosticStatus.h>
00054
00055 #include <XmlRpcException.h>
00056
00057 #include <Eigen/Dense>
00058
00059 #include <numeric>
00060 #include <fstream>
00061
00062
00063 typedef boost::shared_ptr< message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped> > poseMFSubPtr;
00064 typedef boost::shared_ptr< message_filters::Subscriber<geometry_msgs::TwistWithCovarianceStamped> > twistMFSubPtr;
00065 typedef boost::shared_ptr< tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped> > poseMFPtr;
00066 typedef boost::shared_ptr< tf::MessageFilter<geometry_msgs::TwistWithCovarianceStamped> > twistMFPtr;
00067 typedef boost::shared_ptr< tf::MessageFilter<sensor_msgs::Imu> > imuMFPtr;
00068
00069 namespace RobotLocalization
00070 {
00071 typedef std::priority_queue<Measurement, std::vector<Measurement>, Measurement> MeasurementQueue;
00072
00073 template<class T> class RosFilter
00074 {
00075 public:
00076
00082 RosFilter(std::vector<double> args = std::vector<double>());
00083
00088 ~RosFilter();
00089
00097 void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg,
00098 const std::string &topicName,
00099 const std::string &targetFrame,
00100 const std::vector<int> &updateVector,
00101 const double mahalanobisThresh);
00102
00112 void enqueueMeasurement(const std::string &topicName,
00113 const Eigen::VectorXd &measurement,
00114 const Eigen::MatrixXd &measurementCovariance,
00115 const std::vector<int> &updateVector,
00116 const double mahalanobisThresh,
00117 const ros::Time &time);
00118
00128 void forceTwoD(Eigen::VectorXd &measurement,
00129 Eigen::MatrixXd &measurementCovariance,
00130 std::vector<int> &updateVector);
00131
00136 bool getFilteredOdometryMessage(nav_msgs::Odometry &message);
00137
00145 void imuCallback(const sensor_msgs::Imu::ConstPtr &msg,
00146 const std::string &topicName);
00147
00152 void integrateMeasurements(const double currentTime);
00153
00156 void loadParams();
00157
00165 void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg,
00166 const std::string &topicName);
00167
00176 void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
00177 const std::string &topicName,
00178 const std::string &targetFrame,
00179 const std::vector<int> &updateVector,
00180 const bool differential,
00181 const bool relative,
00182 const bool imuData,
00183 const double mahalanobisThresh);
00184
00187 void run();
00188
00192 void setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
00193
00198 bool setPoseSrvCallback(robot_localization::SetPose::Request& request,
00199 robot_localization::SetPose::Response&);
00200
00204 std::string tfFailureReasonString(const tf::FilterFailureReason reason);
00205
00212 void transformImuFailureCallback(const sensor_msgs::Imu::ConstPtr &msg,
00213 const tf::FilterFailureReason reason,
00214 const std::string &topicName,
00215 const std::string &targetFrame);
00216
00223 void transformPoseFailureCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
00224 const tf::FilterFailureReason reason,
00225 const std::string &topicName,
00226 const std::string &targetFrame);
00227
00234 void transformTwistFailureCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
00235 const tf::FilterFailureReason reason,
00236 const std::string &topicName,
00237 const std::string &targetFrame);
00238
00246 void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
00247 const std::string &topicName,
00248 const std::string &targetFrame,
00249 const std::vector<int> &updateVector,
00250 const double mahalanobisThresh);
00251
00252 protected:
00253
00259 void addDiagnostic(const int errLevel,
00260 const std::string &topicAndClass,
00261 const std::string &message,
00262 const bool staticDiag);
00263
00267 void aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper);
00268
00281 void copyCovariance(const double *arr,
00282 Eigen::MatrixXd &covariance,
00283 const std::string &topicName,
00284 const std::vector<int> &updateVector,
00285 const size_t offset,
00286 const size_t dimension);
00287
00295 void copyCovariance(const Eigen::MatrixXd &covariance,
00296 double *arr,
00297 const size_t dimension);
00298
00303 std::vector<int> loadUpdateConfig(const std::string &topicName);
00304
00320 bool lookupTransformSafe(const std::string &targetFrame, const std::string &sourceFrame,
00321 const ros::Time &time, tf::StampedTransform &targetFrameTrans);
00322
00331 bool prepareAcceleration(const sensor_msgs::Imu::ConstPtr &msg,
00332 const std::string &topicName,
00333 const std::string &targetFrame,
00334 std::vector<int> &updateVector,
00335 Eigen::VectorXd &measurement,
00336 Eigen::MatrixXd &measurementCovariance);
00337
00348 bool preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
00349 const std::string &topicName,
00350 const std::string &targetFrame,
00351 const bool differential,
00352 const bool relative,
00353 const bool imuData,
00354 std::vector<int> &updateVector,
00355 Eigen::VectorXd &measurement,
00356 Eigen::MatrixXd &measurementCovariance);
00357
00367 bool prepareTwist(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
00368 const std::string &topicName,
00369 const std::string &targetFrame,
00370 std::vector<int> &updateVector,
00371 Eigen::VectorXd &measurement,
00372 Eigen::MatrixXd &measurementCovariance);
00373
00376 std::map<std::string, imuMFPtr> accelerationMessageFilters_;
00377
00380 std::string baseLinkFrameId_;
00381
00387 std::map<std::string, std::string> staticDiagnostics_;
00388
00394 std::map<std::string, std::string> dynamicDiagnostics_;
00395
00398 std::ofstream debugStream_;
00399
00402 int dynamicDiagErrorLevel_;
00403
00406 diagnostic_updater::Updater diagnosticUpdater_;
00407
00410 T filter_;
00411
00414 double frequency_;
00415
00419 std::vector<ros::Subscriber> imuTopicSubs_;
00420
00431 std::map<std::string, tf::Transform> initialMeasurements_;
00432
00441 std::map<std::string, ros::Time> lastMessageTimes_;
00442
00450 ros::Time lastSetPoseTime_;
00451
00454 std::string mapFrameId_;
00455
00460 MeasurementQueue measurementQueue_;
00461
00464 ros::NodeHandle nh_;
00465
00468 ros::NodeHandle nhLocal_;
00469
00472 std::string odomFrameId_;
00473
00476 std::vector<ros::Subscriber> odomTopicSubs_;
00477
00480 std::map<std::string, poseMFPtr> poseMessageFilters_;
00481
00484 std::vector<poseMFSubPtr> poseTopicSubs_;
00485
00494 std::map<std::string, tf::Transform> previousMeasurements_;
00495
00498 std::map<std::string, Eigen::MatrixXd> previousMeasurementCovariances_;
00499
00502 bool printDiagnostics_;
00503
00506 std::map<std::string, bool> removeGravitationalAcc_;
00507
00511 ros::Subscriber setPoseSub_;
00512
00516 ros::ServiceServer setPoseSrv_;
00517
00520 std::vector<std::string> stateVariableNames_;
00521
00524 int staticDiagErrorLevel_;
00525
00528 tf::TransformListener tfListener_;
00529
00532 std::string tfPrefix_;
00533
00536 ros::Duration tfTimeOffset_;
00537
00540 std::map<std::string, twistMFPtr> twistMessageFilters_;
00541
00545 std::vector<twistMFSubPtr> twistTopicSubs_;
00546
00553 bool twoDMode_;
00554
00561 geometry_msgs::TransformStamped worldBaseLinkTransMsg_;
00562
00565 std::string worldFrameId_;
00566
00567 };
00568 }
00569
00570 #endif