Added messages for 2D Bounding Boxes to ros_gz_bridge (#458)

Signed-off-by: wittenator <[email protected]>

ros_gz_bridge/CMakeLists.txt

@@ -60,6 +60,7 @@ set(BRIDGE_MESSAGE_TYPES
   std_msgs
   tf2_msgs
   trajectory_msgs
+  vision_msgs
 )
 
 set(generated_path "${CMAKE_BINARY_DIR}/generated")
@@ -237,6 +238,7 @@ if(BUILD_TESTING)
     std_msgs
     tf2_msgs
     trajectory_msgs
+    vision_msgs
   )
 
   set(generated_path "${CMAKE_BINARY_DIR}/generated/test")

ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp

@@ -0,0 +1,52 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS_GZ_BRIDGE__CONVERT__VISION_MSGS_HPP_
+#define ROS_GZ_BRIDGE__CONVERT__VISION_MSGS_HPP_
+
+// Gazebo Msgs
+#include <gz/msgs/annotated_axis_aligned_2d_box_v.pb.h>
+
+// ROS 2 messages
+#include "vision_msgs/msg/detection2_d_array.hpp"
+#include <ros_gz_bridge/convert_decl.hpp>
+
+namespace ros_gz_bridge
+{
+template<>
+void
+convert_ros_to_gz(
+  const vision_msgs::msg::Detection2D & ros_msg,
+  gz::msgs::AnnotatedAxisAligned2DBox & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::AnnotatedAxisAligned2DBox & gz_msg,
+  vision_msgs::msg::Detection2D & ros_msg);
+
+template<>
+void
+convert_ros_to_gz(
+  const vision_msgs::msg::Detection2DArray & ros_msg,
+  gz::msgs::AnnotatedAxisAligned2DBox_V & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::AnnotatedAxisAligned2DBox_V & gz_msg,
+  vision_msgs::msg::Detection2DArray & ros_msg);
+}  // namespace ros_gz_bridge
+
+#endif  // ROS_GZ_BRIDGE__CONVERT__VISION_MSGS_HPP_

ros_gz_bridge/package.xml

@@ -29,6 +29,7 @@
   <depend>tf2_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
+  <depend>vision_msgs</depend>
 
   <!-- Garden (default) -->
   <depend condition="$GZ_VERSION == '' or $GZ_VERSION == garden">gz-msgs9</depend>

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -107,4 +107,8 @@
     'trajectory_msgs': [
         Mapping('JointTrajectory', 'JointTrajectory'),
     ],
+    'vision_msgs': [
+        Mapping('Detection2DArray', 'AnnotatedAxisAligned2DBox_V'),
+        Mapping('Detection2D', 'AnnotatedAxisAligned2DBox'),
+    ],
 }

ros_gz_bridge/src/convert/vision_msgs.cpp

@@ -0,0 +1,97 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <limits>
+
+#include "convert/utils.hpp"
+#include "ros_gz_bridge/convert/vision_msgs.hpp"
+
+namespace ros_gz_bridge
+{
+template<>
+void
+convert_ros_to_gz(
+  const vision_msgs::msg::Detection2D & ros_msg,
+  gz::msgs::AnnotatedAxisAligned2DBox & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+
+  gz::msgs::AxisAligned2DBox * box = new gz::msgs::AxisAligned2DBox();
+  gz::msgs::Vector2d * min_corner = new gz::msgs::Vector2d();
+  gz::msgs::Vector2d * max_corner = new gz::msgs::Vector2d();
+
+  if (ros_msg.results.size() != 0) {
+    auto id = ros_msg.results.at(0).hypothesis.class_id;
+    gz_msg.set_label(std::stoi(id));
+  }
+
+  min_corner->set_x(ros_msg.bbox.center.position.x - ros_msg.bbox.size_x / 2);
+  min_corner->set_y(ros_msg.bbox.center.position.y - ros_msg.bbox.size_y / 2);
+  max_corner->set_x(ros_msg.bbox.center.position.x + ros_msg.bbox.size_x / 2);
+  max_corner->set_y(ros_msg.bbox.center.position.y + ros_msg.bbox.size_y / 2);
+  box->set_allocated_min_corner(min_corner);
+  box->set_allocated_max_corner(max_corner);
+  gz_msg.set_allocated_box(box);
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::AnnotatedAxisAligned2DBox & gz_msg,
+  vision_msgs::msg::Detection2D & 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().min_corner().x() + gz_msg.box().max_corner().x()
+    ) / 2;
+  ros_msg.bbox.center.position.y = (
+    gz_msg.box().min_corner().y() + gz_msg.box().max_corner().y()
+    ) / 2;
+  ros_msg.bbox.size_x = gz_msg.box().max_corner().x() - gz_msg.box().min_corner().x();
+  ros_msg.bbox.size_y = gz_msg.box().max_corner().y() - gz_msg.box().min_corner().y();
+}
+
+template<>
+void
+convert_ros_to_gz(
+  const vision_msgs::msg::Detection2DArray & ros_msg,
+  gz::msgs::AnnotatedAxisAligned2DBox_V & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+  for (const auto & ros_box : ros_msg.detections) {
+    gz::msgs::AnnotatedAxisAligned2DBox * gz_box = gz_msg.add_annotated_box();
+    convert_ros_to_gz(ros_box, *gz_box);
+  }
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::AnnotatedAxisAligned2DBox_V & gz_msg,
+  vision_msgs::msg::Detection2DArray & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  for (const auto & gz_box : gz_msg.annotated_box()) {
+    vision_msgs::msg::Detection2D 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

@@ -1479,5 +1479,80 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::VideoRecord> & _msg)
   EXPECT_EQ(expected_msg.save_filename(), _msg->save_filename());
 }
 
+void createTestMsg(gz::msgs::AnnotatedAxisAligned2DBox & _msg)
+{
+  gz::msgs::Header header_msg;
+
+  createTestMsg(header_msg);
+
+  _msg.mutable_header()->CopyFrom(header_msg);
+
+  _msg.set_label(1);
+
+  gz::msgs::AxisAligned2DBox * box = new gz::msgs::AxisAligned2DBox();
+  gz::msgs::Vector2d * min_corner = new gz::msgs::Vector2d();
+  gz::msgs::Vector2d * max_corner = new gz::msgs::Vector2d();
+
+  min_corner->set_x(2.0);
+  min_corner->set_y(2.0);
+  max_corner->set_x(4.0);
+  max_corner->set_y(6.0);
+  box->set_allocated_min_corner(min_corner);
+  box->set_allocated_max_corner(max_corner);
+  _msg.set_allocated_box(box);
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedAxisAligned2DBox> & _msg)
+{
+  gz::msgs::AnnotatedAxisAligned2DBox expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+
+  EXPECT_EQ(expected_msg.label(), _msg->label());
+
+  gz::msgs::AxisAligned2DBox expected_box = expected_msg.box();
+  gz::msgs::Vector2d expected_min_corner = expected_box.min_corner();
+  gz::msgs::Vector2d expected_max_corner = expected_box.max_corner();
+
+  gz::msgs::AxisAligned2DBox box = _msg->box();
+  gz::msgs::Vector2d min_corner = box.min_corner();
+  gz::msgs::Vector2d max_corner = box.max_corner();
+
+  EXPECT_EQ(expected_min_corner.x(), min_corner.x());
+  EXPECT_EQ(expected_min_corner.y(), min_corner.y());
+  EXPECT_EQ(expected_max_corner.x(), max_corner.x());
+  EXPECT_EQ(expected_max_corner.y(), max_corner.y());
+}
+
+void createTestMsg(gz::msgs::AnnotatedAxisAligned2DBox_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::AnnotatedAxisAligned2DBox * box = _msg.add_annotated_box();
+    createTestMsg(*box);
+  }
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::AnnotatedAxisAligned2DBox_V> & _msg)
+{
+  gz::msgs::AnnotatedAxisAligned2DBox_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::AnnotatedAxisAligned2DBox>(
+        _msg->annotated_box(
+          i)));
+  }
+}
+
 }  // namespace testing
 }  // namespace ros_gz_bridge

ros_gz_bridge/test/utils/gz_test_msg.hpp

@@ -64,6 +64,7 @@
 #include <gz/msgs/vector3d.pb.h>
 #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 <memory>
 
@@ -489,6 +490,22 @@ void createTestMsg(gz::msgs::VideoRecord & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<gz::msgs::VideoRecord> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::AnnotatedAxisAligned2DBox & _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::AnnotatedAxisAligned2DBox> & _msg);
+
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::AnnotatedAxisAligned2DBox_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::AnnotatedAxisAligned2DBox_V> & _msg);
+
 }  // namespace testing
 }  // namespace ros_gz_bridge
 

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -1417,5 +1417,64 @@ void compareTestMsg(const std::shared_ptr<rcl_interfaces::msg::ParameterValue> &
   EXPECT_EQ(expected_msg.string_value, _msg->string_value);
 }
 
+void createTestMsg(vision_msgs::msg::Detection2D & _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 = 2.0;
+  _msg.bbox.size_y = 4.0;
+  _msg.bbox.center.position.x = 3.0;
+  _msg.bbox.center.position.y = 4.0;
+}
+
+void compareTestMsg(const std::shared_ptr<vision_msgs::msg::Detection2D> & _msg)
+{
+  vision_msgs::msg::Detection2D 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.center.position.x, _msg->bbox.center.position.x);
+  EXPECT_EQ(expected_msg.bbox.center.position.y, _msg->bbox.center.position.y);
+}
+
+void createTestMsg(vision_msgs::msg::Detection2DArray & _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::Detection2D detection;
+    createTestMsg(detection);
+    _msg.detections.push_back(detection);
+  }
+}
+
+void compareTestMsg(const std::shared_ptr<vision_msgs::msg::Detection2DArray> & _msg)
+{
+  vision_msgs::msg::Detection2DArray 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::Detection2D>(_msg->detections[i]));
+  }
+}
+
 }  // namespace testing
 }  // namespace ros_gz_bridge

ros_gz_bridge/test/utils/ros_test_msg.hpp

@@ -77,6 +77,7 @@
 #include <tf2_msgs/msg/tf_message.hpp>
 #include <trajectory_msgs/msg/joint_trajectory.hpp>
 #include <rcl_interfaces/msg/parameter_value.hpp>
+#include "vision_msgs/msg/detection2_d_array.hpp"
 
 namespace ros_gz_bridge
 {
@@ -601,6 +602,22 @@ void createTestMsg(rcl_interfaces::msg::ParameterValue & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<rcl_interfaces::msg::ParameterValue> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(vision_msgs::msg::Detection2DArray & _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::Detection2DArray> & _msg);
+
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(vision_msgs::msg::Detection2D & _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::Detection2D> & _msg);
+
 }  // namespace testing
 }  // namespace ros_gz_bridge