runtime/env_manager/gz_enviroment/src/gz_enviroment.cpp

#include "gz_enviroment/gz_enviroment.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "ros_gz_bridge/convert.hpp"
namespace gz_enviroment
{

GzEnviroment::GzEnviroment()
: rclcpp_lifecycle::LifecycleNode("gz_enviroment") {}

CallbackReturn 
GzEnviroment::on_configure(const rclcpp_lifecycle::State& )
{
    RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_configure");


    return CallbackReturn::SUCCESS;
}

CallbackReturn 
GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&)
{
    RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_cleanup");
    
    m_tf2_broadcaster.reset();
    m_gz_node.reset();
    
    return CallbackReturn::SUCCESS;
}

CallbackReturn 
GzEnviroment::on_activate(const rclcpp_lifecycle::State&)
{
    RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_activate");
    m_tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
    m_gz_node = std::make_shared<ignition::transport::Node>();

    std::string m_topic_name = "/world/" + getGzWorldName() +"/dynamic_pose/info";
    std::string m_service_spawn = "/world/" + getGzWorldName() + "/create";

    m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
    
    return CallbackReturn::SUCCESS;
}

CallbackReturn 
GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&)
{
    RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_deactivate");
    
    
    return CallbackReturn::SUCCESS;
}

void 
GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v)
{
    m_follow_frames = {"box1", "box2", "box3", "box4"};
    for (const auto &it: pose_v.pose())
    {   
        if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end())
            continue;
            
        geometry_msgs::msg::PoseStamped rpose;
        ros_gz_bridge::convert_gz_to_ros(it, rpose);
        
        geometry_msgs::msg::TransformStamped t;

        t.header.stamp = this->get_clock()->now();
        t.header.frame_id = "world";
        t.child_frame_id = it.name();

        t.transform.translation.x = rpose.pose.position.x;
        t.transform.translation.y = rpose.pose.position.y;
        t.transform.translation.z = rpose.pose.position.z;

        t.transform.rotation.x = rpose.pose.orientation.x;
        t.transform.rotation.y = rpose.pose.orientation.y;
        t.transform.rotation.z = rpose.pose.orientation.z;
        t.transform.rotation.w = rpose.pose.orientation.w;
        
        m_tf2_broadcaster->sendTransform(t);
    }
}

std::string
GzEnviroment::getGzWorldName()
{
    bool executed{false};
    bool result{false};
    unsigned int timeout{5000};
    std::string service{"/gazebo/worlds"};
    gz::msgs::StringMsg_V worlds_msg;

    while (rclcpp::ok() && !executed) {
        RCLCPP_INFO(this->get_logger(), "Requesting list of world names.");
        executed = m_gz_node->Request(service, timeout, worlds_msg, result);
    }

    if (!executed) {
        RCLCPP_INFO(this->get_logger(), "Timed out when getting world names.");
        return "";
    }

    if (!result || worlds_msg.data().empty()) {
        RCLCPP_INFO(this->get_logger(), "Failed to get world names.");
        return "";
    }
    RCLCPP_INFO(this->get_logger(), "%s", worlds_msg.data(0).c_str());
    return worlds_msg.data(0);
}

void
GzEnviroment::doGzSpawn()
{
    gz::msgs::EntityFactory entity;
    gz::msgs::Model model;
    gz::msgs::Link link;

}

}

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
    gz_enviroment::GzEnviroment, rclcpp_lifecycle::LifecycleNode
);