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
00033 #include "tf2_ros/buffer.h"
00034
00035 #include <ros/assert.h>
00036
00037 namespace tf2_ros
00038 {
00039
00040 Buffer::Buffer(ros::Duration cache_time, bool debug) :
00041 BufferCore(cache_time)
00042 {
00043 if(debug && !ros::service::exists("~tf2_frames", false))
00044 {
00045 ros::NodeHandle n("~");
00046 frames_server_ = n.advertiseService("tf2_frames", &Buffer::getFrames, this);
00047 }
00048 }
00049
00050 geometry_msgs::TransformStamped
00051 Buffer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
00052 const ros::Time& time, const ros::Duration timeout) const
00053 {
00054 canTransform(target_frame, source_frame, time, timeout);
00055 return lookupTransform(target_frame, source_frame, time);
00056 }
00057
00058
00059 geometry_msgs::TransformStamped
00060 Buffer::lookupTransform(const std::string& target_frame, const ros::Time& target_time,
00061 const std::string& source_frame, const ros::Time& source_time,
00062 const std::string& fixed_frame, const ros::Duration timeout) const
00063 {
00064 canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
00065 return lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame);
00066 }
00067
00072 ros::Time now_fallback_to_wall()
00073 {
00074 try
00075 {
00076 return ros::Time::now();
00077 }
00078 catch (ros::TimeNotInitializedException ex)
00079 {
00080 ros::WallTime wt = ros::WallTime::now();
00081 return ros::Time(wt.sec, wt.nsec);
00082 }
00083 }
00084
00090 void sleep_fallback_to_wall(const ros::Duration& d)
00091 {
00092 try
00093 {
00094 d.sleep();
00095 }
00096 catch (ros::TimeNotInitializedException ex)
00097 {
00098 ros::WallDuration wd = ros::WallDuration(d.sec, d.nsec);
00099 wd.sleep();
00100 }
00101 }
00102
00103 bool
00104 Buffer::canTransform(const std::string& target_frame, const std::string& source_frame,
00105 const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
00106 {
00107 if (!checkAndErrorDedicatedThreadPresent(errstr))
00108 return false;
00109
00110
00111 ros::Time start_time = now_fallback_to_wall();
00112 while (now_fallback_to_wall() < start_time + timeout &&
00113 !canTransform(target_frame, source_frame, time) &&
00114 now_fallback_to_wall() >= start_time)
00115 sleep_fallback_to_wall(ros::Duration(0.01));
00116 return canTransform(target_frame, source_frame, time, errstr);
00117 }
00118
00119
00120 bool
00121 Buffer::canTransform(const std::string& target_frame, const ros::Time& target_time,
00122 const std::string& source_frame, const ros::Time& source_time,
00123 const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
00124 {
00125 if (!checkAndErrorDedicatedThreadPresent(errstr))
00126 return false;
00127
00128
00129 ros::Time start_time = now_fallback_to_wall();
00130 while (now_fallback_to_wall() < start_time + timeout &&
00131 !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) &&
00132 now_fallback_to_wall() >= start_time)
00133 sleep_fallback_to_wall(ros::Duration(0.01));
00134 return canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, errstr);
00135 }
00136
00137
00138 bool Buffer::getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res)
00139 {
00140 res.frame_yaml = allFramesAsYAML();
00141 return true;
00142 }
00143
00144
00145
00146 bool Buffer::checkAndErrorDedicatedThreadPresent(std::string* error_str) const
00147 {
00148 if (isUsingDedicatedThread())
00149 return true;
00150
00151
00152
00153 if (error_str)
00154 *error_str = tf2_ros::threading_error;
00155
00156 ROS_ERROR("%s", tf2_ros::threading_error.c_str());
00157 return false;
00158 }
00159
00160
00161
00162 }