diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 57ae4598..98792d92 100644 --- a/ros_gz_bridge/README.md +++ b/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: diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp index f370144f..b1556f65 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp @@ -17,9 +17,11 @@ // Gazebo Msgs #include +#include // ROS 2 messages #include "vision_msgs/msg/detection2_d_array.hpp" +#include "vision_msgs/msg/detection3_d_array.hpp" #include 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_ diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 5aeb3fad..808ee5c6 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/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'), ], } diff --git a/ros_gz_bridge/src/convert/vision_msgs.cpp b/ros_gz_bridge/src/convert/vision_msgs.cpp index 057426c7..61924af5 100644 --- a/ros_gz_bridge/src/convert/vision_msgs.cpp +++ b/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 diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 6cf82a3d..f1a0b0ff 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -18,6 +18,7 @@ #include #include +#include #if GZ_MSGS_MAJOR_VERSION >= 10 #define GZ_MSGS_IMU_HAS_COVARIANCE @@ -1557,5 +1558,92 @@ void compareTestMsg(const std::shared_ptr } } +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 & _msg) +{ + gz::msgs::AnnotatedOriented3DBox expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_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 & _msg) +{ + gz::msgs::AnnotatedOriented3DBox_V expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_msg->header())); + + for (size_t i = 0; i < 4; i++) { + compareTestMsg( + std::make_shared( + _msg->annotated_box( + i))); + } +} + } // namespace testing } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index b469121e..3fd7d623 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -65,6 +65,7 @@ #include #include #include +#include #include @@ -506,6 +507,22 @@ void createTestMsg(gz::msgs::AnnotatedAxisAligned2DBox_V & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _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 & _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 & _msg); + } // namespace testing } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 1de83956..90b90bdd 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -18,6 +18,7 @@ #include #include +#include #include "gz/msgs/config.hh" @@ -1488,5 +1489,76 @@ void compareTestMsg(const std::shared_ptr & } } +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 & _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 & _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(_msg->detections[i])); + } +} + } // namespace testing } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index f477df66..eb42d2fb 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -79,6 +79,7 @@ #include #include #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 & _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 & _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 & _msg); + } // namespace testing } // namespace ros_gz_bridge