first commit

2025-11-24 10:18:31 +07:00
commit 0dca929962
146 changed files with 11133 additions and 0 deletions
--- a/tf3_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h
+++ b/tf3_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TF2_SENSOR_MSGS_H
+#define TF2_SENSOR_MSGS_H
+
+#include <tf2/convert.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <Eigen/Eigen>
+#include <Eigen/Geometry>
+
+namespace tf2
+{
+
+/********************/
+/** PointCloud2    **/
+/********************/
+
+/** \brief Extract a timestamp from the header of a PointCloud2 message.
+ * This function is a specialization of the getTimestamp template defined in tf2/convert.h.
+ * \param t PointCloud2 message to extract the timestamp from.
+ * \return The timestamp of the message. The lifetime of the returned reference
+ * is bound to the lifetime of the argument.
+ */
+template <>
+inline
+const ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;}
+
+/** \brief Extract a frame ID from the header of a PointCloud2 message.
+ * This function is a specialization of the getFrameId template defined in tf2/convert.h.
+ * \param t PointCloud2 message to extract the frame ID from.
+ * \return A string containing the frame ID of the message. The lifetime of the
+ * returned reference is bound to the lifetime of the argument.
+ */
+template <>
+inline
+const std::string& getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;}
+
+// this method needs to be implemented by client library developers
+template <>
+inline
+void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in)
+{
+  p_out = p_in;
+  p_out.header = t_in.header;
+  Eigen::Transform<float,3,Eigen::Isometry> t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y,
+                                                                     t_in.transform.translation.z) * Eigen::Quaternion<float>(
+                                                                     t_in.transform.rotation.w, t_in.transform.rotation.x,
+                                                                     t_in.transform.rotation.y, t_in.transform.rotation.z);
+
+  sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z");
+
+  sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x");
+  sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y");
+  sensor_msgs::PointCloud2Iterator<float> z_out(p_out, "z");
+
+  Eigen::Vector3f point;
+  for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
+    point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);
+    *x_out = point.x();
+    *y_out = point.y();
+    *z_out = point.z();
+  }
+}
+inline
+sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in)
+{
+  return in;
+}
+inline
+void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out)
+{
+  out = msg;
+}
+
+} // namespace
+
+#endif // TF2_SENSOR_MSGS_H