0
$\begingroup$

I would like to add custom messages and services to the gazebo ros bridge. But I am not sure what the best approach would be and miss a bit of documentation on how to achieve this.

For the sake of this discussion, let's say I want to bridge the standard gz messages Boolean or Double to my custom stamped ros types BoolStamped or Float64Stamped.

I know the message types need to be known at compile time as described in this talk. However, the speaker also makes it sound 'easy enough' to add messages to it.

Best case, I would like to externalize this addition to my own package, but I suspect I need to fork the entire ros_gz repo and inject those additions directly into the ros_gz_bridge package so the native ros node parameter_bridge has knowledge of those types.

Regardless of each method, I fail to find direct documentation on how to do that or examples of it.

Has anybody done this before? Or can somebody familiar with the ros_gz_bridge package point me into the right direction?

$\endgroup$

1 Answer 1

1
$\begingroup$

if you are fine with the "Embedding ROS 2 in Gazebo" approach mentioned in the given talk, then you could create a gazebo system-plugin to bridge the messages/topics by adapting the following sources:

Create a header file for the plugin:

#ifndef GZ_ROS_BOUNDINGBOX_BRIDGE_SYSTEM_H
#define GZ_ROS_BOUNDINGBOX_BRIDGE_SYSTEM_H

#include <gz/sim/EventManager.hh>
#include <gz/sim/System.hh>
#include <gz/sim/SdfEntityCreator.hh>
#include "rclcpp/rclcpp.hpp"
#include "vision_msgs/msg/detection2_d_array.hpp"
#include <gz/transport.hh>
#include <gz/msgs.hh>

namespace demo::ros::gz::system {

    class BoundingBoxBridge : public ::gz::sim::System,
                              public ::gz::sim::ISystemConfigure{

    public:
        BoundingBoxBridge();

    public:
        void Configure(
                const ::gz::sim::Entity &_entity,
                const std::shared_ptr<const sdf::Element> &_element,
                ::gz::sim::EntityComponentManager &_ecm,
                ::gz::sim::EventManager &_eventManager) override;

    private:
        rclcpp::Node::SharedPtr ros_node;
        rclcpp::Publisher<vision_msgs::msg::Detection2DArray>::SharedPtr ros_bbs_pub0;
        rclcpp::Publisher<vision_msgs::msg::Detection2DArray>::SharedPtr ros_bbs_pub1;
        ::gz::transport::Node gz_node;

    private:
        void OnGzBoundingBoxes0(const ::gz::msgs::AnnotatedAxisAligned2DBox_V &msg);
        void OnGzBoundingBoxes1(const ::gz::msgs::AnnotatedAxisAligned2DBox_V &msg);
        std::list<vision_msgs::msg::ObjectHypothesisWithPose> hypothesis_array_from_label(const std::string &label);
        std::string label_to_class_name(int class_id);
    };

}

#endif //GZ_ROS_BOUNDINGBOX_BRIDGE_SYSTEM_H

Create the source file:

#include "demo/ros/gz/system/boundingbox_bridge.h"

#include <gz/common/Console.hh>
#include <gz/sim/components.hh>
#include <gz/plugin/Register.hh>
#include <gz/sim/SdfEntityCreator.hh>
#include <memory>
#include <sdf/Model.hh>
#include <sdf/sdf.hh>
#include <cmath>
#include <gz/sim/System.hh>



#include <string>

namespace demo::ros::gz::system {

    GZ_ADD_PLUGIN(
            BoundingBoxBridge,
            ::gz::sim::System,
            BoundingBoxBridge::ISystemConfigure
    )

    BoundingBoxBridge::BoundingBoxBridge() {

    }

    void BoundingBoxBridge::Configure(const ::gz::sim::Entity &_entity,
                                  const std::shared_ptr<const sdf::Element> &_element,
                                  ::gz::sim::EntityComponentManager &_ecm,
                                  ::gz::sim::EventManager &_eventManager) {
        (void) _ecm;
        (void) _eventManager;
        ::gz::common::Console::dbg << "Configure BoundingBoxBridge system" << std::endl;
        (void) _entity;
        (void) _element;
        if (!rclcpp::ok()) {
            rclcpp::init(0, nullptr);
        }
        this->ros_node = std::make_shared<rclcpp::Node>("boundingbox_bridge_system");
        this->ros_bbs_pub0 = this->ros_node->create_publisher<vision_msgs::msg::Detection2DArray>("/demo/gz/sense/camera_left/detection", 10);
        this->ros_bbs_pub1 = this->ros_node->create_publisher<vision_msgs::msg::Detection2DArray>("/demo/gz/sense/camera_right/detection", 10);
        gz_node.Subscribe("/demo/sense/camera_0/bounding_boxes", &BoundingBoxBridge::OnGzBoundingBoxes0, this);
        gz_node.Subscribe("/demo/sense/camera_1/bounding_boxes", &BoundingBoxBridge::OnGzBoundingBoxes1, this);

    }

    void BoundingBoxBridge::OnGzBoundingBoxes0(const ::gz::msgs::AnnotatedAxisAligned2DBox_V &_msg) {
        vision_msgs::msg::Detection2DArray ros_msg;

        std_msgs::msg::Header header;
        header.stamp.sec = (int)_msg.header().stamp().sec();
        header.stamp.nanosec = _msg.header().stamp().nsec();
        header.frame_id = "camera_left_optical";
        ros_msg.header = header;
        for (int i = 0; i < _msg.annotated_box_size(); i++) {
            const auto& gz_bb = _msg.annotated_box(i);
            vision_msgs::msg::Detection2D ros_bb;
            ros_bb.header = header;
            ros_bb.id = gz_bb.label();
            ros_bb.bbox.size_x = gz_bb.box().max_corner().x() - gz_bb.box().min_corner().x();
            ros_bb.bbox.size_y = gz_bb.box().max_corner().y() - gz_bb.box().min_corner().y();
            ros_bb.bbox.center.position.x = gz_bb.box().min_corner().x() + ros_bb.bbox.size_x / 2.0;
            ros_bb.bbox.center.position.y = gz_bb.box().min_corner().y() + ros_bb.bbox.size_y / 2.0;
            auto hypothesis = hypothesis_array_from_label(label_to_class_name(gz_bb.label()));
            ros_bb.results.insert(ros_bb.results.end(), hypothesis.begin(), hypothesis.end());
            ros_msg.detections.push_back(ros_bb);
        }
        this->ros_bbs_pub0->publish(ros_msg);
    }

    void BoundingBoxBridge::OnGzBoundingBoxes1(const ::gz::msgs::AnnotatedAxisAligned2DBox_V &_msg) {
        vision_msgs::msg::Detection2DArray ros_msg;

        std_msgs::msg::Header header;
        header.stamp.sec = (int)_msg.header().stamp().sec();
        header.stamp.nanosec = _msg.header().stamp().nsec();
        header.frame_id = "camera_right_optical";
        ros_msg.header = header;
        for (int i = 0; i < _msg.annotated_box_size(); i++) {
            const auto& gz_bb = _msg.annotated_box(i);
            vision_msgs::msg::Detection2D ros_bb;
            ros_bb.header = header;
            ros_bb.id = gz_bb.label();
            ros_bb.bbox.size_x = gz_bb.box().max_corner().x() - gz_bb.box().min_corner().x();
            ros_bb.bbox.size_y = gz_bb.box().max_corner().y() - gz_bb.box().min_corner().y();
            ros_bb.bbox.center.position.x = gz_bb.box().min_corner().x() + ros_bb.bbox.size_x / 2.0;
            ros_bb.bbox.center.position.y = gz_bb.box().min_corner().y() + ros_bb.bbox.size_y / 2.0;
            auto hypothesis = hypothesis_array_from_label(label_to_class_name(gz_bb.label()));
            ros_bb.results.insert(ros_bb.results.end(), hypothesis.begin(), hypothesis.end());
            ros_msg.detections.push_back(ros_bb);
        }
        this->ros_bbs_pub1->publish(ros_msg);
    }

    std::list<vision_msgs::msg::ObjectHypothesisWithPose> BoundingBoxBridge::hypothesis_array_from_label(const std::string &label) {
        std::list<vision_msgs::msg::ObjectHypothesisWithPose> hypothesis_array;

        vision_msgs::msg::ObjectHypothesisWithPose hypothesis_with_pose_blue_small;
        vision_msgs::msg::ObjectHypothesis hypothesis_blue_small;
        hypothesis_blue_small.class_id = "blue_small";
        hypothesis_blue_small.score = label == "blue_small" ? 1.0 : 0.0;
        hypothesis_with_pose_blue_small.hypothesis = hypothesis_blue_small;
        hypothesis_array.push_back(hypothesis_with_pose_blue_small);

        vision_msgs::msg::ObjectHypothesisWithPose hypothesis_with_pose_yellow_small;
        vision_msgs::msg::ObjectHypothesis hypothesis_yellow_small;
        hypothesis_yellow_small.class_id = "yellow_small";
        hypothesis_yellow_small.score = label == "yellow_small" ? 1.0 : 0.0;
        hypothesis_with_pose_yellow_small.hypothesis = hypothesis_yellow_small;
        hypothesis_array.push_back(hypothesis_with_pose_yellow_small);

        vision_msgs::msg::ObjectHypothesisWithPose hypothesis_with_pose_orange_small;
        vision_msgs::msg::ObjectHypothesis hypothesis_orange_small;
        hypothesis_orange_small.class_id = "orange_small";
        hypothesis_orange_small.score = label == "orange_small" ? 1.0 : 0.0;
        hypothesis_with_pose_orange_small.hypothesis = hypothesis_orange_small;
        hypothesis_array.push_back(hypothesis_with_pose_orange_small);

        vision_msgs::msg::ObjectHypothesisWithPose hypothesis_with_pose_orange_large;
        vision_msgs::msg::ObjectHypothesis hypothesis_orange_large;
        hypothesis_orange_large.class_id = "orange_larger";
        hypothesis_orange_large.score = label == "orange_larger" ? 1.0 : 0.0;
        hypothesis_with_pose_orange_large.hypothesis = hypothesis_orange_large;
        hypothesis_array.push_back(hypothesis_with_pose_orange_large);

        vision_msgs::msg::ObjectHypothesisWithPose hypothesis_with_pose_undefined;
        vision_msgs::msg::ObjectHypothesis hypothesis_undefined;
        hypothesis_undefined.class_id = "undefined";
        hypothesis_undefined.score = label == "undefined" ? 1.0 : 0.0;
        hypothesis_with_pose_undefined.hypothesis = hypothesis_undefined;
        hypothesis_array.push_back(hypothesis_with_pose_undefined);

        return hypothesis_array;
    }

    std::string BoundingBoxBridge::label_to_class_name(int class_id) {
        switch (class_id) {
            case 0:
                return "blue_small";
            case 1:
                return "yellow_small";
            case 2:
                return "orange_small";
            case 3:
                return "orange_larger";
            default:
                return "undefined";
        }
    }
}

Then add the plugin to the robot (or to the world, depending on use case):

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="demo">
    <gazebo reference="demo"/>
    <gazebo>
        <plugin filename="BoundingBoxBridge"
                name="demo::ros::gz::system::BoundingBoxBridge">
        </plugin>
    </gazebo>
...

Add the plugin to the CMakeLists.txt for compilation:

add_library(BoundingBoxBridge SHARED src/system/boundingbox_bridge.cpp)
target_include_directories(BoundingBoxBridge PUBLIC include ${RCLCPP_INCLUDE_DIRS})
target_link_libraries(BoundingBoxBridge
        gz-plugin2
        gz-sim7
        gz-sensors7
        )
ament_target_dependencies(BoundingBoxBridge
        rclcpp
        nav_msgs
        geometry_msgs
        interface_ros
        visualization_msgs
        vision_msgs
        )
install(TARGETS BoundingBoxBridge DESTINATION lib/${PROJECT_NAME})

And finally register the required hooks (as shown in the talk):

  1. In the CMakeLists.txt
ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in")
  1. In the "hooks/gz_ros.dsv.in" file
prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/
  1. In the "hooks/gz_ros.sh.in" file:
ament_prepend_unique_value GZ_SIM_PLUGIN_PATH "$AMENT_CURRENT_PREFIX/lib/@PROJECT_NAME@"

In this example gazebo messages of type gz::msgs::AnnotatedAxisAligned2DBox_V are bridged to vision_msgs::msg::Detection2DArray by subscribing to the corresponding gazebo topic and registering a callback which publishes the converted messages via the embedded ros2 node.

This is done for the bounding boxes given by two different boundingbox_camera sensors attached to the robot.

In a similar way you could create a ros2-service with the embedded node and then redirect the service call to gazebo messages or interact with the simulation in a different way.

I originally created this system-plugin for a gazebo garden simulation test with ros2-humble some time ago.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.