43 check_frequency_(check_frequency),
44 timeout_padding_(timeout_padding)
48 geometry_msgs::TransformStamped BufferClient::lookupTransform(
const std::string& target_frame,
const std::string& source_frame,
52 tf2_msgs::LookupTransformGoal goal;
53 goal.target_frame = target_frame;
54 goal.source_frame = source_frame;
55 goal.source_time = time;
56 goal.timeout = timeout;
57 goal.advanced =
false;
59 return processGoal(goal);
62 geometry_msgs::TransformStamped BufferClient::lookupTransform(
const std::string& target_frame,
const ros::Time& target_time,
63 const std::string& source_frame,
const ros::Time& source_time,
64 const std::string& fixed_frame,
const ros::Duration timeout)
const
67 tf2_msgs::LookupTransformGoal goal;
68 goal.target_frame = target_frame;
69 goal.source_frame = source_frame;
70 goal.source_time = source_time;
71 goal.timeout = timeout;
72 goal.target_time = target_time;
73 goal.fixed_frame = fixed_frame;
76 return processGoal(goal);
79 geometry_msgs::TransformStamped BufferClient::processGoal(
const tf2_msgs::LookupTransformGoal& goal)
const
81 client_.sendGoal(goal);
84 if(!client_.waitForResult(goal.timeout + timeout_padding_))
88 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.");
92 throw tf2::TimeoutException(
"The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.");
95 return processResult(*client_.getResult());
98 geometry_msgs::TransformStamped BufferClient::processResult(
const tf2_msgs::LookupTransformResult& result)
const
101 if(result.error.error != result.error.NO_ERROR){
103 if(result.error.error == result.error.LOOKUP_ERROR)
106 if(result.error.error == result.error.CONNECTIVITY_ERROR)
109 if(result.error.error == result.error.EXTRAPOLATION_ERROR)
112 if(result.error.error == result.error.INVALID_ARGUMENT_ERROR)
115 if(result.error.error == result.error.TIMEOUT_ERROR)
121 return result.transform;
124 bool BufferClient::canTransform(
const std::string& target_frame,
const std::string& source_frame,
129 lookupTransform(target_frame, source_frame, time, timeout);
143 bool BufferClient::canTransform(
const std::string& target_frame,
const ros::Time& target_time,
144 const std::string& source_frame,
const ros::Time& source_time,
145 const std::string& fixed_frame,
const ros::Duration timeout, std::string* errstr)
const
149 lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);