42 std::ostream&
operator<<(std::ostream& os,
const tf2::Vector3 &vec)
44 os <<
"(" << std::setprecision(20) << vec.getX() <<
" " << vec.getY() <<
" " << vec.getZ() <<
")\n";
51 double roll, pitch, yaw;
53 orTmp.
getRPY(roll, pitch, yaw);
55 os <<
"(" << std::setprecision(20) << roll <<
", " << pitch <<
", " << yaw <<
")\n";
68 std::ostream& operator<<(std::ostream& os, const std::vector<double> &vec)
70 os <<
"(" << std::setprecision(20);
72 for (
size_t i = 0; i < vec.size(); ++i)
84 namespace RosFilterUtilities
93 mat.
getRPY(dummy, dummy, yaw);
99 const std::string &targetFrame,
100 const std::string &sourceFrame,
127 " was unavailable for the time requested. Using latest instead.\n");
135 " to " << targetFrame <<
". Error was " << ex.what() <<
"\n");
147 if (targetFrame == sourceFrame)
158 const std::string &targetFrame,
159 const std::string &sourceFrame,
170 orTmp.
getRPY(roll, pitch, yaw);
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
void stateToTF(const Eigen::VectorXd &state, tf2::Transform &stateTF)
Converts our Eigen state vector into a TF transform/pose.
void fromMsg(const A &, B &b)
std::ostream & operator<<(std::ostream &os, const tf2::Vector3 &vec)
#define ROS_WARN_STREAM_THROTTLE(rate, args)
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.