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"
#include <memory>
#include <rclcpp/logging.hpp>
#include <ros_gz_interfaces/msg/detail/entity__builder.hpp>

#include <gz/sim/components.hh>
#include <string>

namespace gz_enviroment
{

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

CallbackReturn GzEnviroment::on_init()
{
  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()");
  gz::sim::EntityComponentManager ecm;

  // Получение всех сущностей, которые представляют модели
  const auto modelEntities = ecm.EntitiesByComponents(gz::sim::components::Model());

  // Вывод списка моделей
  for (const auto entity : modelEntities)
  {
    const auto modelName = ecm.Component<gz::sim::components::Name>(entity);
    if (modelName)
    {
      std::cout << "Model Name: " << modelName->Data() << std::endl;
    }
  }
  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_world_name = getGzWorldName();
  m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
  m_service_spawn = std::string("/world/") + m_world_name + "/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"};
  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;
}

}  // namespace gz_enviroment

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