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 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00037 #include <ros/console.h>
00038
00039 #include <string>
00040 #include <vector>
00041
00042 std::ostream& operator<<(std::ostream& os, const tf2::Vector3 &vec)
00043 {
00044 os << "(" << std::setprecision(20) << vec.getX() << " " << vec.getY() << " " << vec.getZ() << ")\n";
00045
00046 return os;
00047 }
00048
00049 std::ostream& operator<<(std::ostream& os, const tf2::Quaternion &quat)
00050 {
00051 double roll, pitch, yaw;
00052 tf2::Matrix3x3 orTmp(quat);
00053 orTmp.getRPY(roll, pitch, yaw);
00054
00055 os << "(" << std::setprecision(20) << roll << ", " << pitch << ", " << yaw << ")\n";
00056
00057 return os;
00058 }
00059
00060 std::ostream& operator<<(std::ostream& os, const tf2::Transform &trans)
00061 {
00062 os << "Origin: " << trans.getOrigin() <<
00063 "Rotation (RPY): " << trans.getRotation();
00064
00065 return os;
00066 }
00067
00068 std::ostream& operator<<(std::ostream& os, const std::vector<double> &vec)
00069 {
00070 os << "(" << std::setprecision(20);
00071
00072 for (size_t i = 0; i < vec.size(); ++i)
00073 {
00074 os << vec[i] << " ";
00075 }
00076
00077 os << ")\n";
00078
00079 return os;
00080 }
00081
00082 namespace RobotLocalization
00083 {
00084 namespace RosFilterUtilities
00085 {
00086
00087 double getYaw(const tf2::Quaternion quat)
00088 {
00089 tf2::Matrix3x3 mat(quat);
00090
00091 double dummy;
00092 double yaw;
00093 mat.getRPY(dummy, dummy, yaw);
00094
00095 return yaw;
00096 }
00097
00098 bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
00099 const std::string &targetFrame,
00100 const std::string &sourceFrame,
00101 const ros::Time &time,
00102 const ros::Duration &timeout,
00103 tf2::Transform &targetFrameTrans)
00104 {
00105 bool retVal = true;
00106
00107
00108 try
00109 {
00110 tf2::fromMsg(buffer.lookupTransform(targetFrame, sourceFrame, time, timeout).transform,
00111 targetFrameTrans);
00112 }
00113 catch (tf2::TransformException &ex)
00114 {
00115
00116
00117
00118 try
00119 {
00120 tf2::fromMsg(buffer.lookupTransform(targetFrame, sourceFrame, ros::Time(0)).transform,
00121 targetFrameTrans);
00122
00123 ROS_WARN_STREAM_THROTTLE(2.0, "Transform from " << sourceFrame << " to " << targetFrame <<
00124 " was unavailable for the time requested. Using latest instead.\n");
00125 }
00126 catch(tf2::TransformException &ex)
00127 {
00128 ROS_WARN_STREAM_THROTTLE(2.0, "Could not obtain transform from " << sourceFrame <<
00129 " to " << targetFrame << ". Error was " << ex.what() << "\n");
00130
00131 retVal = false;
00132 }
00133 }
00134
00135
00136
00137
00138 if (!retVal)
00139 {
00140 if (targetFrame == sourceFrame)
00141 {
00142 targetFrameTrans.setIdentity();
00143 retVal = true;
00144 }
00145 }
00146
00147 return retVal;
00148 }
00149
00150 bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
00151 const std::string &targetFrame,
00152 const std::string &sourceFrame,
00153 const ros::Time &time,
00154 tf2::Transform &targetFrameTrans)
00155 {
00156 return lookupTransformSafe(buffer, targetFrame, sourceFrame, time, ros::Duration(0), targetFrameTrans);
00157 }
00158
00159 void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw)
00160 {
00161 tf2::Matrix3x3 orTmp(quat);
00162 orTmp.getRPY(roll, pitch, yaw);
00163 }
00164
00165 void stateToTF(const Eigen::VectorXd &state, tf2::Transform &stateTF)
00166 {
00167 stateTF.setOrigin(tf2::Vector3(state(StateMemberX),
00168 state(StateMemberY),
00169 state(StateMemberZ)));
00170 tf2::Quaternion quat;
00171 quat.setRPY(state(StateMemberRoll),
00172 state(StateMemberPitch),
00173 state(StateMemberYaw));
00174
00175 stateTF.setRotation(quat);
00176 }
00177
00178 void TFtoState(const tf2::Transform &stateTF, Eigen::VectorXd &state)
00179 {
00180 state(StateMemberX) = stateTF.getOrigin().getX();
00181 state(StateMemberY) = stateTF.getOrigin().getY();
00182 state(StateMemberZ) = stateTF.getOrigin().getZ();
00183 quatToRPY(stateTF.getRotation(), state(StateMemberRoll), state(StateMemberPitch), state(StateMemberYaw));
00184 }
00185
00186 }
00187 }