33 #ifndef ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H 34 #define ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H 40 #include <Eigen/Dense> 47 #define RF_DEBUG(msg) if (filter_.getDebug()) { debugStream_ << msg; } 50 std::ostream&
operator<<(std::ostream& os,
const tf2::Vector3 &vec);
53 std::ostream& operator<<(std::ostream& os, const std::vector<double> &vec);
57 namespace RosFilterUtilities
81 const std::string &targetFrame,
82 const std::string &sourceFrame,
86 const bool silent =
false);
106 const std::string &targetFrame,
107 const std::string &sourceFrame,
110 const bool silent =
false);
135 #endif // ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H
void stateToTF(const Eigen::VectorXd &state, tf2::Transform &stateTF)
Converts our Eigen state vector into a TF transform/pose.
std::ostream & operator<<(std::ostream &os, const tf2::Vector3 &vec)
bool lookupTransformSafe(const tf2_ros::Buffer &buffer, const std::string &targetFrame, const std::string &sourceFrame, const ros::Time &time, const ros::Duration &timeout, tf2::Transform &targetFrameTrans, const bool silent=false)
Method for safely obtaining transforms.
double getYaw(const tf2::Quaternion quat)
void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw)
Utility method for converting quaternion to RPY.
void TFtoState(const tf2::Transform &stateTF, Eigen::VectorXd &state)
Converts a TF transform/pose into our Eigen state vector.