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 #include "robot_localization/ros_filter_utilities.h"
00034 #include "robot_localization/filter_common.h"
00035
00036 std::ostream& operator<<(std::ostream& os, const tf::Vector3 &vec)
00037 {
00038 os << "(" << std::setprecision(20) << vec.getX() << " " << vec.getY() << " " << vec.getZ() << ")\n";
00039
00040 return os;
00041 }
00042
00043 std::ostream& operator<<(std::ostream& os, const tf::Quaternion &quat)
00044 {
00045 double roll, pitch, yaw;
00046 tf::Matrix3x3 orTmp(quat);
00047 orTmp.getRPY(roll, pitch, yaw);
00048
00049 os << "(" << std::setprecision(20) << roll << ", " << pitch << ", " << yaw << ")\n";
00050
00051 return os;
00052 }
00053
00054 std::ostream& operator<<(std::ostream& os, const tf::Transform &trans)
00055 {
00056 os << "Origin: " << trans.getOrigin() <<
00057 "Rotation (RPY): " << trans.getRotation();
00058
00059 return os;
00060 }
00061
00062 namespace RobotLocalization
00063 {
00064 namespace RosFilterUtilities
00065 {
00066 void quatToRPY(const tf::Quaternion &quat, double &roll, double &pitch, double &yaw)
00067 {
00068 tf::Matrix3x3 orTmp(quat);
00069 orTmp.getRPY(roll, pitch, yaw);
00070 }
00071
00072 void stateToTF(const Eigen::VectorXd &state, tf::Transform &stateTF)
00073 {
00074 stateTF.setOrigin(tf::Vector3(state(StateMemberX),
00075 state(StateMemberY),
00076 state(StateMemberZ)));
00077 tf::Quaternion quat;
00078 quat.setRPY(state(StateMemberRoll),
00079 state(StateMemberPitch),
00080 state(StateMemberYaw));
00081
00082 stateTF.setRotation(quat);
00083 }
00084
00085 void TFtoState(const tf::Transform &stateTF, Eigen::VectorXd &state)
00086 {
00087 state(StateMemberX) = stateTF.getOrigin().getX();
00088 state(StateMemberY) = stateTF.getOrigin().getY();
00089 state(StateMemberZ) = stateTF.getOrigin().getZ();
00090 quatToRPY(stateTF.getRotation(), state(StateMemberRoll), state(StateMemberPitch), state(StateMemberYaw));
00091 }
00092 }
00093 }