diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index e71db1db..ba45273c 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/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") 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 new file mode 100644 index 00000000..f370144f --- /dev/null +++ b/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 + +// ROS 2 messages +#include "vision_msgs/msg/detection2_d_array.hpp" +#include + +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_ diff --git a/ros_gz_bridge/package.xml b/ros_gz_bridge/package.xml index 1efdc156..cf626a38 100644 --- a/ros_gz_bridge/package.xml +++ b/ros_gz_bridge/package.xml @@ -29,6 +29,7 @@ tf2_msgs trajectory_msgs yaml_cpp_vendor + vision_msgs gz-msgs9 diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index f87b94bc..6c44a9e1 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/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'), + ], } diff --git a/ros_gz_bridge/src/convert/vision_msgs.cpp b/ros_gz_bridge/src/convert/vision_msgs.cpp new file mode 100644 index 00000000..057426c7 --- /dev/null +++ b/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 + +#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 diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index a2ba4b13..3dd63e9e 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1479,5 +1479,80 @@ void compareTestMsg(const std::shared_ptr & _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 & _msg) +{ + gz::msgs::AnnotatedAxisAligned2DBox expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_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 & _msg) +{ + gz::msgs::AnnotatedAxisAligned2DBox_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 aa4ded44..b469121e 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -64,6 +64,7 @@ #include #include #include +#include #include @@ -489,6 +490,22 @@ void createTestMsg(gz::msgs::VideoRecord & _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::AnnotatedAxisAligned2DBox & _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::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 & _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 8c14ff54..d9fcd06b 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -1417,5 +1417,64 @@ void compareTestMsg(const std::shared_ptr & 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 & _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 & _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(_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 69dfd155..a8897901 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -77,6 +77,7 @@ #include #include #include +#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 & _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 & _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 & _msg); + } // namespace testing } // namespace ros_gz_bridge