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"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"

namespace gz_enviroment {

GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {}

CallbackReturn GzEnviroment::on_init()
{
    RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()");
    return CallbackReturn::SUCCESS;
}

CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State& ) {
    // Configuration of parameters
    RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
    m_gz_node = std::make_shared<gz::transport::Node>();
    m_topic_name = std::string("/world/") + getGzWorldName() +"/dynamic_pose/info";
    m_service_spawn = std::string("/world/") + getGzWorldName() + "/create";
  
    if (!doGzSpawn()) {
        return CallbackReturn::ERROR;
    }
  
    return CallbackReturn::SUCCESS;
}

CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&) {
    // Setting up the subscribers and publishers
    RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");
  
    m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
    m_tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
  
   return CallbackReturn::SUCCESS;
}

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

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

// TODO: Check do this via EntityComponentManager
void 
GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v)
{
    // TODO: Read from config
    m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6", "monkey"};
    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 = getNode()->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(getNode()->get_logger(), "Requesting list of world names.");
        executed = m_gz_node->Request(service, timeout, worlds_msg, result);
    }

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

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

bool
GzEnviroment::doGzSpawn()
{
    gz::msgs::EntityFactory req;
    gz::msgs::Boolean rep;
    bool result;
    unsigned int timeout = 5000;
    // TODO: From Config
    std::vector<double> pps{1.0, 0.0, 1.0, 0.0, 0.0};
    std::string mesh_file = "monkey.stl";
    std::string mesh_name = "monkey";
    // ENDTODO
    std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name);
    req.set_sdf(sdf_string);
    bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);

    return executed;
}

std::string 
GzEnviroment::createGzModelString(
    const std::vector<double>& pose, 
    const std::string& mesh_filepath, 
    const std::string& name
    )
{
  std::string model_template = 
    std::string("<sdf version='1.7'>") +
      "<model name='"+ name +"'>" +
        "<link name='link_"+ name +"'>" +
          "<pose>"
          +std::to_string(pose[0])+" "
          +std::to_string(pose[1])+" "
          +std::to_string(pose[2])+" "
          +std::to_string(pose[3])+" "
          +std::to_string(pose[4])+" "
          +std::to_string(pose[5])+"</pose>" +
          "<visual name='visual_"+name+"'>" +
            "<geometry><mesh><uri>file://"+mesh_filepath+"</uri></mesh></geometry>" +
          "</visual>" +
          "<collision name='collision_"+name+"'>" +
            "<geometry><mesh><uri>file://"+mesh_filepath+"</uri></mesh></geometry>" +
          "</collision>" +
        "</link>" +
      "</model>" +
    "</sdf>";
    return model_template;
}

}

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