add gz_enviroment

env_manager/env_manager/include/env_manager/env_base_interface.hpp

@@ -0,0 +1,23 @@
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+
+
+namespace env_interface
+{
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class EnvBaseIntrerface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
+{
+public:
+    EnvBaseIntrerface() = default;
+
+    virtual ~EnvBaseIntrerface() = default;
+
+    virtual CallbackReturn on_init() = 0;
+    
+
+
+
+};
+} // namespace env_interface

env_manager/env_manager/src/env_manager.cpp

@@ -81,9 +81,16 @@ EnvManager::loadEnv(const std::string& env_name, const std::string& env_class_ty
         if (m_loader->isClassAvailable(env_class_type))
         {
             loaded_env = m_loader->createSharedInstance(env_class_type);
-        } else
+        } 
+        else
         {
-            RCLCPP_ERROR(this->get_logger(), "Loading enviroment is not available '%s'", env_name.c_str());
+            RCLCPP_ERROR(this->get_logger(), "Loading enviroment is not available '%s' with class type '%s'", 
+            env_name.c_str(), env_class_type.c_str());
+            RCLCPP_INFO(this->get_logger(), "Available classes");
+            for (const auto& available_class : m_loader->getDeclaredClasses())
+            {
+                RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str());
+            }
             return nullptr;
         }
     }

env_manager/env_manager/src/run_env_manager.cpp

@@ -15,7 +15,7 @@ int main(int argc, char ** argv)
     std::thread em_thread(
         [em]()
         {
-            em->loadEnv("gz_enviroment", "env_manager::GzEnviroment");
+            em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment");
             // while (rclcpp::ok())
             // {
             //     RCLCPP_INFO(em->get_logger(), "Env manager is running");

env_manager/gz_enviroment/CMakeLists.txt

@@ -5,11 +5,39 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+    rclcpp
+    rclcpp_lifecycle
+    tf2_ros
+    tf2_msgs
+    ros_gz
+    pluginlib
+    ignition-transport11
+    ignition-msgs8
+)
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+    find_package(${Dependency} REQUIRED)    
+endforeach()
+
+# TODO: generate_parameter_library
+
+
+
+add_library(${PROJECT_NAME} SHARED src/gz_enviroment.cpp)
+target_include_directories(${PROJECT_NAME} PUBLIC
+    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include/gz_enviroment>
+)
+ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_compile_definitions(${PROJECT_NAME} PRIVATE "GZENVIROMENT_BUILDING_DLL")
+#TODO: replace rclcpp_lifecycle with custom interface class
+pluginlib_export_plugin_description_file(rclcpp_lifecycle gz_enviroment.xml)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -23,4 +51,18 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+install(
+    DIRECTORY include/
+    DESTINATION include/${PROJECT_NAME}
+)
+
+install(TARGETS ${PROJECT_NAME}
+    EXPORT export_${PROJECT_NAME}
+    RUNTIME DESTINATION bin
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_package()

env_manager/gz_enviroment/gz_enviroment.xml

@@ -0,0 +1,7 @@
+<library path="gz_enviroment">
+    <class 
+        type="gz_enviroment::GzEnviroment" 
+        base_class_type="rclcpp_lifecycle::LifecycleNode">
+      <description>Gazebo enviroment for env_manager</description>
+    </class>
+  </library>

env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp

@@ -1,14 +1,36 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include "ros_gz_bridge/convert.hpp"
+// #include "ros_gz_bridge/convert.hpp"
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <gz/transport/Node.hh>
+#include "gz/msgs/pose_v.pb.h"
 
-namespace env_manager
+namespace gz_enviroment
 {
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
 class GzEnviroment : public rclcpp_lifecycle::LifecycleNode
 {
 public:
 
+    GzEnviroment();
+    CallbackReturn on_configure(const rclcpp_lifecycle::State&) override;
+    CallbackReturn on_activate(const rclcpp_lifecycle::State&) override;
+    CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override;
+    CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override;
+
+
+protected:
+    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);
+
     
 
 };

env_manager/gz_enviroment/package.xml

@@ -9,6 +9,12 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_msgs</depend>
+  <depend>ros_gz</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

env_manager/gz_enviroment/src/gz_enviroment.cpp

@@ -0,0 +1,56 @@
+#include "gz_enviroment/gz_enviroment.hpp"
+
+namespace gz_enviroment
+{
+
+GzEnviroment::GzEnviroment()
+: rclcpp_lifecycle::LifecycleNode("gz_enviroment") {}
+
+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);
+
+    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");
+    
+    
+    return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&)
+{
+    RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_deactivate");
+    
+    
+    return CallbackReturn::SUCCESS;
+}
+
+void onGzPoseSub(const gz::msgs::Pose_V& pose_v)
+{
+    
+}
+
+
+}
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(
+    gz_enviroment::GzEnviroment, rclcpp_lifecycle::LifecycleNode
+);