Adds pose and twist with covariance messages bridging

ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp

@@ -19,17 +19,21 @@
 #include <ignition/msgs/quaternion.pb.h>
 #include <ignition/msgs/vector3d.pb.h>
 #include <ignition/msgs/pose.pb.h>
+#include <ignition/msgs/pose_with_covariance.pb.h>
 #include <ignition/msgs/pose_v.pb.h>
 #include <ignition/msgs/twist.pb.h>
+#include <ignition/msgs/twist_with_covariance.pb.h>
 #include <ignition/msgs/wrench.pb.h>
 
 // ROS 2 messages
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_with_covariance.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <geometry_msgs/msg/wrench.hpp>
 
 #include <ros_ign_bridge/convert_decl.hpp>
@@ -86,6 +90,18 @@ convert_ign_to_ros(
   const ignition::msgs::Pose & ign_msg,
   geometry_msgs::msg::Pose & ros_msg);
 
+template<>
+void
+convert_ros_to_ign(
+  const geometry_msgs::msg::PoseWithCovariance & ros_msg,
+  ignition::msgs::PoseWithCovariance & ign_msg);
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::PoseWithCovariance & ign_msg,
+  geometry_msgs::msg::PoseWithCovariance & ros_msg);
+
 template<>
 void
 convert_ros_to_ign(
@@ -134,6 +150,18 @@ convert_ign_to_ros(
   const ignition::msgs::Twist & ign_msg,
   geometry_msgs::msg::Twist & ros_msg);
 
+template<>
+void
+convert_ros_to_ign(
+  const geometry_msgs::msg::TwistWithCovariance & ros_msg,
+  ignition::msgs::TwistWithCovariance & ign_msg);
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::TwistWithCovariance & ign_msg,
+  geometry_msgs::msg::TwistWithCovariance & ros_msg);
+
 template<>
 void
 convert_ros_to_ign(

ros_ign_bridge/include/ros_ign_bridge/convert/nav_msgs.hpp

@@ -17,6 +17,7 @@
 
 // Ignition messages
 #include <ignition/msgs/odometry.pb.h>
+#include <ignition/msgs/odometry_with_covariance.pb.h>
 
 // ROS 2 messages
 #include <nav_msgs/msg/odometry.hpp>
@@ -38,6 +39,18 @@ convert_ign_to_ros(
   const ignition::msgs::Odometry & ign_msg,
   nav_msgs::msg::Odometry & ros_msg);
 
+template<>
+void
+convert_ros_to_ign(
+  const nav_msgs::msg::Odometry & ros_msg,
+  ignition::msgs::OdometryWithCovariance & ign_msg);
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::OdometryWithCovariance & ign_msg,
+  nav_msgs::msg::Odometry & ros_msg);
+
 }  // namespace ros_ign_bridge
 
 #endif  // ROS_IGN_BRIDGE__CONVERT__NAV_MSGS_HPP_

ros_ign_bridge/src/convert/geometry_msgs.cpp

@@ -106,6 +106,34 @@ convert_ign_to_ros(
   convert_ign_to_ros(ign_msg.orientation(), ros_msg.orientation);
 }
 
+template<>
+void
+convert_ros_to_ign(
+  const geometry_msgs::msg::PoseWithCovariance & ros_msg,
+  ignition::msgs::PoseWithCovariance & ign_msg)
+{
+  convert_ros_to_ign(ros_msg.pose.position, *ign_msg.mutable_pose()->mutable_position());  
+  convert_ros_to_ign(ros_msg.pose.orientation, *ign_msg.mutable_pose()->mutable_orientation());
+  for (const auto & elem : ros_msg.covariance) {
+    ign_msg.mutable_covariance()->add_data(elem);
+  }
+}
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::PoseWithCovariance & ign_msg,
+  geometry_msgs::msg::PoseWithCovariance & ros_msg)
+{
+  convert_ign_to_ros(ign_msg.pose().position(), ros_msg.pose.position);
+  convert_ign_to_ros(ign_msg.pose().orientation(), ros_msg.pose.orientation);
+  int data_size = ign_msg.covariance().data_size();
+  for (int i = 0 ; i < data_size ; i++) {
+    auto data = ign_msg.covariance().data()[i];
+    ros_msg.covariance[i] = data;
+  }
+}
+
 template<>
 void
 convert_ros_to_ign(
@@ -197,6 +225,34 @@ convert_ign_to_ros(
   convert_ign_to_ros(ign_msg.angular(), ros_msg.angular);
 }
 
+template<>
+void
+convert_ros_to_ign(
+  const geometry_msgs::msg::TwistWithCovariance & ros_msg,
+  ignition::msgs::TwistWithCovariance & ign_msg)
+{
+  convert_ros_to_ign(ros_msg.twist.linear, (*ign_msg.mutable_twist()->mutable_linear()));
+  convert_ros_to_ign(ros_msg.twist.angular, (*ign_msg.mutable_twist()->mutable_angular()));
+  for (const auto & elem : ros_msg.covariance) {
+    ign_msg.mutable_covariance()->add_data(elem);
+  }
+}
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::TwistWithCovariance & ign_msg,
+  geometry_msgs::msg::TwistWithCovariance & ros_msg)
+{
+  convert_ign_to_ros(ign_msg.twist().linear(), ros_msg.twist.linear);
+  convert_ign_to_ros(ign_msg.twist().angular(), ros_msg.twist.angular);
+  int data_size = ign_msg.covariance().data_size();
+  for (int i = 0 ; i < data_size ; i++) {
+    auto data = ign_msg.covariance().data()[i];
+    ros_msg.covariance[i] = data;
+  }
+}
+
 template<>
 void
 convert_ros_to_ign(

ros_ign_bridge/src/convert/nav_msgs.cpp

@@ -52,4 +52,38 @@ convert_ign_to_ros(
   }
 }
 
+template<>
+void
+convert_ros_to_ign(
+  const nav_msgs::msg::Odometry & ros_msg,
+  ignition::msgs::OdometryWithCovariance & ign_msg)
+{
+  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
+  convert_ros_to_ign(ros_msg.pose, (*ign_msg.mutable_pose_with_covariance()));
+  convert_ros_to_ign(ros_msg.twist, (*ign_msg.mutable_twist_with_covariance()));
+
+  auto childFrame = ign_msg.mutable_header()->add_data();
+  childFrame->set_key("child_frame_id");
+  childFrame->add_value(ros_msg.child_frame_id);
+}
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::OdometryWithCovariance & ign_msg,
+  nav_msgs::msg::Odometry & ros_msg)
+{
+  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
+  convert_ign_to_ros(ign_msg.pose_with_covariance(), ros_msg.pose);
+  convert_ign_to_ros(ign_msg.twist_with_covariance(), ros_msg.twist);
+
+  for (auto i = 0; i < ign_msg.header().data_size(); ++i) {
+    auto aPair = ign_msg.header().data(i);
+    if (aPair.key() == "child_frame_id" && aPair.value_size() > 0) {
+      ros_msg.child_frame_id = frame_id_ign_to_ros(aPair.value(0));
+      break;
+    }
+  }
+}
+
 }  // namespace ros_ign_bridge

ros_ign_bridge/src/factories/geometry_msgs.cpp

@@ -68,6 +68,16 @@ get_factory__geometry_msgs(
       >
     >("geometry_msgs/msg/Pose", ign_type_name);
   }
+  if ((ros_type_name == "geometry_msgs/msg/PoseWithCovariance" || ros_type_name.empty()) &&
+    ign_type_name == "ignition.msgs.PoseWithCovariance")
+  {
+    return std::make_shared<
+      Factory<
+        geometry_msgs::msg::PoseWithCovariance,
+        ignition::msgs::PoseWithCovariance
+      >
+    >("geometry_msgs/msg/PoseWithCovariance", ign_type_name);
+  }
   if ((ros_type_name == "geometry_msgs/msg/PoseStamped" || ros_type_name.empty()) &&
     ign_type_name == "ignition.msgs.Pose")
   {
@@ -109,6 +119,17 @@ get_factory__geometry_msgs(
       >
     >("geometry_msgs/msg/Twist", ign_type_name);
   }
+  if (
+    (ros_type_name == "geometry_msgs/msg/TwistWithCovariance" || ros_type_name.empty()) &&
+    ign_type_name == "ignition.msgs.TwistWithCovariance")
+  {
+    return std::make_shared<
+      Factory<
+        geometry_msgs::msg::TwistWithCovariance,
+        ignition::msgs::TwistWithCovariance
+      >
+    >("geometry_msgs/msg/TwistWithCovariance", ign_type_name);
+  }
   if (
     (ros_type_name == "geometry_msgs/msg/Wrench" || ros_type_name.empty()) &&
     ign_type_name == "ignition.msgs.Wrench")
@@ -219,6 +240,30 @@ Factory<
   ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
 }
 
+template<>
+void
+Factory<
+  geometry_msgs::msg::PoseWithCovariance,
+  ignition::msgs::PoseWithCovariance
+>::convert_ros_to_ign(
+  const geometry_msgs::msg::PoseWithCovariance & ros_msg,
+  ignition::msgs::PoseWithCovariance & ign_msg)
+{
+  ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg);
+}
+
+template<>
+void
+Factory<
+  geometry_msgs::msg::PoseWithCovariance,
+  ignition::msgs::PoseWithCovariance
+>::convert_ign_to_ros(
+  const ignition::msgs::PoseWithCovariance & ign_msg,
+  geometry_msgs::msg::PoseWithCovariance & ros_msg)
+{
+  ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
+}
+
 template<>
 void
 Factory<
@@ -315,6 +360,30 @@ Factory<
   ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
 }
 
+template<>
+void
+Factory<
+  geometry_msgs::msg::TwistWithCovariance,
+  ignition::msgs::TwistWithCovariance
+>::convert_ros_to_ign(
+  const geometry_msgs::msg::TwistWithCovariance & ros_msg,
+  ignition::msgs::TwistWithCovariance & ign_msg)
+{
+  ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg);
+}
+
+template<>
+void
+Factory<
+  geometry_msgs::msg::TwistWithCovariance,
+  ignition::msgs::TwistWithCovariance
+>::convert_ign_to_ros(
+  const ignition::msgs::TwistWithCovariance & ign_msg,
+  geometry_msgs::msg::TwistWithCovariance & ros_msg)
+{
+  ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
+}
+
 template<>
 void
 Factory<

ros_ign_bridge/src/factories/nav_msgs.cpp

@@ -38,6 +38,16 @@ get_factory__nav_msgs(
       >
     >("nav_msgs/msg/Odometry", ign_type_name);
   }
+  if ((ros_type_name == "nav_msgs/msg/Odometry" || ros_type_name.empty()) &&
+    ign_type_name == "ignition.msgs.OdometryWithCovariance")
+  {
+    return std::make_shared<
+      Factory<
+        nav_msgs::msg::Odometry,
+        ignition::msgs::OdometryWithCovariance
+      >
+    >("nav_msgs/msg/Odometry", ign_type_name);
+  }
   return nullptr;
 }
 
@@ -65,4 +75,28 @@ Factory<
   ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
 }
 
+template<>
+void
+Factory<
+  nav_msgs::msg::Odometry,
+  ignition::msgs::OdometryWithCovariance
+>::convert_ros_to_ign(
+  const nav_msgs::msg::Odometry & ros_msg,
+  ignition::msgs::OdometryWithCovariance & ign_msg)
+{
+  ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg);
+}
+
+template<>
+void
+Factory<
+  nav_msgs::msg::Odometry,
+  ignition::msgs::OdometryWithCovariance
+>::convert_ign_to_ros(
+  const ignition::msgs::OdometryWithCovariance & ign_msg,
+  nav_msgs::msg::Odometry & ros_msg)
+{
+  ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
+}
+
 }  // namespace ros_ign_bridge

ros_ign_bridge/test/launch/test_ign_subscriber.launch.py

@@ -53,6 +53,7 @@ def generate_test_description():
           '/clock@rosgraph_msgs/msg/[email protected]',
           '/point@geometry_msgs/msg/[email protected]',
           '/pose@geometry_msgs/msg/[email protected]',
+          '/pose_with_covariance@geometry_msgs/msg/[email protected]',
           '/pose_stamped@geometry_msgs/msg/[email protected]',
           '/transform@geometry_msgs/msg/[email protected]',
           '/tf2_message@tf2_msgs/msg/[email protected]_V',

ros_ign_bridge/test/launch/test_ros_subscriber.launch.py

@@ -53,6 +53,7 @@ def generate_test_description():
           '/clock@rosgraph_msgs/msg/[email protected]',
           '/point@geometry_msgs/msg/[email protected]',
           '/pose@geometry_msgs/msg/[email protected]',
+          '/pose_with_covariance@geometry_msgs/msg/[email protected]',
           '/pose_stamped@geometry_msgs/msg/[email protected]',
           '/transform@geometry_msgs/msg/[email protected]',
           '/tf2_message@tf2_msgs/msg/[email protected]_V',

ros_ign_bridge/test/publishers/ign_publisher.cpp

@@ -119,6 +119,11 @@ int main(int /*argc*/, char **/*argv*/)
   ignition::msgs::Pose pose_msg;
   ros_ign_bridge::testing::createTestMsg(pose_msg);
 
+  // ignition::msgs::PoseWithCovariance.
+  auto pose_cov_pub = node.Advertise<ignition::msgs::PoseWithCovariance>("pose_with_covariance");
+  ignition::msgs::PoseWithCovariance pose_cov_msg;
+  ros_ign_bridge::testing::createTestMsg(pose_cov_msg);
+
   // ignition::msgs::PoseStamped.
   auto pose_stamped_pub = node.Advertise<ignition::msgs::Pose>("pose_stamped");
   ignition::msgs::Pose pose_stamped_msg;
@@ -274,6 +279,7 @@ int main(int /*argc*/, char **/*argv*/)
     clock_pub.Publish(clock_msg);
     point_pub.Publish(point_msg);
     pose_pub.Publish(pose_msg);
+    pose_cov_pub.Publish(pose_cov_msg);
     pose_stamped_pub.Publish(pose_stamped_msg);
     transform_pub.Publish(transform_msg);
     transform_stamped_pub.Publish(transform_stamped_msg);

ros_ign_bridge/test/publishers/ros_publisher.cpp

@@ -97,6 +97,13 @@ int main(int argc, char ** argv)
   geometry_msgs::msg::Pose pose_msg;
   ros_ign_bridge::testing::createTestMsg(pose_msg);
 
+  // geometry_msgs::msg::PoseWithCovariance.
+  auto pose_cov_pub =
+    node->create_publisher<geometry_msgs::msg::PoseWithCovariance>(
+      "pose_with_covariance", 1000);
+  geometry_msgs::msg::PoseWithCovariance pose_cov_msg;
+  ros_ign_bridge::testing::createTestMsg(pose_cov_msg);
+
   // geometry_msgs::msg::PoseStamped.
   auto pose_stamped_pub =
     node->create_publisher<geometry_msgs::msg::PoseStamped>("pose_stamped", 1000);
@@ -274,6 +281,7 @@ int main(int argc, char ** argv)
     clock_pub->publish(clock_msg);
     point_pub->publish(point_msg);
     pose_pub->publish(pose_msg);
+    pose_cov_pub->publish(pose_cov_msg);
     pose_stamped_pub->publish(pose_stamped_msg);
     transform_pub->publish(transform_msg);
     transform_stamped_pub->publish(transform_stamped_msg);