Add PoseWithCovarianceStamped transform support (ros#312)

* Adding PoseWithCovarianceStamped to tf2 and tf2_geometry_msgs.

* Adding tests for PoseWithCovarianceStamped for tf2_geometry_msgs.

* Addressing feedback and fixing WithCovarianceStamped copy constructor.

tf2/include/tf2/convert.h

@@ -31,6 +31,7 @@
 #ifndef TF2__CONVERT_H_
 #define TF2__CONVERT_H_
 
+#include <array>
 #include <string>
 
 #include "builtin_interfaces/msg/time.hpp"
@@ -72,6 +73,13 @@ tf2::TimePoint getTimestamp(const T & t);
 template<class T>
 std::string getFrameId(const T & t);
 
+/**\brief Get the covariance matrix from data
+ * \param[in] t The data input.
+ * \return The covariance matrix associated with the data.
+ */
+template<class T>
+std::array<std::array<double, 6>, 6> getCovarianceMatrix(const T & t);
+
 /**\brief Get the frame_id from data
  *
  * An implementation for Stamped<P> datatypes.
@@ -98,6 +106,19 @@ std::string getFrameId(const tf2::Stamped<P> & t)
   return t.frame_id_;
 }
 
+/**\brief Get the covariance matrix from data
+ *
+ * An implementation for WithCovarianceStamped<P> datatypes.
+ *
+ * \param[in] c The data input.
+ * \return The covariance matrix associated with the data.
+ */
+template<class P>
+std::array<std::array<double, 6>, 6> getCovarianceMatrix(const tf2::WithCovarianceStamped<P> & t)
+{
+  return t.cov_mat_;
+}
+
 /**\brief Function that converts from one type to a ROS message type. It has to be
  * implemented by each data type in tf2_* (except ROS messages) as it is
  * used in the "convert" function.
@@ -137,6 +158,48 @@ void convert(const A & a1, A & a2)
     a2 = a1;
   }
 }
+
+/**\brief Function that converts from a row-major representation of a 6x6
+ * covariance matrix to a nested array representation.
+ * \param row_major A row-major array of 36 covariance values.
+ * \return A nested array representation of 6x6 covariance values.
+ */
+inline
+std::array<std::array<double, 6>, 6> covarianceRowMajorToNested(const std::array<double, 36> & row_major)
+{
+  std::array<std::array<double, 6>, 6> nested_array = {};
+  size_t l1 = 0, l2 = 0;
+  for (const double & val : row_major) {
+    nested_array[l2][l1] = val;
+
+    l1++;
+
+    if (l1 == nested_array[0].size()) {
+      l1 = 0;
+      l2++;
+    }
+  }
+  return nested_array;
+}
+
+/**\brief Function that converts from a nested array representation of a 6x6
+ * covariance matrix to a row-major representation.
+ * \param nested_array A nested array representation of 6x6 covariance values.
+ * \return A row-major array of 36 covariance values.
+ */
+inline
+std::array<double, 36> covarianceNestedToRowMajor(const std::array<std::array<double, 6>, 6> & nested_array)
+{
+  std::array<double, 36> row_major = {};
+  size_t counter = 0;
+  for (const auto & arr : nested_array) {
+    for (const double & val : arr) {
+      row_major[counter] = val;
+      counter++;
+    }
+  }
+  return row_major;
+}
 }  // namespace tf2
 
 #endif  // TF2__CONVERT_H_

tf2/include/tf2/transform_datatypes.h

@@ -31,6 +31,7 @@
 #ifndef TF2__TRANSFORM_DATATYPES_H_
 #define TF2__TRANSFORM_DATATYPES_H_
 
+#include <array>
 #include <chrono>
 #include <string>
 
@@ -88,6 +89,67 @@ bool operator==(const Stamped<T> & a, const Stamped<T> & b)
          static_cast<const T &>(a) == static_cast<const T &>(b);
 }
 
+/** \brief The data type which will be cross compatable with geometry_msgs
+ * This is the tf2 datatype equivalent of a MessageWithCovarianceStamped */
+template<typename T>
+class WithCovarianceStamped : public T
+{
+public:
+  TimePoint stamp_;   ///< The timestamp associated with this data
+  std::string frame_id_;   ///< The frame_id associated this data
+  std::array<std::array<double, 6>, 6> cov_mat_;  ///< The covariance matrix associated with this data
+
+  /** Default constructor */
+  WithCovarianceStamped()
+  : frame_id_("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"),
+    cov_mat_{}
+  {
+  }
+
+  /** Full constructor */
+  WithCovarianceStamped(
+    const T & input,
+    const TimePoint & timestamp,
+    const std::string & frame_id,
+    const std::array<std::array<double, 6>, 6> & covariance_matrix
+  )
+  : T(input),
+    stamp_(timestamp),
+    frame_id_(frame_id),
+    cov_mat_(covariance_matrix)
+  {
+  }
+
+  /** Copy constructor */
+  WithCovarianceStamped(const WithCovarianceStamped<T> & w)
+  : T(w),
+    stamp_(w.stamp_),
+    frame_id_(w.frame_id_),
+    cov_mat_(w.cov_mat_)
+  {
+  }
+
+  /** Set the data element */
+  void setData(const T & input) {*static_cast<T *>(this) = input;}
+
+  WithCovarianceStamped & operator=(const WithCovarianceStamped<T> & w)
+  {
+    T::operator=(w);
+    this->stamp_ = w.stamp_;
+    this->frame_id_ = w.frame_id_;
+    this->cov_mat_ = w.cov_mat_;
+    return *this;
+  }
+};
+
+/** \brief Comparison operator for WithCovarianceStamped datatypes */
+template<typename T>
+bool operator==(const WithCovarianceStamped<T> & a, const WithCovarianceStamped<T> & b)
+{
+  return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ &&
+         a.cov_mat_ == b.cov_mat_ && static_cast<const T &>(a) == static_cast<const T &>(b);
+}
+
 }  // namespace tf2
 
 #endif  // TF2__TRANSFORM_DATATYPES_H_

tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h

@@ -42,6 +42,7 @@
 #include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <kdl/frames.hpp>
 
 namespace tf2
@@ -254,6 +255,122 @@ void fromMsg(const geometry_msgs::msg::PoseStamped& msg, geometry_msgs::msg::Pos
 }
 
 
+/*******************************/
+/** PoseWithCovarianceStamped **/
+/*******************************/
+
+/** \brief Extract a timestamp from the header of a Pose message.
+ * This function is a specialization of the getTimestamp template defined in tf2/convert.h.
+ * \param t PoseWithCovarianceStamped message to extract the timestamp from.
+ * \return The timestamp of the message.
+ */
+template <>
+inline
+  tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseWithCovarianceStamped& t)  {return tf2_ros::fromMsg(t.header.stamp);}
+
+/** \brief Extract a frame ID from the header of a Pose message.
+ * This function is a specialization of the getFrameId template defined in tf2/convert.h.
+ * \param t PoseWithCovarianceStamped message to extract the frame ID from.
+ * \return A string containing the frame ID of the message.
+ */
+template <>
+inline
+  std::string getFrameId(const geometry_msgs::msg::PoseWithCovarianceStamped& t)  {return t.header.frame_id;}
+
+/** \brief Extract a covariance matrix from a PoseWithCovarianceStamped message.
+ * This function is a specialization of the getCovarianceMatrix template defined in tf2/convert.h.
+ * \param t PoseWithCovarianceStamped message to extract the covariance matrix from.
+ * \return A nested-array representation of the covariance matrix from the message.
+ */
+template <>
+inline
+  std::array<std::array<double, 6>, 6> getCovarianceMatrix(const geometry_msgs::msg::PoseWithCovarianceStamped& t)  {return covarianceRowMajorToNested(t.pose.covariance);}
+
+/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type.
+ * This function is a specialization of the doTransform template defined in tf2/convert.h.
+ * \param t_in The pose to transform, as a timestamped Pose3 message with covariance.
+ * \param t_out The transformed pose, as a timestamped Pose3 message with covariance.
+ * \param transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template <>
+inline
+  void doTransform(const geometry_msgs::msg::PoseWithCovarianceStamped& t_in, geometry_msgs::msg::PoseWithCovarianceStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
+  {
+    KDL::Vector v(t_in.pose.pose.position.x, t_in.pose.pose.position.y, t_in.pose.pose.position.z);
+    KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.pose.orientation.x, t_in.pose.pose.orientation.y, t_in.pose.pose.orientation.z, t_in.pose.pose.orientation.w);
+
+    KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
+    t_out.pose.pose.position.x = v_out.p[0];
+    t_out.pose.pose.position.y = v_out.p[1];
+    t_out.pose.pose.position.z = v_out.p[2];
+    v_out.M.GetQuaternion(t_out.pose.pose.orientation.x, t_out.pose.pose.orientation.y, t_out.pose.pose.orientation.z, t_out.pose.pose.orientation.w);
+    t_out.header.stamp = transform.header.stamp;
+    t_out.header.frame_id = transform.header.frame_id;
+    t_out.pose.covariance = t_in.pose.covariance;
+  }
+
+/** \brief Trivial "conversion" function for Pose message type.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * \param in A PoseWithCovarianceStamped message.
+ * \return The input argument.
+ */
+inline
+geometry_msgs::msg::PoseWithCovarianceStamped toMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in)
+{
+  return in;
+}
+
+/** \brief Trivial "conversion" function for Pose message type.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * \param msg A PoseWithCovarianceStamped message.
+ * \param out The input argument.
+ */
+inline
+void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& msg, geometry_msgs::msg::PoseWithCovarianceStamped& out)
+{
+  out = msg;
+}
+
+/** \brief Convert a tf2 TransformWithCovarianceStamped type to its equivalent geometry_msgs representation.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * \param in A instance of the tf2::Transform specialization of the tf2::WithCovarianceStamped template.
+ * \return The TransformWithCovarianceStamped converted to a geometry_msgs PoseStamped message type.
+ */
+template<>
+inline
+geometry_msgs::msg::PoseWithCovarianceStamped toMsg(const tf2::WithCovarianceStamped<tf2::Transform>& in)
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped out;
+  out.header.stamp = tf2_ros::toMsg(in.stamp_);
+  out.header.frame_id = in.frame_id_;
+  out.pose.covariance = covarianceNestedToRowMajor(in.cov_mat_);
+  out.pose.pose.orientation.x = in.getRotation().getX();
+  out.pose.pose.orientation.y = in.getRotation().getY();
+  out.pose.pose.orientation.z = in.getRotation().getZ();
+  out.pose.pose.orientation.w = in.getRotation().getW();
+  out.pose.pose.position.x = in.getOrigin().getX();
+  out.pose.pose.position.y = in.getOrigin().getY();
+  out.pose.pose.position.z = in.getOrigin().getZ();
+  return out;
+}
+
+/** \brief Convert a PoseWithCovarianceStamped message to its equivalent tf2 representation.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * \param in A PoseWithCovarianceStamped message type.
+ * \param out The PoseWithCovarianceStamped converted to the equivalent tf2 type.
+ */
+template<>
+inline
+void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in, tf2::WithCovarianceStamped<tf2::Transform>& out)
+{
+  out.stamp_ = tf2_ros::fromMsg(in.header.stamp);
+  out.frame_id_ = in.header.frame_id;
+  out.cov_mat_ = covarianceRowMajorToNested(in.pose.covariance);
+  tf2::Transform tmp;
+  fromMsg(in.pose.pose, tmp);
+  out.setData(tmp);
+}
+
 /****************/
 /** Quaternion **/
 /****************/

tf2_geometry_msgs/scripts/test.py

@@ -5,7 +5,7 @@
 import PyKDL
 import tf2_ros
 import tf2_geometry_msgs
-from geometry_msgs.msg import TransformStamped, PointStamped, Vector3Stamped, PoseStamped, Quaternion
+from geometry_msgs.msg import TransformStamped, PointStamped, Vector3Stamped, PoseStamped, PoseWithCovarianceStamped, Quaternion
 
 class GeometryMsgs(unittest.TestCase):
     def test_transform(self):
@@ -47,6 +47,27 @@ def test_transform(self):
         self.assertEqual(out.pose.position.y, -2)
         self.assertEqual(out.pose.position.z, -3)
 
+        v = PoseWithCovarianceStamped()
+        v.header.stamp = rclpy.time.Time(seconds=2.0).to_msg()
+        v.header.frame_id = 'a'
+        v.pose.covariance = (
+          1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
+          1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
+          1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
+          1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
+          1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
+          1.0, 2.0, 3.0, 4.0, 5.0, 6.0
+        )
+        v.pose.pose.position.x = 1.0
+        v.pose.pose.position.y = 2.0
+        v.pose.pose.position.z = 3.0
+        v.pose.pose.orientation = Quaternion(w=0.0, x=1.0, y=0.0, z=0.0)
+        out = b.transform(v, 'b')
+        self.assertEqual(out.pose.pose.position.x, 0)
+        self.assertEqual(out.pose.pose.position.y, -2)
+        self.assertEqual(out.pose.pose.position.z, -3)
+        self.assertEqual(out.pose.covariance, v.pose.covariance)
+
         # Translation shouldn't affect Vector3
         t = TransformStamped()
         t.transform.translation.x = 1.0

tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py

@@ -27,7 +27,7 @@
 
 # author: Wim Meeussen
 
-from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped
+from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped, PoseWithCovarianceStamped
 import PyKDL
 import tf2_ros
 
@@ -92,3 +92,18 @@ def do_transform_pose(pose, transform):
     res.header = transform.header
     return res
 tf2_ros.TransformRegistration().add(PoseStamped, do_transform_pose)
+
+# PoseWithCovarianceStamped
+def do_transform_pose_with_covariance_stamped(pose, transform):
+    f = transform_to_kdl(transform) * PyKDL.Frame(PyKDL.Rotation.Quaternion(pose.pose.pose.orientation.x, pose.pose.pose.orientation.y,
+                                                                          pose.pose.pose.orientation.z, pose.pose.pose.orientation.w),
+                                                PyKDL.Vector(pose.pose.pose.position.x, pose.pose.pose.position.y, pose.pose.pose.position.z))
+    res = PoseWithCovarianceStamped()
+    res.pose.pose.position.x = f.p[0]
+    res.pose.pose.position.y = f.p[1]
+    res.pose.pose.position.z = f.p[2]
+    (res.pose.pose.orientation.x, res.pose.pose.orientation.y, res.pose.pose.orientation.z, res.pose.pose.orientation.w) = f.M.GetQuaternion()
+    res.pose.covariance = pose.pose.covariance
+    res.header = transform.header
+    return res
+tf2_ros.TransformRegistration().add(PoseWithCovarianceStamped, do_transform_pose_with_covariance_stamped)