Go to the documentation of this file.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_UTILITIES_H
00034 #define ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H
00035
00036 #include <tf2/LinearMath/Quaternion.h>
00037 #include <tf2/LinearMath/Transform.h>
00038 #include <tf2_ros/buffer.h>
00039
00040 #include <Eigen/Dense>
00041
00042 #include <iomanip>
00043 #include <iostream>
00044 #include <string>
00045 #include <vector>
00046
00047 #define RF_DEBUG(msg) if (filter_.getDebug()) { debugStream_ << msg; }
00048
00049
00050 std::ostream& operator<<(std::ostream& os, const tf2::Vector3 &vec);
00051 std::ostream& operator<<(std::ostream& os, const tf2::Quaternion &quat);
00052 std::ostream& operator<<(std::ostream& os, const tf2::Transform &trans);
00053 std::ostream& operator<<(std::ostream& os, const std::vector<double> &vec);
00054
00055 namespace RobotLocalization
00056 {
00057 namespace RosFilterUtilities
00058 {
00059
00060 double getYaw(const tf2::Quaternion quat);
00061
00079 bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
00080 const std::string &targetFrame,
00081 const std::string &sourceFrame,
00082 const ros::Time &time,
00083 const ros::Duration &timeout,
00084 tf2::Transform &targetFrameTrans);
00085
00102 bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
00103 const std::string &targetFrame,
00104 const std::string &sourceFrame,
00105 const ros::Time &time,
00106 tf2::Transform &targetFrameTrans);
00107
00114 void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw);
00115
00120 void stateToTF(const Eigen::VectorXd &state, tf2::Transform &stateTF);
00121
00126 void TFtoState(const tf2::Transform &stateTF, Eigen::VectorXd &state);
00127
00128 }
00129 }
00130
00131 #endif // ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H