From 4bac670628481bc2af9579a7004be766cd7c1a98 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Mon, 1 Apr 2024 17:25:59 +0200 Subject: [PATCH] Added conversion for Detection3D and Detection3DArray (#523) Signed-off-by: wittenator <9154515+wittenator@users.noreply.github.com> --- ros_gz_bridge/README.md | 118 +++++++++--------- .../ros_gz_bridge/convert/vision_msgs.hpp | 26 ++++ ros_gz_bridge/ros_gz_bridge/mappings.py | 2 + ros_gz_bridge/src/convert/vision_msgs.cpp | 85 +++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 88 +++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 17 +++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 72 +++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 17 +++ 8 files changed, 367 insertions(+), 58 deletions(-) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 19b8a77d..242d92b2 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -5,64 +5,66 @@ between ROS and Gazebo Transport. The following message types can be bridged for topics: -| ROS type | Gazebo type | -|---------------------------------------------|:--------------------------------------:| -| builtin_interfaces/msg/Time | ignition::msgs::Time | -| std_msgs/msg/Bool | ignition::msgs::Boolean | -| std_msgs/msg/ColorRGBA | ignition::msgs::Color | -| std_msgs/msg/Empty | ignition::msgs::Empty | -| std_msgs/msg/Float32 | ignition::msgs::Float | -| std_msgs/msg/Float64 | ignition::msgs::Double | -| std_msgs/msg/Header | ignition::msgs::Header | -| std_msgs/msg/Int32 | ignition::msgs::Int32 | -| std_msgs/msg/UInt32 | ignition::msgs::UInt32 | -| std_msgs/msg/String | ignition::msgs::StringMsg | -| geometry_msgs/msg/Wrench | ignition::msgs::Wrench | -| geometry_msgs/msg/WrenchStamped | ignition::msgs::Wrench | -| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion | -| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d | -| geometry_msgs/msg/Point | ignition::msgs::Vector3d | -| geometry_msgs/msg/Pose | ignition::msgs::Pose | -| geometry_msgs/msg/PoseArray | ignition::msgs::Pose_V | -| geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance | -| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose | -| geometry_msgs/msg/Transform | ignition::msgs::Pose | -| geometry_msgs/msg/TransformStamped | ignition::msgs::Pose | -| geometry_msgs/msg/Twist | ignition::msgs::Twist | -| geometry_msgs/msg/TwistStamped | ignition::msgs::Twist | -| geometry_msgs/msg/TwistWithCovariance | ignition::msgs::TwistWithCovariance | -| geometry_msgs/msg/TwistWithCovarianceStamped| ignition::msgs::TwistWithCovariance | -| nav_msgs/msg/Odometry | ignition::msgs::Odometry | -| nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance | -| rcl_interfaces/msg/ParameterValue | ignition::msgs::Any | -| ros_gz_interfaces/msg/Altimeter | ignition::msgs::Altimeter | -| ros_gz_interfaces/msg/Contact | ignition::msgs::Contact | -| ros_gz_interfaces/msg/Contacts | ignition::msgs::Contacts | -| ros_gz_interfaces/msg/Dataframe | ignition::msgs::Dataframe | -| ros_gz_interfaces/msg/Entity | ignition::msgs::Entity | -| ros_gz_interfaces/msg/Float32Array | ignition::msgs::Float_V | -| ros_gz_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | -| ros_gz_interfaces/msg/JointWrench | ignition::msgs::JointWrench | -| ros_gz_interfaces/msg/Light | ignition::msgs::Light | -| ros_gz_interfaces/msg/SensorNoise | ignition::msgs::SensorNoise | -| ros_gz_interfaces/msg/StringVec | ignition::msgs::StringMsg_V | -| ros_gz_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual | -| ros_gz_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | -| ros_gz_interfaces/msg/WorldControl | ignition::msgs::WorldControl | -| rosgraph_msgs/msg/Clock | ignition::msgs::Clock | -| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState | -| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo | -| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure | -| sensor_msgs/msg/Imu | ignition::msgs::IMU | -| sensor_msgs/msg/Image | ignition::msgs::Image | -| sensor_msgs/msg/JointState | ignition::msgs::Model | -| sensor_msgs/msg/Joy | ignition::msgs::Joy | -| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan | -| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer | -| sensor_msgs/msg/NavSatFix | ignition::msgs::NavSat | -| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked | -| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V | -| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory | +| ROS type | Gazebo type | +|---------------------------------------------|:-------------------------------------------:| +| builtin_interfaces/msg/Time | ignition::msgs::Time | +| std_msgs/msg/Bool | ignition::msgs::Boolean | +| std_msgs/msg/ColorRGBA | ignition::msgs::Color | +| std_msgs/msg/Empty | ignition::msgs::Empty | +| std_msgs/msg/Float32 | ignition::msgs::Float | +| std_msgs/msg/Float64 | ignition::msgs::Double | +| std_msgs/msg/Header | ignition::msgs::Header | +| std_msgs/msg/Int32 | ignition::msgs::Int32 | +| std_msgs/msg/UInt32 | ignition::msgs::UInt32 | +| std_msgs/msg/String | ignition::msgs::StringMsg | +| geometry_msgs/msg/Wrench | ignition::msgs::Wrench | +| geometry_msgs/msg/WrenchStamped | ignition::msgs::Wrench | +| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion | +| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d | +| geometry_msgs/msg/Point | ignition::msgs::Vector3d | +| geometry_msgs/msg/Pose | ignition::msgs::Pose | +| geometry_msgs/msg/PoseArray | ignition::msgs::Pose_V | +| geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance | +| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose | +| geometry_msgs/msg/Transform | ignition::msgs::Pose | +| geometry_msgs/msg/TransformStamped | ignition::msgs::Pose | +| geometry_msgs/msg/Twist | ignition::msgs::Twist | +| geometry_msgs/msg/TwistStamped | ignition::msgs::Twist | +| geometry_msgs/msg/TwistWithCovariance | ignition::msgs::TwistWithCovariance | +| geometry_msgs/msg/TwistWithCovarianceStamped| ignition::msgs::TwistWithCovariance | +| nav_msgs/msg/Odometry | ignition::msgs::Odometry | +| nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance | +| rcl_interfaces/msg/ParameterValue | ignition::msgs::Any | +| ros_gz_interfaces/msg/Altimeter | ignition::msgs::Altimeter | +| ros_gz_interfaces/msg/Contact | ignition::msgs::Contact | +| ros_gz_interfaces/msg/Contacts | ignition::msgs::Contacts | +| ros_gz_interfaces/msg/Dataframe | ignition::msgs::Dataframe | +| ros_gz_interfaces/msg/Entity | ignition::msgs::Entity | +| ros_gz_interfaces/msg/Float32Array | ignition::msgs::Float_V | +| ros_gz_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | +| ros_gz_interfaces/msg/JointWrench | ignition::msgs::JointWrench | +| ros_gz_interfaces/msg/Light | ignition::msgs::Light | +| ros_gz_interfaces/msg/SensorNoise | ignition::msgs::SensorNoise | +| ros_gz_interfaces/msg/StringVec | ignition::msgs::StringMsg_V | +| ros_gz_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual | +| ros_gz_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | +| ros_gz_interfaces/msg/WorldControl | ignition::msgs::WorldControl | +| rosgraph_msgs/msg/Clock | ignition::msgs::Clock | +| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState | +| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo | +| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure | +| sensor_msgs/msg/Imu | ignition::msgs::IMU | +| sensor_msgs/msg/Image | ignition::msgs::Image | +| sensor_msgs/msg/JointState | ignition::msgs::Model | +| sensor_msgs/msg/Joy | ignition::msgs::Joy | +| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan | +| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer | +| sensor_msgs/msg/NavSatFix | ignition::msgs::NavSat | +| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked | +| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V | +| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory | +| vision_msgs/msg/Detection3D | ignition::msgs::AnnotatedOriented3DBox | +| vision_msgs/msg/Detection3DArray | ignition::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 04137082..ddec0f68 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -106,6 +106,8 @@ '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 b64c0f40..71a651b3 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 @@ -1591,5 +1592,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 d939a4ff..ad063b62 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 @@ -525,6 +526,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 f80b8553..c6f085e2 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" @@ -1476,5 +1477,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 ee4426fd..5a39c4ab 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -82,6 +82,7 @@ #include #include #include "vision_msgs/msg/detection2_d_array.hpp" +#include "vision_msgs/msg/detection3_d_array.hpp" namespace ros_gz_bridge { @@ -620,6 +621,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