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 ROBOT_LOCALIZATION_ROS_FILTER_H
00034 #define ROBOT_LOCALIZATION_ROS_FILTER_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
00040 #include <robot_localization/SetPose.h>
00041
00042 #include <ros/ros.h>
00043 #include <std_msgs/String.h>
00044 #include <nav_msgs/Odometry.h>
00045 #include <sensor_msgs/Imu.h>
00046 #include <geometry_msgs/Twist.h>
00047 #include <geometry_msgs/TwistStamped.h>
00048 #include <geometry_msgs/TwistWithCovarianceStamped.h>
00049 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00050 #include <geometry_msgs/AccelWithCovarianceStamped.h>
00051 #include <tf2_ros/transform_listener.h>
00052 #include <tf2_ros/transform_broadcaster.h>
00053 #include <tf2_ros/message_filter.h>
00054 #include <tf2/LinearMath/Transform.h>
00055 #include <message_filters/subscriber.h>
00056 #include <diagnostic_updater/diagnostic_updater.h>
00057 #include <diagnostic_updater/publisher.h>
00058 #include <diagnostic_msgs/DiagnosticStatus.h>
00059
00060 #include <XmlRpcException.h>
00061
00062 #include <Eigen/Dense>
00063
00064 #include <fstream>
00065 #include <map>
00066 #include <numeric>
00067 #include <queue>
00068 #include <string>
00069 #include <vector>
00070 #include <deque>
00071
00072 namespace RobotLocalization
00073 {
00074
00075 struct CallbackData
00076 {
00077 CallbackData(const std::string &topicName,
00078 const std::vector<int> &updateVector,
00079 const int updateSum,
00080 const bool differential,
00081 const bool relative,
00082 const double rejectionThreshold) :
00083 topicName_(topicName),
00084 updateVector_(updateVector),
00085 updateSum_(updateSum),
00086 differential_(differential),
00087 relative_(relative),
00088 rejectionThreshold_(rejectionThreshold)
00089 {
00090 }
00091
00092 std::string topicName_;
00093 std::vector<int> updateVector_;
00094 int updateSum_;
00095 bool differential_;
00096 bool relative_;
00097 double rejectionThreshold_;
00098 };
00099
00100 typedef std::priority_queue<MeasurementPtr, std::vector<MeasurementPtr>, Measurement> MeasurementQueue;
00101 typedef std::deque<MeasurementPtr> MeasurementHistoryDeque;
00102 typedef std::deque<FilterStatePtr> FilterStateHistoryDeque;
00103
00104 template<class T> class RosFilter
00105 {
00106 public:
00112 explicit RosFilter(std::vector<double> args = std::vector<double>());
00113
00118 ~RosFilter();
00119
00125 void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg,
00126 const CallbackData &callbackData,
00127 const std::string &targetFrame);
00128
00132 void controlCallback(const geometry_msgs::Twist::ConstPtr &msg);
00133
00137 void controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg);
00138
00148 void enqueueMeasurement(const std::string &topicName,
00149 const Eigen::VectorXd &measurement,
00150 const Eigen::MatrixXd &measurementCovariance,
00151 const std::vector<int> &updateVector,
00152 const double mahalanobisThresh,
00153 const ros::Time &time);
00154
00164 void forceTwoD(Eigen::VectorXd &measurement,
00165 Eigen::MatrixXd &measurementCovariance,
00166 std::vector<int> &updateVector);
00167
00172 bool getFilteredOdometryMessage(nav_msgs::Odometry &message);
00173
00178 bool getFilteredAccelMessage(geometry_msgs::AccelWithCovarianceStamped &message);
00179
00190 void imuCallback(const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName,
00191 const CallbackData &poseCallbackData, const CallbackData &twistCallbackData,
00192 const CallbackData &accelCallbackData);
00193
00198 void integrateMeasurements(const ros::Time ¤tTime);
00199
00202 void loadParams();
00203
00213 void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName,
00214 const CallbackData &poseCallbackData, const CallbackData &twistCallbackData);
00215
00222 void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
00223 const CallbackData &callbackData,
00224 const std::string &targetFrame,
00225 const bool imuData);
00226
00229 void run();
00230
00234 void setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
00235
00240 bool setPoseSrvCallback(robot_localization::SetPose::Request& request,
00241 robot_localization::SetPose::Response&);
00242
00246 std::string tfFailureReasonString(const tf2_ros::FilterFailureReason reason);
00247
00253 void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
00254 const CallbackData &callbackData,
00255 const std::string &targetFrame);
00256
00257 protected:
00266 bool revertTo(const double time);
00267
00273 void saveFilterState(FilterBase &filter);
00274
00278 void clearExpiredHistory(const double cutoffTime);
00279
00286 void addDiagnostic(const int errLevel,
00287 const std::string &topicAndClass,
00288 const std::string &message,
00289 const bool staticDiag);
00290
00294 void aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper);
00295
00308 void copyCovariance(const double *arr,
00309 Eigen::MatrixXd &covariance,
00310 const std::string &topicName,
00311 const std::vector<int> &updateVector,
00312 const size_t offset,
00313 const size_t dimension);
00314
00322 void copyCovariance(const Eigen::MatrixXd &covariance,
00323 double *arr,
00324 const size_t dimension);
00325
00330 std::vector<int> loadUpdateConfig(const std::string &topicName);
00331
00340 bool prepareAcceleration(const sensor_msgs::Imu::ConstPtr &msg,
00341 const std::string &topicName,
00342 const std::string &targetFrame,
00343 std::vector<int> &updateVector,
00344 Eigen::VectorXd &measurement,
00345 Eigen::MatrixXd &measurementCovariance);
00346
00359 bool preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
00360 const std::string &topicName,
00361 const std::string &targetFrame,
00362 const bool differential,
00363 const bool relative,
00364 const bool imuData,
00365 std::vector<int> &updateVector,
00366 Eigen::VectorXd &measurement,
00367 Eigen::MatrixXd &measurementCovariance);
00368
00378 bool prepareTwist(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
00379 const std::string &topicName,
00380 const std::string &targetFrame,
00381 std::vector<int> &updateVector,
00382 Eigen::VectorXd &measurement,
00383 Eigen::MatrixXd &measurementCovariance);
00384
00387 std::string baseLinkFrameId_;
00388
00391 ros::Subscriber controlSub_;
00392
00398 std::map<std::string, std::string> staticDiagnostics_;
00399
00405 std::map<std::string, std::string> dynamicDiagnostics_;
00406
00409 std::ofstream debugStream_;
00410
00413 int dynamicDiagErrorLevel_;
00414
00417 diagnostic_updater::Updater diagnosticUpdater_;
00418
00421 T filter_;
00422
00425 double frequency_;
00426
00431 double historyLength_;
00432
00435 Eigen::VectorXd latestControl_;
00436
00439 ros::Time latestControlTime_;
00440
00443 ros::Duration tfTimeout_;
00444
00447 std::vector<ros::Subscriber> topicSubs_;
00448
00459 std::map<std::string, tf2::Transform> initialMeasurements_;
00460
00469 std::map<std::string, ros::Time> lastMessageTimes_;
00470
00478 ros::Time lastSetPoseTime_;
00479
00482 std::string mapFrameId_;
00483
00488 MeasurementQueue measurementQueue_;
00489
00492 ros::NodeHandle nh_;
00493
00496 ros::NodeHandle nhLocal_;
00497
00500 std::string odomFrameId_;
00501
00510 std::map<std::string, tf2::Transform> previousMeasurements_;
00511
00514 std::map<std::string, Eigen::MatrixXd> previousMeasurementCovariances_;
00515
00518 bool printDiagnostics_;
00519
00522 std::map<std::string, bool> removeGravitationalAcc_;
00523
00526 double gravitationalAcc_;
00527
00531 ros::Subscriber setPoseSub_;
00532
00536 ros::ServiceServer setPoseSrv_;
00537
00540 bool smoothLaggedData_;
00541
00544 std::vector<std::string> stateVariableNames_;
00545
00548 int staticDiagErrorLevel_;
00549
00552 tf2_ros::Buffer tfBuffer_;
00553
00556 tf2_ros::TransformListener tfListener_;
00557
00560 ros::Duration tfTimeOffset_;
00561
00568 bool twoDMode_;
00569
00572 bool useControl_;
00573
00580 geometry_msgs::TransformStamped worldBaseLinkTransMsg_;
00581
00584 std::string worldFrameId_;
00585
00588 bool publishTransform_;
00589
00592 bool publishAcceleration_;
00593
00595
00596
00597
00598 FilterStateHistoryDeque filterStateHistory_;
00599
00601
00602
00603
00604 MeasurementHistoryDeque measurementHistory_;
00605 };
00606
00607 }
00608
00609 #endif // ROBOT_LOCALIZATION_ROS_FILTER_H