[forwardport rolling] Added conversion for Detection3D and Detection3DArray (#523)

ros_gz_bridge/README.md

@@ -69,7 +69,8 @@ The following message types can be bridged for topics:
 | trajectory_msgs/msg/JointTrajectory            | gz.msgs.JointTrajectory             |
 | vision_msgs/msg/Detection2D                    | gz.msgs.AnnotatedAxisAligned2DBox   |
 | vision_msgs/msg/Detection2DArray               | gz.msgs.AnnotatedAxisAligned2DBox_V |
-
+| vision_msgs/msg/Detection3D                    | gz::msgs::AnnotatedOriented3DBox    |
+| vision_msgs/msg/Detection3DArray               | gz::msgs::AnnotatedOriented3DBox_V  |
 
 And the following for services:
 

ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp

@@ -17,9 +17,11 @@
 
 // Gazebo Msgs
 #include <gz/msgs/annotated_axis_aligned_2d_box_v.pb.h>
+#include <gz/msgs/annotated_oriented_3d_box_v.pb.h>
 
 // ROS 2 messages
 #include "vision_msgs/msg/detection2_d_array.hpp"
+#include "vision_msgs/msg/detection3_d_array.hpp"
 #include <ros_gz_bridge/convert_decl.hpp>
 
 namespace ros_gz_bridge
@@ -47,6 +49,30 @@ void
 convert_gz_to_ros(
   const gz::msgs::AnnotatedAxisAligned2DBox_V & gz_msg,
   vision_msgs::msg::Detection2DArray & ros_msg);
+
+template<>
+void
+convert_ros_to_gz(
+  const vision_msgs::msg::Detection3D & ros_msg,
+  gz::msgs::AnnotatedOriented3DBox & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::AnnotatedOriented3DBox & gz_msg,
+  vision_msgs::msg::Detection3D & ros_msg);
+
+template<>
+void
+convert_ros_to_gz(
+  const vision_msgs::msg::Detection3DArray & ros_msg,
+  gz::msgs::AnnotatedOriented3DBox_V & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::AnnotatedOriented3DBox_V & gz_msg,
+  vision_msgs::msg::Detection3DArray & ros_msg);
 }  // namespace ros_gz_bridge
 
 #endif  // ROS_GZ_BRIDGE__CONVERT__VISION_MSGS_HPP_

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -111,5 +111,7 @@
     'vision_msgs': [
         Mapping('Detection2DArray', 'AnnotatedAxisAligned2DBox_V'),
         Mapping('Detection2D', 'AnnotatedAxisAligned2DBox'),
+        Mapping('Detection3DArray', 'AnnotatedOriented3DBox_V'),
+        Mapping('Detection3D', 'AnnotatedOriented3DBox'),
     ],
 }

ros_gz_bridge/src/convert/vision_msgs.cpp

@@ -94,4 +94,89 @@ convert_gz_to_ros(
   }
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const vision_msgs::msg::Detection3D & ros_msg,
+  gz::msgs::AnnotatedOriented3DBox & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+
+  gz::msgs::Oriented3DBox * box = new gz::msgs::Oriented3DBox();
+  gz::msgs::Vector3d * center = new gz::msgs::Vector3d();
+  gz::msgs::Vector3d * box_size = new gz::msgs::Vector3d();
+  gz::msgs::Quaternion * orientation = new gz::msgs::Quaternion();
+
+  if (ros_msg.results.size() != 0) {
+    auto id = ros_msg.results.at(0).hypothesis.class_id;
+    gz_msg.set_label(std::stoi(id));
+  }
+
+  center->set_x(ros_msg.bbox.center.position.x);
+  center->set_y(ros_msg.bbox.center.position.y);
+  center->set_z(ros_msg.bbox.center.position.z);
+  box_size->set_x(ros_msg.bbox.size.x);
+  box_size->set_y(ros_msg.bbox.size.y);
+  box_size->set_z(ros_msg.bbox.size.z);
+  orientation->set_x(ros_msg.bbox.center.orientation.x);
+  orientation->set_y(ros_msg.bbox.center.orientation.y);
+  orientation->set_z(ros_msg.bbox.center.orientation.z);
+  orientation->set_w(ros_msg.bbox.center.orientation.w);
+  box->set_allocated_center(center);
+  box->set_allocated_boxsize(box_size);
+  box->set_allocated_orientation(orientation);
+  gz_msg.set_allocated_box(box);
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::AnnotatedOriented3DBox & gz_msg,
+  vision_msgs::msg::Detection3D & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+
+  ros_msg.results.resize(1);
+  ros_msg.results.at(0).hypothesis.class_id = std::to_string(gz_msg.label());
+  ros_msg.results.at(0).hypothesis.score = 1.0;
+
+  ros_msg.bbox.center.position.x = gz_msg.box().center().x();
+  ros_msg.bbox.center.position.y = gz_msg.box().center().y();
+  ros_msg.bbox.center.position.z = gz_msg.box().center().z();
+  ros_msg.bbox.size.x = gz_msg.box().boxsize().x();
+  ros_msg.bbox.size.y = gz_msg.box().boxsize().y();
+  ros_msg.bbox.size.z = gz_msg.box().boxsize().z();
+  ros_msg.bbox.center.orientation.x = gz_msg.box().orientation().x();
+  ros_msg.bbox.center.orientation.y = gz_msg.box().orientation().y();
+  ros_msg.bbox.center.orientation.z = gz_msg.box().orientation().z();
+  ros_msg.bbox.center.orientation.w = gz_msg.box().orientation().w();
+}
+
+template<>
+void
+convert_ros_to_gz(
+  const vision_msgs::msg::Detection3DArray & ros_msg,
+  gz::msgs::AnnotatedOriented3DBox_V & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+  for (const auto & ros_box : ros_msg.detections) {
+    gz::msgs::AnnotatedOriented3DBox * gz_box = gz_msg.add_annotated_box();
+    convert_ros_to_gz(ros_box, *gz_box);
+  }
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::AnnotatedOriented3DBox_V & gz_msg,
+  vision_msgs::msg::Detection3DArray & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  for (const auto & gz_box : gz_msg.annotated_box()) {
+    vision_msgs::msg::Detection3D ros_box;
+    convert_gz_to_ros(gz_box, ros_box);
+    ros_msg.detections.push_back(ros_box);
+  }
+}
+
 }  // namespace ros_gz_bridge

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <cstddef>
 
 #if GZ_MSGS_MAJOR_VERSION >= 10
 #define GZ_MSGS_IMU_HAS_COVARIANCE
@@ -1557,5 +1558,92 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedAxisAligned2DBox_V>
   }
 }
 
+void createTestMsg(gz::msgs::AnnotatedOriented3DBox & _msg)
+{
+  gz::msgs::Header header_msg;
+
+  createTestMsg(header_msg);
+
+  _msg.mutable_header()->CopyFrom(header_msg);
+
+  _msg.set_label(1);
+
+  gz::msgs::Oriented3DBox * box = new gz::msgs::Oriented3DBox();
+  gz::msgs::Vector3d * center = new gz::msgs::Vector3d();
+  gz::msgs::Vector3d * size = new gz::msgs::Vector3d();
+  gz::msgs::Quaternion * orientation = new gz::msgs::Quaternion();
+
+  center->set_x(2.0);
+  center->set_y(4.0);
+  center->set_z(6.0);
+  size->set_x(3.0);
+  size->set_y(5.0);
+  size->set_z(7.0);
+  orientation->set_x(0.733);
+  orientation->set_y(0.462);
+  orientation->set_z(0.191);
+  orientation->set_w(0.462);
+
+  box->set_allocated_center(center);
+  box->set_allocated_boxsize(size);
+  box->set_allocated_orientation(orientation);
+  _msg.set_allocated_box(box);
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedOriented3DBox> & _msg)
+{
+  gz::msgs::AnnotatedOriented3DBox expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+
+  EXPECT_EQ(expected_msg.label(), _msg->label());
+
+  gz::msgs::Oriented3DBox box = _msg->box();
+  gz::msgs::Vector3d center = box.center();
+  gz::msgs::Vector3d size = box.boxsize();
+  gz::msgs::Quaternion orientation = box.orientation();
+
+  EXPECT_EQ(expected_msg.box().center().x(), center.x());
+  EXPECT_EQ(expected_msg.box().center().y(), center.y());
+  EXPECT_EQ(expected_msg.box().center().z(), center.z());
+  EXPECT_EQ(expected_msg.box().boxsize().x(), size.x());
+  EXPECT_EQ(expected_msg.box().boxsize().y(), size.y());
+  EXPECT_EQ(expected_msg.box().boxsize().z(), size.z());
+  EXPECT_EQ(expected_msg.box().orientation().w(), orientation.w());
+  EXPECT_EQ(expected_msg.box().orientation().x(), orientation.x());
+  EXPECT_EQ(expected_msg.box().orientation().y(), orientation.y());
+  EXPECT_EQ(expected_msg.box().orientation().z(), orientation.z());
+}
+
+void createTestMsg(gz::msgs::AnnotatedOriented3DBox_V & _msg)
+{
+  gz::msgs::Header header_msg;
+
+  createTestMsg(header_msg);
+
+  _msg.mutable_header()->CopyFrom(header_msg);
+
+  for (size_t i = 0; i < 4; i++) {
+    gz::msgs::AnnotatedOriented3DBox * box = _msg.add_annotated_box();
+    createTestMsg(*box);
+  }
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedOriented3DBox_V> & _msg)
+{
+  gz::msgs::AnnotatedOriented3DBox_V expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+
+  for (size_t i = 0; i < 4; i++) {
+    compareTestMsg(
+      std::make_shared<gz::msgs::AnnotatedOriented3DBox>(
+        _msg->annotated_box(
+          i)));
+  }
+}
+
 }  // namespace testing
 }  // namespace ros_gz_bridge

ros_gz_bridge/test/utils/gz_test_msg.hpp

@@ -65,6 +65,7 @@
 #include <gz/msgs/video_record.pb.h>
 #include <gz/msgs/wrench.pb.h>
 #include <gz/msgs/annotated_axis_aligned_2d_box_v.pb.h>
+#include <gz/msgs/annotated_oriented_3d_box_v.pb.h>
 
 #include <memory>
 
@@ -506,6 +507,22 @@ void createTestMsg(gz::msgs::AnnotatedAxisAligned2DBox_V & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedAxisAligned2DBox_V> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::AnnotatedOriented3DBox & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedOriented3DBox> & _msg);
+
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::AnnotatedOriented3DBox_V & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedOriented3DBox_V> & _msg);
+
 }  // namespace testing
 }  // namespace ros_gz_bridge
 

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <cstddef>
 
 #include "gz/msgs/config.hh"
 
@@ -1488,5 +1489,76 @@ void compareTestMsg(const std::shared_ptr<vision_msgs::msg::Detection2DArray> &
   }
 }
 
+void createTestMsg(vision_msgs::msg::Detection3D & _msg)
+{
+  std_msgs::msg::Header header_msg;
+  createTestMsg(header_msg);
+  _msg.header = header_msg;
+
+  vision_msgs::msg::ObjectHypothesisWithPose class_prob;
+  class_prob.hypothesis.class_id = "1";
+  class_prob.hypothesis.score = 1.0;
+  _msg.results.push_back(class_prob);
+
+  _msg.bbox.size.x = 3.0;
+  _msg.bbox.size.y = 5.0;
+  _msg.bbox.size.z = 7.0;
+  _msg.bbox.center.position.x = 2.0;
+  _msg.bbox.center.position.y = 4.0;
+  _msg.bbox.center.position.z = 6.0;
+  _msg.bbox.center.orientation.x = 0.733;
+  _msg.bbox.center.orientation.y = 0.462;
+  _msg.bbox.center.orientation.z = 0.191;
+  _msg.bbox.center.orientation.w = 0.462;
+}
+
+void compareTestMsg(const std::shared_ptr<vision_msgs::msg::Detection3D> & _msg)
+{
+  vision_msgs::msg::Detection3D expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(_msg->header);
+  EXPECT_EQ(expected_msg.results.size(), _msg->results.size());
+  for (size_t i = 0; i < _msg->results.size(); i++) {
+    EXPECT_EQ(expected_msg.results[i].hypothesis.class_id, _msg->results[i].hypothesis.class_id);
+    EXPECT_EQ(expected_msg.results[i].hypothesis.score, _msg->results[i].hypothesis.score);
+  }
+  EXPECT_EQ(expected_msg.bbox.size.x, _msg->bbox.size.x);
+  EXPECT_EQ(expected_msg.bbox.size.y, _msg->bbox.size.y);
+  EXPECT_EQ(expected_msg.bbox.size.z, _msg->bbox.size.z);
+  EXPECT_EQ(expected_msg.bbox.center.position.x, _msg->bbox.center.position.x);
+  EXPECT_EQ(expected_msg.bbox.center.position.y, _msg->bbox.center.position.y);
+  EXPECT_EQ(expected_msg.bbox.center.position.z, _msg->bbox.center.position.z);
+  EXPECT_EQ(expected_msg.bbox.center.orientation.x, _msg->bbox.center.orientation.x);
+  EXPECT_EQ(expected_msg.bbox.center.orientation.y, _msg->bbox.center.orientation.y);
+  EXPECT_EQ(expected_msg.bbox.center.orientation.z, _msg->bbox.center.orientation.z);
+  EXPECT_EQ(expected_msg.bbox.center.orientation.w, _msg->bbox.center.orientation.w);
+}
+
+void createTestMsg(vision_msgs::msg::Detection3DArray & _msg)
+{
+  std_msgs::msg::Header header_msg;
+  createTestMsg(header_msg);
+  _msg.header = header_msg;
+
+  for (size_t i = 0; i < 4; i++) {
+    vision_msgs::msg::Detection3D detection;
+    createTestMsg(detection);
+    _msg.detections.push_back(detection);
+  }
+}
+
+void compareTestMsg(const std::shared_ptr<vision_msgs::msg::Detection3DArray> & _msg)
+{
+  vision_msgs::msg::Detection3DArray expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(_msg->header);
+  EXPECT_EQ(expected_msg.detections.size(), _msg->detections.size());
+  for (size_t i = 0; i < _msg->detections.size(); i++) {
+    compareTestMsg(std::make_shared<vision_msgs::msg::Detection3D>(_msg->detections[i]));
+  }
+}
+
 }  // namespace testing
 }  // namespace ros_gz_bridge

ros_gz_bridge/test/utils/ros_test_msg.hpp

@@ -79,6 +79,7 @@
 #include <trajectory_msgs/msg/joint_trajectory.hpp>
 #include <rcl_interfaces/msg/parameter_value.hpp>
 #include "vision_msgs/msg/detection2_d_array.hpp"
+#include "vision_msgs/msg/detection3_d_array.hpp"
 
 namespace ros_gz_bridge
 {
@@ -627,6 +628,22 @@ void createTestMsg(vision_msgs::msg::Detection2D & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<vision_msgs::msg::Detection2D> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(vision_msgs::msg::Detection3DArray & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<vision_msgs::msg::Detection3DArray> & _msg);
+
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(vision_msgs::msg::Detection3D & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<vision_msgs::msg::Detection3D> & _msg);
+
 }  // namespace testing
 }  // namespace ros_gz_bridge