44 #include <boost/unordered_map.hpp> 45 #include <boost/signals2.hpp> 46 #include "geometry_msgs/TwistStamped.h" 53 std::string
resolve(
const std::string& prefix,
const std::string& frame_name);
129 void lookupTransform(
const std::string& target_frame,
const std::string& source_frame,
143 const std::string& source_frame,
const ros::Time& source_time,
166 void lookupTwist(
const std::string& tracking_frame,
const std::string& observation_frame,
const std::string& reference_frame,
167 const tf::Point & reference_point,
const std::string& reference_point_frame,
169 geometry_msgs::Twist& twist)
const;
181 void lookupTwist(
const std::string& tracking_frame,
const std::string& observation_frame,
183 geometry_msgs::Twist& twist)
const;
193 bool waitForTransform(
const std::string& target_frame,
const std::string& source_frame,
195 std::string* error_msg = NULL)
const;
202 bool canTransform(
const std::string& target_frame,
const std::string& source_frame,
204 std::string* error_msg = NULL)
const;
215 const std::string& source_frame,
const ros::Time& source_time,
216 const std::string& fixed_frame,
217 std::string* error_msg = NULL)
const;
230 const std::string& source_frame,
const ros::Time& source_time,
231 const std::string& fixed_frame,
233 std::string* error_msg = NULL)
const;
237 int getLatestCommonTime(
const std::string &source_frame,
const std::string &target_frame,
ros::Time& time, std::string* error_string)
const;
256 const std::string& fixed_frame,
262 const std::string& fixed_frame,
268 const std::string& fixed_frame,
274 const std::string& fixed_frame,
287 void chainAsVector(
const std::string & target_frame,
ros::Time target_time,
const std::string & source_frame,
ros::Time source_time,
const std::string & fixed_frame, std::vector<std::string>& output)
const;
306 bool getParent(
const std::string& frame_id,
ros::Time time, std::string& parent)
const;
310 bool frameExists(
const std::string& frame_id_str)
const;
369 if (!fall_back_to_wall_time_)
379 virtual bool ok()
const;
395 if(std::isnan(q.x()) || std::isnan(q.y()) || std::isnan(q.z()) || std::isnan(q.w()))
397 std::stringstream ss;
398 ss <<
"Quaternion contains a NaN" << std::endl;
402 if(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() - 1) > 0.01)
404 std::stringstream ss;
405 ss <<
"Quaternion malformed, magnitude: " << q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() <<
" should be 1.0" <<std::endl;
413 if(std::isnan(q.x) || std::isnan(q.y) || std::isnan(q.z) || std::isnan(q.w))
415 std::stringstream ss;
416 ss <<
"Quaternion contains a NaN" << std::endl;
420 if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01)
422 std::stringstream ss;
423 ss <<
"Quaternion malformed, magnitude: " << q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w <<
" should be 1.0" <<std::endl;
bool isUsingDedicatedThread() const
void assertQuaternionValid(const tf::Quaternion &q)
Throw InvalidArgument if quaternion is malformed.
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
std::string remap(const std::string &frame_id) __attribute__((deprecated))
resolve names
void setUsingDedicatedThread(bool value)
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
Return the distance between the ends of this and another vector This is symantically treating the vec...
std::string resolve(const std::string &prefix, const std::string &frame_name)
resolve tf names
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
ros::Duration getCacheLength()
tf2::InvalidArgumentException InvalidArgument
std::string strip_leading_slash(const std::string &frame_name)
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
tf::tfVector4 __attribute__