58 return cache->
getParent(time, error_string);
75 : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
76 , source_to_top_vec(0.0, 0.0, 0.0)
77 , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
78 , target_to_top_vec(0.0, 0.0, 0.0)
79 , result_quat(0.0, 0.0, 0.0, 1.0)
80 , result_vec(0.0, 0.0, 0.0)
86 if (!cache->
getData(time, st, error_string))
98 source_to_top_vec =
quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
99 source_to_top_quat = st.rotation_ * source_to_top_quat;
103 target_to_top_vec =
quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
104 target_to_top_quat = st.rotation_ * target_to_top_quat;
115 result_vec = source_to_top_vec;
116 result_quat = source_to_top_quat;
121 tf::Vector3 inv_target_vec =
quatRotate(inv_target_quat, -target_to_top_vec);
122 result_vec = inv_target_vec;
123 result_quat = inv_target_quat;
129 tf::Vector3 inv_target_vec =
quatRotate(inv_target_quat, -target_to_top_vec);
131 result_vec =
quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
132 result_quat = inv_target_quat * source_to_top_quat;
154 ROS_DEBUG(
"tf::assert_resolved just calls tf::resolve");
158 std::string
tf::resolve(
const std::string& prefix,
const std::string& frame_name)
161 if (frame_name.size() > 0)
162 if (frame_name[0] ==
'/')
166 if (prefix.size() > 0)
168 if (prefix[0] ==
'/')
171 composite.append(
"/");
172 composite.append(frame_name);
177 std::string composite;
178 composite.append(prefix);
179 composite.append(
"/");
180 composite.append(frame_name);
187 std::string composite;
188 composite.append(frame_name);
196 if (frame_name.size() > 0)
197 if (frame_name[0] ==
'/')
199 std::string shorter = frame_name;
210 fall_back_to_wall_time_(false),
211 tf2_buffer_ptr_(
std::make_shared<
tf2_ros::Buffer>(cache_time))
230 geometry_msgs::TransformStamped msgtf;
240 geometry_msgs::TransformStamped output =
251 geometry_msgs::TransformStamped output =
261 geometry_msgs::Twist& twist)
const 264 lookupTwist(tracking_frame, observation_frame, observation_frame,
tf::Point(0,0,0), tracking_frame, time, averaging_interval, twist);
269 void Transformer::lookupTwist(
const std::string& tracking_frame,
const std::string& observation_frame,
const std::string& reference_frame,
270 const tf::Point & reference_point,
const std::string& reference_point_frame,
272 geometry_msgs::Twist& twist)
const 279 target_time = latest_time;
283 ros::Time end_time = std::min(target_time + averaging_interval *0.5 , latest_time);
285 ros::Time start_time = std::max(
ros::Time().fromSec(.00001) + averaging_interval, end_time) - averaging_interval;
286 ros::Duration corrected_averaging_interval = end_time - start_time;
303 tf::Vector3 twist_vel ((delta_x)/corrected_averaging_interval.
toSec(),
304 (delta_y)/corrected_averaging_interval.
toSec(),
305 (delta_z)/corrected_averaging_interval.
toSec());
306 tf::Vector3 twist_rot = o * (ang / corrected_averaging_interval.
toSec());
315 tf::Vector3 out_rot = inverse.getBasis() * twist_rot;
316 tf::Vector3 out_vel = inverse.getBasis()* twist_vel + inverse.getOrigin().cross(out_rot);
329 out_vel = out_vel + out_rot * delta;
338 twist.linear.x = out_vel.x();
339 twist.linear.y = out_vel.y();
340 twist.linear.z = out_vel.z();
341 twist.angular.x = out_rot.x();
342 twist.angular.y = out_rot.y();
343 twist.angular.z = out_rot.z();
350 std::string* error_msg)
const 358 const ros::Time& time, std::string* error_msg)
const 366 const ros::Time& source_time,
const std::string& fixed_frame,
367 std::string* error_msg)
const 375 const ros::Time& source_time,
const std::string& fixed_frame,
377 std::string* error_msg)
const 398 ROS_WARN(
"Transformer::setExtrapolationLimit is deprecated and does not do anything");
410 return rhs.second == id;
421 return tf2_buffer_ptr_->_getLatestCommonTime(source_id, target_id, time, error_string);
429 source_frame, source_time,
430 fixed_frame, output);
459 stamped_out.
setData( transform * stamped_in);
473 tf::Vector3 end = stamped_in;
474 tf::Vector3 origin = tf::Vector3(0,0,0);
475 tf::Vector3 output = (transform * end) - (transform * origin);
488 stamped_out.
setData(transform * stamped_in);
498 stamped_out.
setData(transform * stamped_in);
506 const std::string& fixed_frame,
513 fixed_frame, transform);
515 stamped_out.
setData( transform * stamped_in);
523 const std::string& fixed_frame,
529 fixed_frame, transform);
532 tf::Vector3 end = stamped_in;
533 tf::Vector3 origin = tf::Vector3(0,0,0);
534 tf::Vector3 output = (transform * end) - (transform * origin);
544 const std::string& fixed_frame,
550 fixed_frame, transform);
552 stamped_out.
setData(transform * stamped_in);
559 const std::string& fixed_frame,
565 fixed_frame, transform);
567 stamped_out.
setData(transform * stamped_in);
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)
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Quaternion inverse() const
Return the inverse of this quaternion.
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...
Matrix3x3 inverse() const
Return the inverse of the matrix.
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data...
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
bool operator()(const P_TimeAndFrameID &rhs) const
std::string resolve(const std::string &prefix, const std::string &frame_name)
resolve tf names
Vector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
Return the inverse of a quaternion.
TFSIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
CompactFrameID getParent(ros::Time time, std::string *error_str)
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.
std::string frame_id_
The frame_id associated this data.
tfScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
convert tf::StampedTransform to TransformStamped msg
static void transformStampedMsgToTF(const geometry_msgs::TransformStamped &msg, StampedTransform &bt)
convert TransformStamped msg to tf::StampedTransform