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
00047 static boost::numeric::ublas::matrix<double> transformAsMatrix(const Transform& bt)
00048 {
00049 boost::numeric::ublas::matrix<double> outMat(4,4);
00050
00051
00052
00053 double mv[12];
00054 bt.getBasis().getOpenGLSubMatrix(mv);
00055
00056 Vector3 origin = bt.getOrigin();
00057
00058 outMat(0,0)= mv[0];
00059 outMat(0,1) = mv[4];
00060 outMat(0,2) = mv[8];
00061 outMat(1,0) = mv[1];
00062 outMat(1,1) = mv[5];
00063 outMat(1,2) = mv[9];
00064 outMat(2,0) = mv[2];
00065 outMat(2,1) = mv[6];
00066 outMat(2,2) = mv[10];
00067
00068 outMat(3,0) = outMat(3,1) = outMat(3,2) = 0;
00069 outMat(0,3) = origin.x();
00070 outMat(1,3) = origin.y();
00071 outMat(2,3) = origin.z();
00072 outMat(3,3) = 1;
00073
00074
00075 return outMat;
00076 };
00077
00078 TransformListener::TransformListener(ros::Duration max_cache_time, bool spin_thread):
00079 Transformer(true, max_cache_time), dedicated_listener_thread_(NULL)
00080 {
00081 if (spin_thread)
00082 initWithThread();
00083 else
00084 init();
00085 }
00086
00087 TransformListener::TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time, bool spin_thread):
00088 Transformer(true, max_cache_time), dedicated_listener_thread_(NULL), node_(nh)
00089 {
00090 if (spin_thread)
00091 initWithThread();
00092 else
00093 init();
00094 }
00095
00096 TransformListener::~TransformListener()
00097 {
00098 using_dedicated_thread_ = false;
00099 if (dedicated_listener_thread_)
00100 {
00101 dedicated_listener_thread_->join();
00102 delete dedicated_listener_thread_;
00103 }
00104 }
00105
00106
00107 bool TransformListener::ok() const { return ros::ok(); }
00108
00109 void TransformListener::init()
00110 {
00111 message_subscriber_tf_ = node_.subscribe<tf::tfMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1));
00112
00113
00114 if (! ros::service::exists("~tf_frames", false))
00115 {
00116 ros::NodeHandle nh("~");
00117 tf_frames_srv_ = nh.advertiseService("tf_frames", &TransformListener::getFrames, this);
00118 }
00119
00120 ros::NodeHandle local_nh("~");
00121
00122 tf_prefix_ = getPrefixParam(local_nh);
00123 last_update_ros_time_ = ros::Time::now();
00124 }
00125
00126 void TransformListener::initWithThread()
00127 {
00128 using_dedicated_thread_ = true;
00129 ros::SubscribeOptions ops_tf = ros::SubscribeOptions::create<tf::tfMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1), ros::VoidPtr(), &tf_message_callback_queue_);
00130
00131 message_subscriber_tf_ = node_.subscribe(ops_tf);
00132
00133 dedicated_listener_thread_ = new boost::thread(boost::bind(&TransformListener::dedicatedListenerThread, this));
00134
00135 if (! ros::service::exists("~tf_frames", false))
00136 {
00137 ros::NodeHandle nh("~");
00138 tf_frames_srv_ = nh.advertiseService("tf_frames", &TransformListener::getFrames, this);
00139 }
00140
00141 ros::NodeHandle local_nh("~");
00142 tf_prefix_ = getPrefixParam(local_nh);
00143 last_update_ros_time_ = ros::Time::now();
00144 }
00145
00146 void TransformListener::transformQuaternion(const std::string& target_frame,
00147 const geometry_msgs::QuaternionStamped& msg_in,
00148 geometry_msgs::QuaternionStamped& msg_out) const
00149 {
00150 tf::assertQuaternionValid(msg_in.quaternion);
00151
00152 Stamped<Quaternion> pin, pout;
00153 quaternionStampedMsgToTF(msg_in, pin);
00154 transformQuaternion(target_frame, pin, pout);
00155 quaternionStampedTFToMsg(pout, msg_out);
00156 }
00157
00158 void TransformListener::transformVector(const std::string& target_frame,
00159 const geometry_msgs::Vector3Stamped& msg_in,
00160 geometry_msgs::Vector3Stamped& msg_out) const
00161 {
00162 Stamped<Vector3> pin, pout;
00163 vector3StampedMsgToTF(msg_in, pin);
00164 transformVector(target_frame, pin, pout);
00165 vector3StampedTFToMsg(pout, msg_out);
00166 }
00167
00168 void TransformListener::transformPoint(const std::string& target_frame,
00169 const geometry_msgs::PointStamped& msg_in,
00170 geometry_msgs::PointStamped& msg_out) const
00171 {
00172 Stamped<Point> pin, pout;
00173 pointStampedMsgToTF(msg_in, pin);
00174 transformPoint(target_frame, pin, pout);
00175 pointStampedTFToMsg(pout, msg_out);
00176 }
00177
00178 void TransformListener::transformPose(const std::string& target_frame,
00179 const geometry_msgs::PoseStamped& msg_in,
00180 geometry_msgs::PoseStamped& msg_out) const
00181 {
00182 tf::assertQuaternionValid(msg_in.pose.orientation);
00183
00184 Stamped<Pose> pin, pout;
00185 poseStampedMsgToTF(msg_in, pin);
00186 transformPose(target_frame, pin, pout);
00187 poseStampedTFToMsg(pout, msg_out);
00188 }
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222 void TransformListener::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00223 const geometry_msgs::QuaternionStamped& msg_in,
00224 const std::string& fixed_frame, geometry_msgs::QuaternionStamped& msg_out) const
00225 {
00226 tf::assertQuaternionValid(msg_in.quaternion);
00227 Stamped<Quaternion> pin, pout;
00228 quaternionStampedMsgToTF(msg_in, pin);
00229 transformQuaternion(target_frame, target_time, pin, fixed_frame, pout);
00230 quaternionStampedTFToMsg(pout, msg_out);
00231 }
00232
00233 void TransformListener::transformVector(const std::string& target_frame, const ros::Time& target_time,
00234 const geometry_msgs::Vector3Stamped& msg_in,
00235 const std::string& fixed_frame, geometry_msgs::Vector3Stamped& msg_out) const
00236 {
00237 Stamped<Vector3> pin, pout;
00238 vector3StampedMsgToTF(msg_in, pin);
00239 transformVector(target_frame, target_time, pin, fixed_frame, pout);
00240 vector3StampedTFToMsg(pout, msg_out);
00241 }
00242
00243 void TransformListener::transformPoint(const std::string& target_frame, const ros::Time& target_time,
00244 const geometry_msgs::PointStamped& msg_in,
00245 const std::string& fixed_frame, geometry_msgs::PointStamped& msg_out) const
00246 {
00247 Stamped<Point> pin, pout;
00248 pointStampedMsgToTF(msg_in, pin);
00249 transformPoint(target_frame, target_time, pin, fixed_frame, pout);
00250 pointStampedTFToMsg(pout, msg_out);
00251 }
00252
00253 void TransformListener::transformPose(const std::string& target_frame, const ros::Time& target_time,
00254 const geometry_msgs::PoseStamped& msg_in,
00255 const std::string& fixed_frame, geometry_msgs::PoseStamped& msg_out) const
00256 {
00257 tf::assertQuaternionValid(msg_in.pose.orientation);
00258
00259 Stamped<Pose> pin, pout;
00260 poseStampedMsgToTF(msg_in, pin);
00261 transformPose(target_frame, target_time, pin, fixed_frame, pout);
00262 poseStampedTFToMsg(pout, msg_out);
00263 }
00264
00265 void TransformListener::transformPointCloud(const std::string & target_frame, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const
00266 {
00267 StampedTransform transform;
00268 lookupTransform(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp, transform);
00269
00270 transformPointCloud(target_frame, transform, cloudIn.header.stamp, cloudIn, cloudOut);
00271 }
00272 void TransformListener::transformPointCloud(const std::string& target_frame, const ros::Time& target_time,
00273 const sensor_msgs::PointCloud& cloudIn,
00274 const std::string& fixed_frame, sensor_msgs::PointCloud& cloudOut) const
00275 {
00276 StampedTransform transform;
00277 lookupTransform(target_frame, target_time,
00278 cloudIn.header.frame_id, cloudIn.header.stamp,
00279 fixed_frame,
00280 transform);
00281
00282 transformPointCloud(target_frame, transform, target_time, cloudIn, cloudOut);
00283
00284
00285 }
00286
00287
00288 void TransformListener::transformPointCloud(const std::string & target_frame, const Transform& net_transform,
00289 const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const
00290 {
00291 boost::numeric::ublas::matrix<double> transform = transformAsMatrix(net_transform);
00292
00293 unsigned int length = cloudIn.get_points_size();
00294
00295 boost::numeric::ublas::matrix<double> matIn(4, length);
00296
00297 double * matrixPtr = matIn.data().begin();
00298
00299 for (unsigned int i = 0; i < length ; i++)
00300 {
00301 matrixPtr[i] = cloudIn.points[i].x;
00302 matrixPtr[i+length] = cloudIn.points[i].y;
00303 matrixPtr[i+ 2* length] = cloudIn.points[i].z;
00304 matrixPtr[i+ 3* length] = 1;
00305 };
00306
00307 boost::numeric::ublas::matrix<double> matOut = prod(transform, matIn);
00308
00309
00310 if (&cloudIn != &cloudOut)
00311 {
00312 cloudOut.header = cloudIn.header;
00313 cloudOut.set_points_size(length);
00314 cloudOut.set_channels_size(cloudIn.get_channels_size());
00315 for (unsigned int i = 0 ; i < cloudIn.get_channels_size() ; ++i)
00316 cloudOut.channels[i] = cloudIn.channels[i];
00317 }
00318
00319 matrixPtr = matOut.data().begin();
00320
00321
00322 cloudOut.header.stamp = target_time;
00323 cloudOut.header.frame_id = target_frame;
00324 for (unsigned int i = 0; i < length ; i++)
00325 {
00326 cloudOut.points[i].x = matrixPtr[i];
00327 cloudOut.points[i].y = matrixPtr[i + length];
00328 cloudOut.points[i].z = matrixPtr[i + 2* length];
00329 };
00330 }
00331
00332
00333
00334
00335 void TransformListener::subscription_callback(const tf::tfMessageConstPtr& msg)
00336 {
00337 ros::Duration ros_diff = ros::Time::now() - last_update_ros_time_;
00338 float ros_dt = ros_diff.toSec();
00339
00340 if (ros_dt < 0.0)
00341 {
00342 ROS_WARN("Saw a negative time change of %f seconds, clearing the tf buffer.", ros_dt);
00343 clear();
00344 }
00345
00346 last_update_ros_time_ = ros::Time::now();
00347
00348 const tf::tfMessage& msg_in = *msg;
00349 for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
00350 {
00351 StampedTransform trans;
00352 transformStampedMsgToTF(msg_in.transforms[i], trans);
00353 try
00354 {
00355 std::map<std::string, std::string>* msg_header_map = msg_in.__connection_header.get();
00356 std::string authority;
00357 std::map<std::string, std::string>::iterator it = msg_header_map->find("callerid");
00358 if (it == msg_header_map->end())
00359 {
00360 ROS_WARN("Message recieved without callerid");
00361 authority = "no callerid";
00362 }
00363 else
00364 {
00365 authority = it->second;
00366 }
00367 setTransform(trans, authority);
00368 }
00369
00370 catch (TransformException& ex)
00371 {
00373 std::string temp = ex.what();
00374 ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str());
00375 }
00376 }
00377 };
00378
00379
00380
00381
00382