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 new file mode 100644 index 0000000..d10e92b --- /dev/null +++ b/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 diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp index dc0f3e8..de222aa 100644 --- a/env_manager/env_manager/src/env_manager.cpp +++ b/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; } } diff --git a/env_manager/env_manager/src/run_env_manager.cpp b/env_manager/env_manager/src/run_env_manager.cpp index 488807f..f28ed98 100644 --- a/env_manager/env_manager/src/run_env_manager.cpp +++ b/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"); diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt index 2d0c543..0584003 100644 --- a/env_manager/gz_enviroment/CMakeLists.txt +++ b/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( 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 + $ + $ +) +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() diff --git a/env_manager/gz_enviroment/gz_enviroment.xml b/env_manager/gz_enviroment/gz_enviroment.xml new file mode 100644 index 0000000..3729aa0 --- /dev/null +++ b/env_manager/gz_enviroment/gz_enviroment.xml @@ -0,0 +1,7 @@ + + + Gazebo enviroment for env_manager + + \ No newline at end of file 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 eaf5c1b..0f2b3ec 100644 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ b/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 +#include +#include +#include +#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 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); + }; diff --git a/env_manager/gz_enviroment/package.xml b/env_manager/gz_enviroment/package.xml index b906232..e08ac80 100644 --- a/env_manager/gz_enviroment/package.xml +++ b/env_manager/gz_enviroment/package.xml @@ -9,6 +9,12 @@ ament_cmake + rclcpp + rclcpp_lifecycle + tf2_ros + tf2_msgs + ros_gz + ament_lint_auto ament_lint_common diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp index e69de29..cde3079 100644 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ b/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(this); + m_gz_node = std::make_shared(); + 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 +); \ No newline at end of file