59 return cache->
getParent(time, error_string);
76 : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
77 , source_to_top_vec(0.0, 0.0, 0.0)
78 , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
79 , target_to_top_vec(0.0, 0.0, 0.0)
80 , result_quat(0.0, 0.0, 0.0, 1.0)
81 , result_vec(0.0, 0.0, 0.0)
87 if (!cache->
getData(time, st, error_string))
99 source_to_top_vec =
quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
100 source_to_top_quat = st.rotation_ * source_to_top_quat;
104 target_to_top_vec =
quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
105 target_to_top_quat = st.rotation_ * target_to_top_quat;
116 result_vec = source_to_top_vec;
117 result_quat = source_to_top_quat;
123 result_vec = inv_target_vec;
124 result_quat = inv_target_quat;
132 result_vec =
quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
133 result_quat = inv_target_quat * source_to_top_quat;
155 ROS_DEBUG(
"tf::assert_resolved just calls tf::resolve");
159 std::string
tf::resolve(
const std::string& prefix,
const std::string& frame_name)
162 if (frame_name.size() > 0)
163 if (frame_name[0] ==
'/')
167 if (prefix.size() > 0)
169 if (prefix[0] ==
'/')
172 composite.append(
"/");
173 composite.append(frame_name);
178 std::string composite;
179 composite.append(prefix);
180 composite.append(
"/");
181 composite.append(frame_name);
188 std::string composite;
189 composite.append(frame_name);
197 if (frame_name.size() > 0)
198 if (frame_name[0] ==
'/')
200 std::string shorter = frame_name;
211 fall_back_to_wall_time_(false),
212 tf2_buffer_(cache_time)
231 geometry_msgs::TransformStamped msgtf;
241 geometry_msgs::TransformStamped output =
252 geometry_msgs::TransformStamped output =
262 geometry_msgs::Twist& twist)
const 265 lookupTwist(tracking_frame, observation_frame, observation_frame,
tf::Point(0,0,0), tracking_frame, time, averaging_interval, twist);
270 void Transformer::lookupTwist(
const std::string& tracking_frame,
const std::string& observation_frame,
const std::string& reference_frame,
271 const tf::Point & reference_point,
const std::string& reference_point_frame,
273 geometry_msgs::Twist& twist)
const 280 target_time = latest_time;
284 ros::Time end_time = std::min(target_time + averaging_interval *0.5 , latest_time);
286 ros::Time start_time = std::max(
ros::Time().fromSec(.00001) + averaging_interval, end_time) - averaging_interval;
287 ros::Duration corrected_averaging_interval = end_time - start_time;
305 (delta_y)/corrected_averaging_interval.
toSec(),
306 (delta_z)/corrected_averaging_interval.
toSec());
316 tf::Vector3 out_rot = inverse.getBasis() * twist_rot;
317 tf::Vector3 out_vel = inverse.getBasis()* twist_vel + inverse.getOrigin().
cross(out_rot);
330 out_vel = out_vel + out_rot * delta;
339 twist.linear.
x = out_vel.
x();
340 twist.linear.y = out_vel.
y();
341 twist.linear.z = out_vel.
z();
342 twist.angular.x = out_rot.
x();
343 twist.angular.y = out_rot.
y();
344 twist.angular.z = out_rot.
z();
351 std::string* error_msg)
const 359 const ros::Time& time, std::string* error_msg)
const 367 const ros::Time& source_time,
const std::string& fixed_frame,
368 std::string* error_msg)
const 376 const ros::Time& source_time,
const std::string& fixed_frame,
378 std::string* error_msg)
const 399 ROS_WARN(
"Transformer::setExtrapolationLimit is deprecated and does not do anything");
411 return rhs.second == id;
430 source_frame, source_time,
431 fixed_frame, output);
460 stamped_out.
setData( transform * stamped_in);
476 tf::Vector3 output = (transform * end) - (transform * origin);
489 stamped_out.
setData(transform * stamped_in);
499 stamped_out.
setData(transform * stamped_in);
507 const std::string& fixed_frame,
514 fixed_frame, transform);
516 stamped_out.
setData( transform * stamped_in);
524 const std::string& fixed_frame,
530 fixed_frame, transform);
535 tf::Vector3 output = (transform * end) - (transform * origin);
545 const std::string& fixed_frame,
551 fixed_frame, transform);
553 stamped_out.
setData(transform * stamped_in);
560 const std::string& fixed_frame,
566 fixed_frame, transform);
568 stamped_out.
setData(transform * stamped_in);
bool _getParent(const std::string &frame_id, ros::Time time, std::string &parent) const
bool operator()(const P_TimeAndFrameID &rhs) const
void assertQuaternionValid(const tf::Quaternion &q)
Throw InvalidArgument if quaternion is malformed.
std::string assert_resolved(const std::string &prefix, const std::string &frame_id)
tfScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
std::string allFramesAsString() const
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
TimeAndFrameIDFrameComparator(CompactFrameID id)
void setData(const T &input)
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
boost::signals2::connection _addTransformsChangedListener(boost::function< void(void)> callback)
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
TFSIMD_FORCE_INLINE const tfScalar & getY() const
Return the y 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...
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data...
Matrix3x3 inverse() const
Return the inverse of the matrix.
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
Return the z value.
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
Return the cross product between this and another vector.
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
std::string resolve(const std::string &prefix, const std::string &frame_name)
resolve tf names
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
void _removeTransformsChangedListener(boost::signals2::connection c)
bool _frameExists(const std::string &frame_id_str) const
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
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
TFSIMD_FORCE_INLINE Vector3()
No initialization constructor.
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
Return the inverse of a quaternion.
Vector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
TFSIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
CompactFrameID _lookupFrameNumber(const std::string &frameid_str) const
CompactFrameID getParent(ros::Time time, std::string *error_str)
int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time &time, std::string *error_string) const
Quaternion inverse() const
Return the inverse of this quaternion.
bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
std::string strip_leading_slash(const std::string &frame_name)
ros::Time stamp_
The timestamp associated with this data.
void _getFrameStrings(std::vector< std::string > &ids) const
std::string frame_id_
The frame_id associated this data.
TFSIMD_FORCE_INLINE const tfScalar & getX() const
Return the x value.
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &target_time, const ros::Duration timeout, std::string *errstr=NULL) const
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
convert tf::StampedTransform to TransformStamped msg
std::string _allFramesAsDot(double current_time) const
static void transformStampedMsgToTF(const geometry_msgs::TransformStamped &msg, StampedTransform &bt)
convert TransformStamped msg to tf::StampedTransform
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...