add gz env

env_manager/env_manager/include/env_manager/env_base_interface.hpp

@@ -1,6 +1,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
+// TODO: BaseInterface class for next enviroments based on LifecycleNodeInterface
+// It's should be like ControllerInterfaceBase class from ros2_control
 
 namespace env_interface
 {

env_manager/env_manager/include/env_manager/env_manager.hpp

@@ -43,11 +43,20 @@ public:
 
     // EnvInterfaceSharedPtr loadEnv(const std::string& env_name);
     EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type);
-
+    EnvInterfaceSharedPtr unloadEnv();
     EnvStateReturnType configureEnv(const std::string& env_name);
-
     EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment);
 
+    //TODO: Define the input data format
+    // Set Target for RL enviroment
+    EnvInterfaceSharedPtr setTarget();
+    EnvInterfaceSharedPtr switchTarget();
+    EnvInterfaceSharedPtr unsetTarget();
+    // Load Constraints for RL enviroment
+    EnvInterfaceSharedPtr loadConstraints();
+    EnvInterfaceSharedPtr switchConstraints();
+    EnvInterfaceSharedPtr unloadConstraints();
+
 
 protected:
     void initServices();

env_manager/env_manager/src/env_manager.cpp

@@ -59,7 +59,7 @@ EnvManager::loadEnv_cb(
     const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
     env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response)
 {
-    std::lock_guard<std::mutex> lock(m_env_mutex); // Добавьте эту строку
+    std::lock_guard<std::mutex> lock(m_env_mutex);
     response->ok = loadEnv(request->name, request->type).get() != nullptr;
 }
 
@@ -71,6 +71,43 @@ EnvManager::configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Req
     response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS;
 }
 
+void
+EnvManager::unloadEnv_cb(
+    const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
+    env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response)
+{
+    std::lock_guard<std::mutex> lock(m_env_mutex);
+
+    auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&request](const EnvSpec& env) {
+        return env.name == request->name;
+    });
+
+    if (it != m_active_envs.end()) {
+        m_executor->remove_node(it->env_ptr->get_node_base_interface());
+        m_active_envs.erase(it);
+        response->ok = true;
+    } else {
+        RCLCPP_ERROR(this->get_logger(),
+            "Environment '%s' not found in the active environments list.",
+            request->name.c_str());
+        response->ok = false;
+    }
+}
+
+
+EnvInterfaceSharedPtr
+EnvManager::addEnv(const EnvSpec& enviroment)
+{
+    std::lock_guard<std::mutex> lock(m_env_mutex);
+    // generate list of active enviroments
+    // std::string unique_env_name = enviroment.name;// + "_" + std::to_string(m_active_envs.size());
+    // m_active_envs[unique_env_name] = enviroment.env_ptr;
+    m_active_envs.push_back(enviroment);
+    m_executor->add_node(enviroment.env_ptr->get_node_base_interface());
+    return enviroment.env_ptr;
+}
+
+
 EnvInterfaceSharedPtr
 EnvManager::loadEnv(const std::string& env_name, const std::string& env_class_type)
 {
@@ -131,44 +168,9 @@ EnvManager::configureEnv(const std::string& env_name)
         env_name.c_str());
         return EnvStateReturnType::FAILURE;
     }
-
+    it->env_ptr->configure();
+    // it->env_ptr->activate();
     return EnvStateReturnType::SUCCESS;
 }
 
-void
-EnvManager::unloadEnv_cb(
-    const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
-    env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response)
-{
-    std::lock_guard<std::mutex> lock(m_env_mutex);
-
-    auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&request](const EnvSpec& env) {
-        return env.name == request->name;
-    });
-
-    if (it != m_active_envs.end()) {
-        m_executor->remove_node(it->env_ptr->get_node_base_interface());
-        m_active_envs.erase(it);
-        response->ok = true;
-    } else {
-        RCLCPP_ERROR(this->get_logger(),
-            "Environment '%s' not found in the active environments list.",
-            request->name.c_str());
-        response->ok = false;
-    }
-}
-
-
-EnvInterfaceSharedPtr
-EnvManager::addEnv(const EnvSpec& enviroment)
-{
-    std::lock_guard<std::mutex> lock(m_env_mutex);
-    // generate list of active enviroments
-    // std::string unique_env_name = enviroment.name;// + "_" + std::to_string(m_active_envs.size());
-    // m_active_envs[unique_env_name] = enviroment.env_ptr;
-    m_active_envs.push_back(enviroment);
-    m_executor->add_node(enviroment.env_ptr->get_node_base_interface());
-    return enviroment.env_ptr;
-}
-
 }

env_manager/env_manager/src/run_env_manager.cpp

@@ -12,21 +12,11 @@ int main(int argc, char ** argv)
 
     RCLCPP_INFO(em->get_logger(), "Env manager is starting");
 
-    std::thread em_thread(
-        [em]()
-        {
-            em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment");
-            // while (rclcpp::ok())
-            // {
-            //     RCLCPP_INFO(em->get_logger(), "Env manager is running");
-            // }
-            
-        }
-    );
+    em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment");
+    em->configureEnv("gz_enviroment");
 
     executor->add_node(em);
     executor->spin();
-    em_thread.join();
     rclcpp::shutdown();
     return 0;
 }

env_manager/gz_enviroment/CMakeLists.txt

@@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     pluginlib
     ignition-transport11
     ignition-msgs8
+    ros_gz_bridge
 )
 
 # find dependencies

env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp

@@ -23,15 +23,18 @@ public:
 
 
 protected:
+
+
+private:
     std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster;
     std::shared_ptr<gz::transport::Node> m_gz_node;
-
     std::vector<std::string> m_follow_frames;
-    std::string m_topic_name = "/world/mir/dynamic_pose/info";
-private:
+
     void onGzPoseSub(const gz::msgs::Pose_V& pose_v);
 
-    
+    void doGzSpawn();
+
+    std::string getGzWorldName();
 
 };
 }

env_manager/gz_enviroment/src/gz_enviroment.cpp

@@ -1,22 +1,23 @@
 #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& )
+CallbackReturn 
+GzEnviroment::on_configure(const rclcpp_lifecycle::State& )
 {
-    RCLCPP_INFO(this->get_logger(), "GzEnvirometn is on_configure");
-    m_tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
-    m_gz_node = std::make_shared<ignition::transport::Node>();
-    m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
+    RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_configure");
+
 
     return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&)
+CallbackReturn 
+GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&)
 {
     RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_cleanup");
     
@@ -26,15 +27,23 @@ CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&)
     return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&)
+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&)
+CallbackReturn 
+GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&)
 {
     RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_deactivate");
     
@@ -42,11 +51,72 @@ CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&)
     return CallbackReturn::SUCCESS;
 }
 
-void onGzPoseSub(const gz::msgs::Pose_V& pose_v)
+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;
+
+}
 
 }