Skip to content

Commit

Permalink
Added conversion for Detection3D and Detection3DArray (#523)
Browse files Browse the repository at this point in the history 
Signed-off-by: wittenator <[email protected]>
  • Loading branch information
@wittenator @ahcorde
wittenator authored and ahcorde committed Apr 1, 2024
1 parent 0b7c5e9 commit 4bac670
Show file tree
Hide file tree
Showing 8 changed files with 367 additions and 58 deletions.
118 changes: 60 additions & 58 deletions ros_gz_bridge/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:

Expand Down
26 changes: 26 additions & 0 deletions ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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_
2 changes: 2 additions & 0 deletions ros_gz_bridge/ros_gz_bridge/mappings.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,8 @@
'vision_msgs': [
Mapping('Detection2DArray', 'AnnotatedAxisAligned2DBox_V'),
Mapping('Detection2D', 'AnnotatedAxisAligned2DBox'),
Mapping('Detection3DArray', 'AnnotatedOriented3DBox_V'),
Mapping('Detection3D', 'AnnotatedOriented3DBox'),
],
}

Expand Down
85 changes: 85 additions & 0 deletions ros_gz_bridge/src/convert/vision_msgs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
88 changes: 88 additions & 0 deletions ros_gz_bridge/test/utils/gz_test_msg.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

#include <memory>
#include <string>
#include <cstddef>

#if GZ_MSGS_MAJOR_VERSION >= 10
#define GZ_MSGS_IMU_HAS_COVARIANCE
Expand Down Expand Up @@ -1591,5 +1592,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
17 changes: 17 additions & 0 deletions ros_gz_bridge/test/utils/gz_test_msg.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down Expand Up @@ -525,6 +526,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

Expand Down
Loading

0 comments on commit 4bac670

Please sign in to comment.