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