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