From 48e17c2481a9032e7113ee1e6b1cb13c9fea06ca Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 26 Sep 2023 23:18:28 +0300 Subject: [PATCH] add gz env --- .../env_manager/env_base_interface.hpp | 2 + .../include/env_manager/env_manager.hpp | 13 ++- env_manager/env_manager/src/env_manager.cpp | 78 +++++++-------- .../env_manager/src/run_env_manager.cpp | 14 +-- env_manager/gz_enviroment/CMakeLists.txt | 1 + .../include/gz_enviroment/gz_enviroment.hpp | 11 ++- .../gz_enviroment/src/gz_enviroment.cpp | 94 ++++++++++++++++--- env_manager/gz_enviroment/test.json | 0 8 files changed, 145 insertions(+), 68 deletions(-) create mode 100644 env_manager/gz_enviroment/test.json diff --git a/env_manager/env_manager/include/env_manager/env_base_interface.hpp b/env_manager/env_manager/include/env_manager/env_base_interface.hpp index d10e92b..355f0e5 100644 --- a/env_manager/env_manager/include/env_manager/env_base_interface.hpp +++ b/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 { diff --git a/env_manager/env_manager/include/env_manager/env_manager.hpp b/env_manager/env_manager/include/env_manager/env_manager.hpp index cf8db26..d821699 100644 --- a/env_manager/env_manager/include/env_manager/env_manager.hpp +++ b/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(); diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp index de222aa..50159bf 100644 --- a/env_manager/env_manager/src/env_manager.cpp +++ b/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 lock(m_env_mutex); // Добавьте эту строку + std::lock_guard 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 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 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 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 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; -} - } \ No newline at end of file diff --git a/env_manager/env_manager/src/run_env_manager.cpp b/env_manager/env_manager/src/run_env_manager.cpp index f28ed98..419e409 100644 --- a/env_manager/env_manager/src/run_env_manager.cpp +++ b/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; } diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt index 0584003..1aa81a5 100644 --- a/env_manager/gz_enviroment/CMakeLists.txt +++ b/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 diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp index 0f2b3ec..4730cb4 100644 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -23,15 +23,18 @@ public: protected: + + +private: std::unique_ptr m_tf2_broadcaster; std::shared_ptr m_gz_node; - std::vector 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(); }; } diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp index cde3079..35b50a1 100644 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ b/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(this); - m_gz_node = std::make_shared(); - 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(this); + m_gz_node = std::make_shared(); + + 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; + +} } diff --git a/env_manager/gz_enviroment/test.json b/env_manager/gz_enviroment/test.json new file mode 100644 index 0000000..e69de29