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
00032 #include "tf/transform_listener.h"
00033
00034 #include <boost/numeric/ublas/matrix.hpp>
00035 #include <boost/numeric/ublas/io.hpp>
00036
00037
00038 using namespace tf;
00039 std::string tf::remap(const std::string& frame_id)
00040 {
00041 ros::NodeHandle n("~");
00042 return tf::resolve(getPrefixParam(n), frame_id);
00043 };
00044
00045
00046 TransformListener::TransformListener(ros::Duration max_cache_time, bool spin_thread):
00047 Transformer(true, max_cache_time), tf2_listener_(Transformer::tf2_buffer_, node_, spin_thread)
00048 {
00049
00050 }
00051
00052 TransformListener::TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time, bool spin_thread):
00053 Transformer(true, max_cache_time), node_(nh), tf2_listener_(Transformer::tf2_buffer_, nh, spin_thread)
00054 {
00055
00056 }
00057
00058 TransformListener::~TransformListener()
00059 {
00060
00061 }
00062
00063
00064 bool TransformListener::ok() const { return ros::ok(); }
00065
00066
00067
00068 void TransformListener::transformQuaternion(const std::string& target_frame,
00069 const geometry_msgs::QuaternionStamped& msg_in,
00070 geometry_msgs::QuaternionStamped& msg_out) const
00071 {
00072 tf::assertQuaternionValid(msg_in.quaternion);
00073
00074 Stamped<Quaternion> pin, pout;
00075 quaternionStampedMsgToTF(msg_in, pin);
00076 transformQuaternion(target_frame, pin, pout);
00077 quaternionStampedTFToMsg(pout, msg_out);
00078 }
00079
00080 void TransformListener::transformVector(const std::string& target_frame,
00081 const geometry_msgs::Vector3Stamped& msg_in,
00082 geometry_msgs::Vector3Stamped& msg_out) const
00083 {
00084 Stamped<Vector3> pin, pout;
00085 vector3StampedMsgToTF(msg_in, pin);
00086 transformVector(target_frame, pin, pout);
00087 vector3StampedTFToMsg(pout, msg_out);
00088 }
00089
00090 void TransformListener::transformPoint(const std::string& target_frame,
00091 const geometry_msgs::PointStamped& msg_in,
00092 geometry_msgs::PointStamped& msg_out) const
00093 {
00094 Stamped<Point> pin, pout;
00095 pointStampedMsgToTF(msg_in, pin);
00096 transformPoint(target_frame, pin, pout);
00097 pointStampedTFToMsg(pout, msg_out);
00098 }
00099
00100 void TransformListener::transformPose(const std::string& target_frame,
00101 const geometry_msgs::PoseStamped& msg_in,
00102 geometry_msgs::PoseStamped& msg_out) const
00103 {
00104 tf::assertQuaternionValid(msg_in.pose.orientation);
00105
00106 Stamped<Pose> pin, pout;
00107 poseStampedMsgToTF(msg_in, pin);
00108 transformPose(target_frame, pin, pout);
00109 poseStampedTFToMsg(pout, msg_out);
00110 }
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144 void TransformListener::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00145 const geometry_msgs::QuaternionStamped& msg_in,
00146 const std::string& fixed_frame, geometry_msgs::QuaternionStamped& msg_out) const
00147 {
00148 tf::assertQuaternionValid(msg_in.quaternion);
00149 Stamped<Quaternion> pin, pout;
00150 quaternionStampedMsgToTF(msg_in, pin);
00151 transformQuaternion(target_frame, target_time, pin, fixed_frame, pout);
00152 quaternionStampedTFToMsg(pout, msg_out);
00153 }
00154
00155 void TransformListener::transformVector(const std::string& target_frame, const ros::Time& target_time,
00156 const geometry_msgs::Vector3Stamped& msg_in,
00157 const std::string& fixed_frame, geometry_msgs::Vector3Stamped& msg_out) const
00158 {
00159 Stamped<Vector3> pin, pout;
00160 vector3StampedMsgToTF(msg_in, pin);
00161 transformVector(target_frame, target_time, pin, fixed_frame, pout);
00162 vector3StampedTFToMsg(pout, msg_out);
00163 }
00164
00165 void TransformListener::transformPoint(const std::string& target_frame, const ros::Time& target_time,
00166 const geometry_msgs::PointStamped& msg_in,
00167 const std::string& fixed_frame, geometry_msgs::PointStamped& msg_out) const
00168 {
00169 Stamped<Point> pin, pout;
00170 pointStampedMsgToTF(msg_in, pin);
00171 transformPoint(target_frame, target_time, pin, fixed_frame, pout);
00172 pointStampedTFToMsg(pout, msg_out);
00173 }
00174
00175 void TransformListener::transformPose(const std::string& target_frame, const ros::Time& target_time,
00176 const geometry_msgs::PoseStamped& msg_in,
00177 const std::string& fixed_frame, geometry_msgs::PoseStamped& msg_out) const
00178 {
00179 tf::assertQuaternionValid(msg_in.pose.orientation);
00180
00181 Stamped<Pose> pin, pout;
00182 poseStampedMsgToTF(msg_in, pin);
00183 transformPose(target_frame, target_time, pin, fixed_frame, pout);
00184 poseStampedTFToMsg(pout, msg_out);
00185 }
00186
00187 void TransformListener::transformPointCloud(const std::string & target_frame, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const
00188 {
00189 StampedTransform transform;
00190 lookupTransform(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp, transform);
00191
00192 transformPointCloud(target_frame, transform, cloudIn.header.stamp, cloudIn, cloudOut);
00193 }
00194 void TransformListener::transformPointCloud(const std::string& target_frame, const ros::Time& target_time,
00195 const sensor_msgs::PointCloud& cloudIn,
00196 const std::string& fixed_frame, sensor_msgs::PointCloud& cloudOut) const
00197 {
00198 StampedTransform transform;
00199 lookupTransform(target_frame, target_time,
00200 cloudIn.header.frame_id, cloudIn.header.stamp,
00201 fixed_frame,
00202 transform);
00203
00204 transformPointCloud(target_frame, transform, target_time, cloudIn, cloudOut);
00205
00206
00207 }
00208
00209 inline void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out)
00210 {
00211
00212 double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x();
00213 double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y();
00214 double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z();
00215
00216 out.x = x; out.y = y; out.z = z;
00217 }
00218
00219
00220 void TransformListener::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform,
00221 const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn,
00222 sensor_msgs::PointCloud & cloudOut) const
00223 {
00224 tf::Vector3 origin = net_transform.getOrigin();
00225 tf::Matrix3x3 basis = net_transform.getBasis();
00226
00227 unsigned int length = cloudIn.points.size();
00228
00229
00230 if (&cloudIn != &cloudOut)
00231 {
00232 cloudOut.header = cloudIn.header;
00233 cloudOut.points.resize(length);
00234 cloudOut.channels.resize(cloudIn.channels.size());
00235 for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i)
00236 cloudOut.channels[i] = cloudIn.channels[i];
00237 }
00238
00239
00240 cloudOut.header.stamp = target_time;
00241 cloudOut.header.frame_id = target_frame;
00242 for (unsigned int i = 0; i < length ; i++) {
00243 transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]);
00244 }
00245 }
00246
00247
00248
00249