66 const geometry_msgs::QuaternionStamped& msg_in,
67 geometry_msgs::QuaternionStamped& msg_out)
const 78 const geometry_msgs::Vector3Stamped& msg_in,
79 geometry_msgs::Vector3Stamped& msg_out)
const 88 const geometry_msgs::PointStamped& msg_in,
89 geometry_msgs::PointStamped& msg_out)
const 98 const geometry_msgs::PoseStamped& msg_in,
99 geometry_msgs::PoseStamped& msg_out)
const 142 const geometry_msgs::QuaternionStamped& msg_in,
143 const std::string& fixed_frame, geometry_msgs::QuaternionStamped& msg_out)
const 153 const geometry_msgs::Vector3Stamped& msg_in,
154 const std::string& fixed_frame, geometry_msgs::Vector3Stamped& msg_out)
const 163 const geometry_msgs::PointStamped& msg_in,
164 const std::string& fixed_frame, geometry_msgs::PointStamped& msg_out)
const 168 transformPoint(target_frame, target_time, pin, fixed_frame, pout);
173 const geometry_msgs::PoseStamped& msg_in,
174 const std::string& fixed_frame, geometry_msgs::PoseStamped& msg_out)
const 180 transformPose(target_frame, target_time, pin, fixed_frame, pout);
187 lookupTransform(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp, transform);
192 const sensor_msgs::PointCloud& cloudIn,
193 const std::string& fixed_frame, sensor_msgs::PointCloud& cloudOut)
const 197 cloudIn.header.frame_id, cloudIn.header.stamp,
209 double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x();
210 double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y();
211 double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z();
213 out.x = x; out.y = y; out.z = z;
218 const ros::Time& target_time,
const sensor_msgs::PointCloud & cloudIn,
219 sensor_msgs::PointCloud & cloudOut)
const 221 tf::Vector3 origin = net_transform.
getOrigin();
224 unsigned int length = cloudIn.points.size();
227 if (&cloudIn != &cloudOut)
229 cloudOut.header = cloudIn.header;
230 cloudOut.points.resize(length);
231 cloudOut.channels.resize(cloudIn.channels.size());
232 for (
unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i)
233 cloudOut.channels[i] = cloudIn.channels[i];
237 cloudOut.header.stamp = target_time;
238 cloudOut.header.frame_id = target_frame;
239 for (
unsigned int i = 0; i <
length ; i++) {
void assertQuaternionValid(const tf::Quaternion &q)
Throw InvalidArgument if quaternion is malformed.
std::string getPrefixParam(ros::NodeHandle &nh)
Get the tf_prefix from the parameter server.
static ROS_DEPRECATED std::string remap(const std::string &prefix, const std::string &frame_name)
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
convert Stamped<Pose> to PoseStamped msg
static void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped &msg, Stamped< Quaternion > &bt)
convert QuaternionStamped msg to Stamped<Quaternion>
static void vector3StampedTFToMsg(const Stamped< Vector3 > &bt, geometry_msgs::Vector3Stamped &msg)
convert Stamped<Vector3> to Vector3Stamped msg
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
std::string resolve(const std::string &prefix, const std::string &frame_name)
resolve tf names
static void pointStampedMsgToTF(const geometry_msgs::PointStamped &msg, Stamped< Point > &bt)
convert PointStamped msg to Stamped<Point>
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
static void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped &msg, Stamped< Vector3 > &bt)
convert Vector3Stamped msg to Stamped<Vector3>
static void pointStampedTFToMsg(const Stamped< Point > &bt, geometry_msgs::PointStamped &msg)
convert Stamped<Point> to PointStamped msg
static void quaternionStampedTFToMsg(const Stamped< Quaternion > &bt, geometry_msgs::QuaternionStamped &msg)
convert Stamped<Quaternion> to QuaternionStamped msg
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
convert PoseStamped msg to Stamped<Pose>
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
Return the length of a quaternion.