Go to the documentation of this file.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
00030
00031
00032
00033
00034
00035
00036
00037 #include <tf2_ros/buffer_client.h>
00038
00039 namespace tf2_ros
00040 {
00041 BufferClient::BufferClient(std::string ns, double check_frequency, ros::Duration timeout_padding):
00042 client_(ns),
00043 check_frequency_(check_frequency),
00044 timeout_padding_(timeout_padding)
00045 {
00046 }
00047
00048 geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const std::string& source_frame,
00049 const ros::Time& time, const ros::Duration timeout) const
00050 {
00051
00052 tf2_msgs::LookupTransformGoal goal;
00053 goal.target_frame = target_frame;
00054 goal.source_frame = source_frame;
00055 goal.source_time = time;
00056 goal.timeout = timeout;
00057 goal.advanced = false;
00058
00059 return processGoal(goal);
00060 }
00061
00062 geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const ros::Time& target_time,
00063 const std::string& source_frame, const ros::Time& source_time,
00064 const std::string& fixed_frame, const ros::Duration timeout) const
00065 {
00066
00067 tf2_msgs::LookupTransformGoal goal;
00068 goal.target_frame = target_frame;
00069 goal.source_frame = source_frame;
00070 goal.source_time = source_time;
00071 goal.timeout = timeout;
00072 goal.target_time = target_time;
00073 goal.fixed_frame = fixed_frame;
00074 goal.advanced = true;
00075
00076 return processGoal(goal);
00077 }
00078
00079 geometry_msgs::TransformStamped BufferClient::processGoal(const tf2_msgs::LookupTransformGoal& goal) const
00080 {
00081 client_.sendGoal(goal);
00082 ros::Rate r(check_frequency_);
00083 bool timed_out = false;
00084 ros::Time start_time = ros::Time::now();
00085 while(ros::ok() && !client_.getState().isDone() && !timed_out)
00086 {
00087 timed_out = ros::Time::now() > start_time + goal.timeout + timeout_padding_;
00088 r.sleep();
00089 }
00090
00091
00092 if(timed_out)
00093 {
00094
00095 client_.cancelGoal();
00096 throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server.");
00097 }
00098
00099 if(client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00100 throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.");
00101
00102
00103 return processResult(*client_.getResult());
00104 }
00105
00106 geometry_msgs::TransformStamped BufferClient::processResult(const tf2_msgs::LookupTransformResult& result) const
00107 {
00108
00109 if(result.error.error != result.error.NO_ERROR){
00110
00111 if(result.error.error == result.error.LOOKUP_ERROR)
00112 throw tf2::LookupException(result.error.error_string);
00113
00114 if(result.error.error == result.error.CONNECTIVITY_ERROR)
00115 throw tf2::ConnectivityException(result.error.error_string);
00116
00117 if(result.error.error == result.error.EXTRAPOLATION_ERROR)
00118 throw tf2::ExtrapolationException(result.error.error_string);
00119
00120 if(result.error.error == result.error.INVALID_ARGUMENT_ERROR)
00121 throw tf2::InvalidArgumentException(result.error.error_string);
00122
00123 if(result.error.error == result.error.TIMEOUT_ERROR)
00124 throw tf2::TimeoutException(result.error.error_string);
00125
00126 throw tf2::TransformException(result.error.error_string);
00127 }
00128
00129 return result.transform;
00130 }
00131
00132 bool BufferClient::canTransform(const std::string& target_frame, const std::string& source_frame,
00133 const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
00134 {
00135 try
00136 {
00137 lookupTransform(target_frame, source_frame, time, timeout);
00138 return true;
00139 }
00140 catch(tf2::TransformException& ex)
00141 {
00142 if(errstr)
00143 *errstr = ex.what();
00144 return false;
00145 }
00146 }
00147
00148 bool BufferClient::canTransform(const std::string& target_frame, const ros::Time& target_time,
00149 const std::string& source_frame, const ros::Time& source_time,
00150 const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
00151 {
00152 try
00153 {
00154 lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
00155 return true;
00156 }
00157 catch(tf2::TransformException& ex)
00158 {
00159 if(errstr)
00160 *errstr = ex.what();
00161 return false;
00162 }
00163 }
00164 };