diff --git a/env_components/camera_component/CMakeLists.txt b/env_components/camera_component/CMakeLists.txt deleted file mode 100644 index 13e27e7..0000000 --- a/env_components/camera_component/CMakeLists.txt +++ /dev/null @@ -1,68 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(camera_component) - -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -set(CMAKE_CXX_STANDARD_REQUIRED True) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(image_transport REQUIRED) -find_package(ros_gz_bridge REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(env_manager REQUIRED) - -find_package(ignition-transport11 REQUIRED) -set(GZ_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) - -find_package(ignition-msgs8 REQUIRED) -set(GZ_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) - -set(GZ_TARGET_PREFIX ignition) - -add_library(ign_camera_component SHARED - src/ign_camera_component.cpp) -target_compile_definitions(ign_camera_component - PRIVATE "COMPOSITION_BUILDING_DLL") -target_link_libraries(ign_camera_component - ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core - ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core - ) -ament_target_dependencies(ign_camera_component - "image_transport" - "ros_gz_bridge" - "rclcpp" - "rclcpp_components" - "sensor_msgs" - "env_manager") -rclcpp_components_register_nodes(ign_camera_component "camera_component::IgnCameraComponent") -set(node_plugins "${node_plugins}camera_component::IgnCameraComponent;$\n") - -install(TARGETS ign_camera_component - DESTINATION lib) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/env_components/camera_component/package.xml b/env_components/camera_component/package.xml deleted file mode 100644 index 5f37451..0000000 --- a/env_components/camera_component/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - camera_component - 0.0.0 - TODO: Package description - splinter1984 - TODO: License declaration - - ament_cmake - pkg-config - - image_transport - ros_gz_bridge - rclcpp - rclcpp_components - sensor_msgs - env_manager - - ignition-msgs8 - ignition-transport11 - ignition-msgs8 - ignition-transport11 - - ament_lint_auto - ament_lint_common - - ament_cmake - - diff --git a/env_components/camera_component/src/ign_camera_component.cpp b/env_components/camera_component/src/ign_camera_component.cpp deleted file mode 100644 index bbf7d69..0000000 --- a/env_components/camera_component/src/ign_camera_component.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -namespace camera_component -{ - -class IgnCameraComponent: public env_manager::component_manager::PublisherComponent -{ -public: - explicit IgnCameraComponent(const rclcpp::NodeOptions &options) - : env_manager::component_manager::PublisherComponent(options) - { - auto topic_name = "/camera"; - _ign_node = std::make_shared(); - _ign_node->Subscribe(topic_name, &IgnCameraComponent::on_image, this); - } - -private: - void on_image(const ignition::msgs::Image& img) - { - sensor_msgs::msg::Image rimg; - ros_gz_bridge::convert_gz_to_ros(img, rimg); - populate_publication(rimg); - } - - std::shared_ptr _ign_node; -}; -} // namespace pub_component -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(camera_component::IgnCameraComponent) diff --git a/env_components/component_interfaces/CMakeLists.txt b/env_components/component_interfaces/CMakeLists.txt deleted file mode 100644 index 4ad69bf..0000000 --- a/env_components/component_interfaces/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(component_interfaces) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "srv/DetectObject.srv" - DEPENDENCIES std_msgs) - -ament_package() diff --git a/env_components/component_interfaces/srv/DetectObject.srv b/env_components/component_interfaces/srv/DetectObject.srv deleted file mode 100644 index cab64c2..0000000 --- a/env_components/component_interfaces/srv/DetectObject.srv +++ /dev/null @@ -1,10 +0,0 @@ -uint8 DETECT=0 -uint8 FOLLOW=1 -uint8 CANCEL=2 - -string object_name -uint8 req_kind ---- - -bool call_status -string error_msg diff --git a/env_components/scene_component/CMakeLists.txt b/env_components/scene_component/CMakeLists.txt deleted file mode 100644 index e756164..0000000 --- a/env_components/scene_component/CMakeLists.txt +++ /dev/null @@ -1,118 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(scene_component) - -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -set(CMAKE_CXX_STANDARD_REQUIRED True) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(image_transport REQUIRED) -find_package(ros_gz_bridge REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(env_manager REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(moveit_core REQUIRED) -find_package(moveit_msgs REQUIRED) -find_package(moveit_ros_planning REQUIRED) -find_package(moveit_ros_planning_interface REQUIRED) -find_package(component_interfaces REQUIRED) - -find_package(ignition-transport11 REQUIRED) -set(GZ_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) - -find_package(ignition-msgs8 REQUIRED) -set(GZ_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) - -set(GZ_TARGET_PREFIX ignition) - -message(STATUS "Compiling against Ignition Fortress") - - -add_library(ign_tf2_broadcaster_component SHARED - src/ign_tf2_broadcaster_component.cpp) -target_compile_definitions(ign_tf2_broadcaster_component - PRIVATE "COMPOSITION_BUILDING_DLL") -target_link_libraries(ign_tf2_broadcaster_component - ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core - ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core - ) -ament_target_dependencies(ign_tf2_broadcaster_component - "ros_gz_bridge" - "rclcpp" - "rclcpp_components" - "env_manager" - "geometry_msgs" - "tf2_ros") -rclcpp_components_register_nodes(ign_tf2_broadcaster_component "scene_component::IgnTf2Broadcaster") -set(node_plugins "${node_plugins}scene_component::IgnTf2Broadcaster;$\n") - -add_library(ign_detect_object_component SHARED - src/ign_detect_object_component.cpp) -target_compile_definitions(ign_detect_object_component - PRIVATE "COMPOSITION_BUILDING_DLL") -target_link_libraries(ign_detect_object_component - ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core - ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core - ) -ament_target_dependencies(ign_detect_object_component - "ros_gz_bridge" - "rclcpp" - "rclcpp_components" - "env_manager" - "geometry_msgs" - "tf2_ros" - "component_interfaces") - rclcpp_components_register_nodes(ign_detect_object_component "scene_component::IgnDetectOpjectService") - set(node_plugins "${node_plugins}scene_component::IgnDetectObjectService;$\n") - -add_library(moveit_scene_component SHARED - src/moveit_scene_component.cpp) -target_compile_definitions(moveit_scene_component - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(moveit_scene_component - "rclcpp" - "rclcpp_components" - "env_manager" - "geometry_msgs" - "tf2_ros" - "moveit_core" - "moveit_msgs" - "moveit_ros_planning" - "moveit_ros_planning_interface") -rclcpp_components_register_nodes(moveit_scene_component "scene_component::MoveitSceneComponent") -set(node_plugins "${node_plugins}scene_component::MoveitSceneComponent;$\n") - -install( - TARGETS - ign_detect_object_component - ign_tf2_broadcaster_component - moveit_scene_component - DESTINATION - lib) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/env_components/scene_component/package.xml b/env_components/scene_component/package.xml deleted file mode 100644 index 1dd44bd..0000000 --- a/env_components/scene_component/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - scene_component - 0.0.0 - TODO: Package description - splinter1984 - TODO: License declaration - - ament_cmake - pkg-config - - image_transport - ros_gz_bridge - rclcpp - rclcpp_components - geometry_msgs - env_manager - component_interfaces - - ignition-msgs8 - ignition-transport11 - ignition-msgs8 - ignition-transport11 - - ament_lint_auto - ament_lint_common - - ament_cmake - - diff --git a/env_components/scene_component/src/ign_detect_object_component.cpp b/env_components/scene_component/src/ign_detect_object_component.cpp deleted file mode 100644 index bf908d1..0000000 --- a/env_components/scene_component/src/ign_detect_object_component.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include "component_manager/service_component.hpp" -#include "component_interfaces/srv/detect_object.hpp" - -#include -#include -#include -#include - -#include -#include - -#define IGN_DEFAULT_BASE_FRAME "world" -#define IGN_DEFAULT_WORLD_NAME "mir" - -namespace scene_component -{ - -class IgnDetectObjectService: public - env_manager::component_manager::ServiceComponent -{ - public: - IgnDetectObjectService(const rclcpp::NodeOptions &options) - : env_manager::component_manager::ServiceComponent(options) - { - std::string ign_topic_name = "/world/" + std::string(IGN_DEFAULT_WORLD_NAME) + "/dynamic_pose/info"; - _tf2_broadcaster = std::make_unique(this); - _ign_node = std::make_shared(); - _ign_node->Subscribe(ign_topic_name, &IgnDetectObjectService::on_pose, this); - } - - void callback( - std::shared_ptr request, - std::shared_ptr response) - { - auto kind = static_cast(request->req_kind); - bool status = false; - switch(kind) - { - case DETECT: { - (void) kind; - break; - } case FOLLOW: { - auto ok = std::find(_follow_frames.begin(), _follow_frames.end(), - request->object_name) == _follow_frames.end(); - if (ok) - _follow_frames.push_back(request->object_name); - status = ok; - break; - } case CANCEL: { - auto it = std::find(_follow_frames.begin(), _follow_frames.end(), - request->object_name); - auto ok = it != _follow_frames.end(); - if (ok) - _follow_frames.erase(it); - status = ok; - break; - } - } - response->call_status = status; - } - - private: - enum DetectObjectReqKind - { - DETECT=0, - FOLLOW=1, - CANCEL=2 - }; - - void on_pose(const ignition::msgs::Pose_V &pose_v) - { - auto poses = pose_v.pose(); - for (const auto& obj: _follow_frames) - { - auto it = std::find_if(poses.begin(), poses.end(), - [&obj](const auto& pose){ return pose.name() == obj;}); - if (it == poses.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 = IGN_DEFAULT_BASE_FRAME; - 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; - - _tf2_broadcaster->sendTransform(t); - } - } - - std::vector _follow_frames; - std::unique_ptr _tf2_broadcaster; - std::shared_ptr _ign_node; - -}; - -} // namespace scene_component - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnDetectObjectService) diff --git a/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp b/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp deleted file mode 100644 index 1108962..0000000 --- a/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "component_manager/node_component.hpp" - -#include -#include -#include -#include - -#include -#include - -namespace scene_component -{ - -const std::string DEFAULT_NODE_NAME = "ign_tf2_broadcaster"; -const std::string DEFAULT_IGN_WORLD_NAME = "mir"; -const std::string DEFAULT_BASE_FRAME_NAME = "world"; - -class IgnTf2Broadcaster: public env_manager::component_manager::NodeComponent -{ -public: - IgnTf2Broadcaster(const rclcpp::NodeOptions & options) - : env_manager::component_manager::NodeComponent(options) - { - _follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6", "fork_gt"}; - std::string topic_name = "/world/" + DEFAULT_IGN_WORLD_NAME + "/dynamic_pose/info"; - // init broadcaster - _tf2_broadcaster = std::make_unique(*this); - _ign_node = std::make_shared(); - _ign_node->Subscribe(topic_name, &IgnTf2Broadcaster::on_pose, this); - } - -private: - void on_pose(const ignition::msgs::Pose_V& pose_v) - { - for (const auto &it: pose_v.pose()) - { - if (std::find(_follow_frames.begin(), _follow_frames.end(), it.name()) == _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 = DEFAULT_BASE_FRAME_NAME; - 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; - - RCLCPP_ERROR(get_logger(), "Provide transform for frame: %s", it.name().c_str()); - - _tf2_broadcaster->sendTransform(t); - } - } - - std::unique_ptr _tf2_broadcaster; - std::shared_ptr _ign_node; - std::vector _follow_frames; - -}; -} // namespace scene_component -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnTf2Broadcaster) diff --git a/env_components/scene_component/src/moveit_scene_component.cpp b/env_components/scene_component/src/moveit_scene_component.cpp deleted file mode 100644 index 44c3d1f..0000000 --- a/env_components/scene_component/src/moveit_scene_component.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include - -#include - -#include -#include - -#include - -#include -#include -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "tf2/exceptions.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" - -using namespace std::chrono_literals; - -namespace scene_component -{ - -const std::string DEFAULT_PLANNING_GOUP_NAME = "panda_arm"; -const std::string DEFAULT_SUBNODE_NS = "moveit_cpp_node"; -const std::string DEFAULT_BASE_FRAME = "world"; - -class MoveitSceneComponent: public env_manager::component_manager::NodeComponent -{ -public: - MoveitSceneComponent(const rclcpp::NodeOptions& options) - : env_manager::component_manager::NodeComponent(options) - { - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - - node_ = rclcpp::Node::make_shared(DEFAULT_SUBNODE_NS, "", node_options); - - moveit_cpp_ptr_ = std::make_shared(node_); - moveit_cpp_ptr_->getPlanningSceneMonitor()->providePlanningSceneService(); - planning_components_ = std::make_shared( - DEFAULT_PLANNING_GOUP_NAME, moveit_cpp_ptr_); - robot_model_ptr_ = moveit_cpp_ptr_->getRobotModel(); - - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - timer_ = this->create_wall_timer(1s, std::bind(&MoveitSceneComponent::on_timer, this)); - } - -private: - void on_timer() - { - auto available_frames = tf_buffer_->getAllFrameNames(); - auto robot_link_names = robot_model_ptr_->getLinkModelNames(); - - for (const auto &frame: available_frames) - { - if (std::find(robot_link_names.begin(), robot_link_names.end(), frame) != robot_link_names.end()) - continue; - - geometry_msgs::msg::TransformStamped t; - try { - t = tf_buffer_->lookupTransform( - DEFAULT_BASE_FRAME, frame, - tf2::TimePointZero); - } catch (const tf2::TransformException &ex) - { - RCLCPP_WARN(get_logger(), "Could not transform %s to %s: %s", - frame.c_str(), DEFAULT_BASE_FRAME.c_str(), ex.what()); - } - - moveit_msgs::msg::CollisionObject co; - co.header.frame_id = robot_model_ptr_->getRootLink()->getName(); - co.id = frame; - - //TODO: replace with stl model - shape_msgs::msg::SolidPrimitive obj; - obj.type = obj.BOX; - obj.dimensions = {0.1, 0.1, 0.1}; - - geometry_msgs::msg::Pose obj_pose; - obj_pose.position.x = t.transform.translation.x; - obj_pose.position.y = t.transform.translation.y; - obj_pose.position.z = t.transform.translation.z; - - obj_pose.orientation.x = t.transform.rotation.x; - obj_pose.orientation.y = t.transform.rotation.y; - obj_pose.orientation.z = t.transform.rotation.z; - obj_pose.orientation.w = t.transform.rotation.w; - - co.primitives.push_back(obj); - co.primitive_poses.push_back(obj_pose); - co.operation = co.ADD; - - { - planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr_->getPlanningSceneMonitor()); - scene->processCollisionObjectMsg(co); - } - } - } - - rclcpp::Node::SharedPtr node_; - std::shared_ptr moveit_cpp_ptr_; - moveit::core::RobotModelConstPtr robot_model_ptr_; - std::shared_ptr planning_components_; - std::shared_ptr tf_listener_{nullptr}; - std::unique_ptr tf_buffer_; - rclcpp::TimerBase::SharedPtr timer_{nullptr}; - - -}; -} // namesapce scene_component - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::MoveitSceneComponent) diff --git a/env_manager/CMakeLists.txt b/env_manager/CMakeLists.txt deleted file mode 100644 index fa98e56..0000000 --- a/env_manager/CMakeLists.txt +++ /dev/null @@ -1,144 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(env_manager) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -set(CMAKE_CXX_STANDARD_REQUIRED True) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(ENV_MANAGER_CONFIGURATION_FILES_DIRECTORY ${PROJECT_SOURCE_DIR}/config - CACHE PATH ".lua configuration files dir") - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(Boost REQUIRED) -find_package(rcl REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(rcutils REQUIRED) -find_package(glog 0.4.0 REQUIRED) -find_package(Lua 5.3 REQUIRED) - -add_library(env_manager SHARED - src/component_manager/component_manager.cpp - src/config/config_file_resolver.cpp - src/config/lua_file_resolver.cpp - src/config/options.cpp - ) - -configure_file( - ${PROJECT_SOURCE_DIR}/include/config/config.hpp.cmake - ${PROJECT_BINARY_DIR}/include/config/config.hpp) - -target_include_directories(env_manager PUBLIC - $ - $) -ament_target_dependencies( - env_manager - Boost - rcl - glog - Lua - class_loader - rclcpp - rclcpp_components -) - -target_include_directories(env_manager PUBLIC - $) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(env_manager PRIVATE "COMPONENT_MANAGER_BUILDING_LIBRARY") - -if(NOT WIN32) - ament_environment_hooks( - "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}") -endif() - -add_executable(env_manager_node - src/main.cpp - ) -target_link_libraries(env_manager_node env_manager lua glog) -ament_target_dependencies( - env_manager_node - Boost - rcl - rclcpp - glog - Lua -) - -install(DIRECTORY config DESTINATION config) - -install( - DIRECTORY include/ - DESTINATION include -) -install( - TARGETS env_manager - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - TARGETS env_manager_node - DESTINATION lib/${PROJECT_NAME} - ) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - set(ament_cmake_cppcheck_FOUND TRUE) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_pep257_FOUND TRUE) - set(ament_cmake_lint_cmake_FOUND TRUE) - set(ament_cmake_xmllint_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gtest REQUIRED) - - - #include_directories(test) - #ament_add_gtest(env_manager_test test/general.cpp) - #target_link_libraries(env_manager_test env_manager lua glog) - - #set(TEST_TEST_FILES_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_files) - #set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) - - #install(DIRECTORY - # ${TEST_TEST_FILES_DIR} - # DESTINATION share/${PROJECT_NAME} - #) -endif() - -ament_export_include_directories( - include -) -ament_export_libraries( - env_manager -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_export_dependencies(rcutils) - -ament_package() diff --git a/env_manager/README.md b/env_manager/README.md deleted file mode 100644 index 17bff87..0000000 --- a/env_manager/README.md +++ /dev/null @@ -1,149 +0,0 @@ -## Environment Manager -Environment Manager is a package that allow you to create custom environments based on *component*, switch between different environments, synchronization of different environments with each other. - -![](docs/assets/env_manager.png) - -### Getting Started -To get a local copy up and running follow these simple example steps. - -#### Prerequisites -```bash -# install glog -sudo apt-get install libgoogle-glog-dev - -#install lua -wget http://www.lua.org/ftp/lua-5.3.1.tar.gz -tar zxf lua-5.3.1.tar.gz -sudo make -C lua-5.3.1 linux && sudo make -C lua-5.3.1 install -``` -### Config -Configuration is done by lua. -```lua - --- require configuration table -ENV_MANAGER = { - -- dictionary of environments - environments = { - -- environment to upload with manager - example_environment = { - -- namespace in which all components will be - namespace = "", - -- dictionary of components needed to upload for current environment - components = { - -- node match one of node in ENV_MANAGER.nodes - example_node = { - -- example libpub_component.so - lib = "" - -- name of class which implement example_node logic - class = "" - } - }, - }, - }, - -- dictionary of nodes, that will be created by env_manager - nodes = { - -- node for initialization by env_manger - example_node = { - -- node name - name = "", - -- one node can produce only one topic/service - type = "", - -- example "std_msgs/String", - msg_type = "", - }, - }, -} - --- return configuration table -return ENV_MANAGER -``` -You can safely use all the feature of `lua` to write configuration. -### Usage -Create component library. -```c++ -#include "component_manager/publisher_component.hpp" -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include - -namespace pub_component -{ -using namespace std::chrono_literals; - -class Publisher: public env_manager::component_manager::PublisherComponent -{ -public: - explicit Publisher(const rclcpp::NodeOptions &options) - : env_manager::component_manager::PublisherComponent(options) - { - timer_ = create_wall_timer(1s, std::bind(&Publisher::on_timer, this)); - } - -protected: - void on_timer() - { - auto msg = std::make_unique(); - msg->data = "Hello World!"; - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg->data.c_str()); - std::flush(std::cout); - - this->populate_publication(*msg.get()); - } - - -private: - rclcpp::TimerBase::SharedPtr timer_; -}; -} // namespace pub_component -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(pub_component::Publisher) -``` -```cmake -add_library(pub_component SHARED - src/pub_component.cpp) -target_compile_definitions(pub_component - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(pub_component - "rclcpp" - "env_manager" - "rclcpp_components" - "std_msgs") -rclcpp_components_register_nodes(pub_component "pub_component::Publisher") -set(node_plugins "${node_plugins}pub_component::Publisher;$\n") -``` -Add component_library to your configuration file. -```lua -ENV_MANAGER = { - environments = { - example_environment = { - namespace = "", - components = { - example_node = { - lib = "libpub_component.so" - class = "pub_component::Publisher" - } - }, - }, - }, - nodes = { - example_node = { - name = "", - type = "Publisher", - msg_type = "std_msgs/String", - }, - }, -} -return ENV_MANAGER -``` -Start env_manager -```bash -source install/setup.bash -ros2 run env_manager env_manager_node -``` -### License -Distributed under the Apache 2.0 License. See LICENSE for more information. diff --git a/env_manager/config/config.lua b/env_manager/config/config.lua deleted file mode 100644 index a778f16..0000000 --- a/env_manager/config/config.lua +++ /dev/null @@ -1,19 +0,0 @@ --- env_manager configuraiton file -include "nodes.lua" -include "utils/utils.lua" - --- include environments configs -include "environments/simulator.lua" -include "environments/ground_true.lua" - --- env_manager configuration -ENV_MANAGER = { - environments = { - GROUND_TRUE - }, - nodes = NODES -} - -check_nodes(ENV_MANAGER) - -return ENV_MANAGER diff --git a/env_manager/config/environments/ground_true.lua b/env_manager/config/environments/ground_true.lua deleted file mode 100644 index 624b6c7..0000000 --- a/env_manager/config/environments/ground_true.lua +++ /dev/null @@ -1,14 +0,0 @@ --- environment configuraiton -GROUND_TRUE = { - namespace = "ground_true", - components = { - -- camera_node = { - -- lib = "libign_camera_component.so", - -- class = "camera_component::IgnCameraComponent" - -- }, - scene_component = { - lib = "libign_tf2_broadcaster_component.so", - class = "scene_component::IgnTf2Broadcaster" - }, - } -} diff --git a/env_manager/config/environments/simulator.lua b/env_manager/config/environments/simulator.lua deleted file mode 100644 index 3e36699..0000000 --- a/env_manager/config/environments/simulator.lua +++ /dev/null @@ -1,19 +0,0 @@ --- environment configuration -SIMULATOR = { - namespace = "simulator_env", - components = { - --[[ - talker_node = { - lib = "libpub_component.so", - class = "pub_component::Publisher", - }, - service_node = { - lib = "libsrv_component.so", - class = "srv_component::Service" - },]]-- - camera_node = { - lib = "libign_camera_component.so", - class = "camera_component::IgnCameraComponent" - }, - } -} diff --git a/env_manager/config/nodes.lua b/env_manager/config/nodes.lua deleted file mode 100644 index 413522a..0000000 --- a/env_manager/config/nodes.lua +++ /dev/null @@ -1,12 +0,0 @@ -NODES = { - camera_node = { - name = "camera_node", - type = "Publisher", - msg_type = "sensor_msgs/Image" - }, - scene_component = { - name = "scene_component", - type = "Publisher", - msg_type = "tf2_msgs/tf_message" - }, -} diff --git a/env_manager/config/utils/utils.lua b/env_manager/config/utils/utils.lua deleted file mode 100644 index ff746d1..0000000 --- a/env_manager/config/utils/utils.lua +++ /dev/null @@ -1,9 +0,0 @@ -function check_nodes(config) - for env, cfg in pairs(config.environments) do - for comp, opt in pairs(cfg.components) do - assert(config.nodes[comp] ~= nil, "not all nodes presented.") - assert(opt.lib ~= nil, "not library provided.") - assert(opt.class ~= nil, "not class provided.") - end - end -end diff --git a/env_manager/docs/assets/env_manager.png b/env_manager/docs/assets/env_manager.png deleted file mode 100644 index 834d492..0000000 Binary files a/env_manager/docs/assets/env_manager.png and /dev/null differ diff --git a/env_manager/env_interface/CMakeLists.txt b/env_manager/env_interface/CMakeLists.txt new file mode 100644 index 0000000..867b8e1 --- /dev/null +++ b/env_manager/env_interface/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.8) +project(env_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(INCLUDE_DEPENDS + rclcpp_lifecycle +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN LISTS INCLUDE_DEPENDS) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(${PROJECT_NAME} SHARED + src/env_interface_base.cpp) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${INCLUDE_DEPENDS}) + +target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIROMENT_INTERFACE_BUILDING_DLL") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install( + DIRECTORY include/ + DESTINATION include/env_interface +) +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${INCLUDE_DEPENDS}) + +ament_package() diff --git a/env_manager/LICENSE b/env_manager/env_interface/LICENSE similarity index 99% rename from env_manager/LICENSE rename to env_manager/env_interface/LICENSE index 2431aec..d645695 100644 --- a/env_manager/LICENSE +++ b/env_manager/env_interface/LICENSE @@ -1,3 +1,4 @@ + Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ @@ -186,7 +187,7 @@ same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright 2022 Robossembler + Copyright [yyyy] [name of copyright owner] Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -199,4 +200,3 @@ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. - diff --git a/env_manager/env_interface/include/env_interface/env_interface.hpp b/env_manager/env_interface/include/env_interface/env_interface.hpp new file mode 100644 index 0000000..09fb681 --- /dev/null +++ b/env_manager/env_interface/include/env_interface/env_interface.hpp @@ -0,0 +1,11 @@ +#include "env_interface/env_interface_base.hpp" + +namespace env_interface +{ +class EnvInterface : public EnvInterfaceBase +{ +public: + EnvInterface() = default; + virtual ~EnvInterface() = default; +}; +} // namespace env_interface diff --git a/env_manager/env_interface/include/env_interface/env_interface_base.hpp b/env_manager/env_interface/include/env_interface/env_interface_base.hpp new file mode 100644 index 0000000..5977b71 --- /dev/null +++ b/env_manager/env_interface/include/env_interface/env_interface_base.hpp @@ -0,0 +1,40 @@ +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include +#include + +namespace env_interface +{ + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +// template +class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +{ +public: + EnvInterfaceBase() = default; + + virtual ~EnvInterfaceBase() = default; + + virtual CallbackReturn init(const std::string& t_env_name, const std::string& t_namespace = "", + const rclcpp::NodeOptions& t_node_options = rclcpp::NodeOptions()); + + const rclcpp_lifecycle::State& configure(); + + virtual CallbackReturn on_init() = 0; + + std::shared_ptr getNode(); + std::shared_ptr getNode() const; + + const rclcpp_lifecycle::State& getState() const; + + // virtual void setActionSpace(const A_space& action, const std::size_t& size); + +protected: + // A_space action_space; + // S_space observation_space; + +private: + std::shared_ptr m_node; +}; +} // namespace env_interface diff --git a/env_components/component_interfaces/package.xml b/env_manager/env_interface/package.xml similarity index 56% rename from env_components/component_interfaces/package.xml rename to env_manager/env_interface/package.xml index b9b61c0..d545986 100644 --- a/env_components/component_interfaces/package.xml +++ b/env_manager/env_interface/package.xml @@ -1,18 +1,16 @@ - component_interfaces + env_interface 0.0.0 TODO: Package description - splinter1984 + bill-finger Apache-2.0 ament_cmake - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - std_msgs + ament_lint_auto + ament_lint_common ament_cmake diff --git a/env_manager/env_interface/src/env_interface_base.cpp b/env_manager/env_interface/src/env_interface_base.cpp new file mode 100644 index 0000000..c4e2d26 --- /dev/null +++ b/env_manager/env_interface/src/env_interface_base.cpp @@ -0,0 +1,45 @@ +#include "env_interface/env_interface_base.hpp" + +namespace env_interface +{ + +CallbackReturn EnvInterfaceBase::init(const std::string& t_env_name, const std::string& t_namespace, + const rclcpp::NodeOptions& t_node_options) +{ + m_node = std::make_shared(t_env_name, t_namespace, t_node_options, false); + + if (on_init() == CallbackReturn::FAILURE) + { + return CallbackReturn::FAILURE; + } + + m_node->register_on_configure(std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1)); + m_node->register_on_activate(std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1)); + m_node->register_on_cleanup(std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1)); + m_node->register_on_deactivate(std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1)); + m_node->register_on_error(std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1)); + m_node->register_on_shutdown(std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1)); + + return CallbackReturn::SUCCESS; +} + +const rclcpp_lifecycle::State& EnvInterfaceBase::configure() +{ + return getNode()->configure(); +} + +std::shared_ptr EnvInterfaceBase::getNode() +{ + if (!m_node.get()) + { + throw std::runtime_error("Lifecycle node hasn't been initialized yet"); + } + return m_node; +} + +const rclcpp_lifecycle::State& EnvInterfaceBase::getState() const +{ + return m_node->get_current_state(); +} + +} // namespace env_interface diff --git a/env_manager/env_manager/CMakeLists.txt b/env_manager/env_manager/CMakeLists.txt new file mode 100644 index 0000000..5f2b07c --- /dev/null +++ b/env_manager/env_manager/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(env_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(INCLUDE_DEPENDS + rclcpp + rclcpp_lifecycle + env_manager_interfaces + pluginlib + env_interface +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dep IN ITEMS ${INCLUDE_DEPENDS}) + find_package(${Dep} REQUIRED) +endforeach() +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + + + +add_library(${PROJECT_NAME} SHARED + src/env_manager.cpp +) +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} ${INCLUDE_DEPENDS}) +target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIRONMENT_MANAGER_BUILDING_DLL") + +add_executable(run_env_manager src/run_env_manager.cpp) +target_include_directories(run_env_manager PRIVATE include) +target_link_libraries(run_env_manager ${PROJECT_NAME}) +ament_target_dependencies(run_env_manager ${INCLUDE_DEPENDS}) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +install(TARGETS run_env_manager + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_include_directories( + include +) +ament_export_dependencies( + ${INCLUDE_DEPENDS} +) + +ament_package() diff --git a/env_manager/env_manager/LICENSE b/env_manager/env_manager/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/env_manager/env_manager/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/env_manager/env_manager/config/test_params.yaml b/env_manager/env_manager/config/test_params.yaml new file mode 100644 index 0000000..8ba46b3 --- /dev/null +++ b/env_manager/env_manager/config/test_params.yaml @@ -0,0 +1,9 @@ +env_manager: + ros__parameters: + gz_enviroment: + type: gz_enviroment/GzEnviroment + + + gz_enviroment: + ros__parameters: + mesh_name: "monkey" \ No newline at end of file diff --git a/env_manager/env_manager/include/env_manager/env_manager.hpp b/env_manager/env_manager/include/env_manager/env_manager.hpp new file mode 100644 index 0000000..692478f --- /dev/null +++ b/env_manager/env_manager/include/env_manager/env_manager.hpp @@ -0,0 +1,86 @@ +#ifndef __ENV_MANAGER_HPP__ +#define __ENV_MANAGER_HPP__ + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "env_manager_interfaces/srv/load_env.hpp" +#include "env_manager_interfaces/srv/configure_env.hpp" +#include "env_manager_interfaces/srv/unload_env.hpp" + +#include "env_interface/env_interface.hpp" + +#include "pluginlib/class_loader.hpp" + +namespace env_manager +{ +using EnvInterface = env_interface::EnvInterface; +using EnvInterfaceSharedPtr = std::shared_ptr; +using EnvStateReturnType = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +struct EnvSpec +{ + std::string name; + std::string type; + EnvInterfaceSharedPtr env_ptr; +}; + +class EnvManager : public rclcpp::Node +{ +public: + // EnvManager(rclcpp::Executor::SharedPtr executor, + // const std::string & json_config_path, + // const std::string & node_name = "env_manager", + // const std::string & node_namespace = "", + // rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() + // .allow_undeclared_parameters(true) + // .automatically_declare_parameters_from_overrides(true)); + EnvManager(rclcpp::Executor::SharedPtr executor, const std::string& node_name = "env_manager", + const std::string& node_namespace = "", + rclcpp::NodeOptions& node_options = rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); + virtual ~EnvManager() = default; + + // EnvInterfaceSharedPtr loadEnv(const std::string& env_name); + EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type); + EnvStateReturnType unloadEnv(const std::string& env_name); + 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(); + rclcpp::Node::SharedPtr getNode(); + void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response); + + void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, + env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response); + + void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response); + +private: + rclcpp::Node::SharedPtr m_node; + std::mutex m_env_mutex; + std::vector m_active_envs; + rclcpp::Service::SharedPtr m_load_env_srv; + rclcpp::Service::SharedPtr m_configure_env_srv; + rclcpp::Service::SharedPtr m_unload_env_srv; + + rclcpp::CallbackGroup::SharedPtr m_callback_group; + rclcpp::Executor::SharedPtr m_executor; + + std::shared_ptr> m_loader; +}; +} // namespace env_manager +#endif // __ENV_MANAGER_HPP__ \ No newline at end of file diff --git a/env_manager/env_manager/package.xml b/env_manager/env_manager/package.xml new file mode 100644 index 0000000..5abbdf0 --- /dev/null +++ b/env_manager/env_manager/package.xml @@ -0,0 +1,23 @@ + + + + env_manager + 0.0.0 + TODO: Package description + banana + Apache-2.0 + + ament_cmake + + env_manager_interfaces + rclcpp + rclcpp_lifecycle + env_inteface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp new file mode 100644 index 0000000..00231da --- /dev/null +++ b/env_manager/env_manager/src/env_manager.cpp @@ -0,0 +1,172 @@ +#include "env_manager/env_manager.hpp" +#include "nlohmann/json.hpp" + +static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE = + "env_interface"; +static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAME = + "env_interface::EnvInterface"; + +namespace env_manager { +EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor, + const std::string &node_name, + const std::string &node_namespace, + rclcpp::NodeOptions &options) + : rclcpp::Node(node_name, node_namespace, options), m_executor(executor), + m_loader( + std::make_shared>( + ENV_INTERFACE_BASE_CLASS_NAMESPACE, + ENV_INTERFACE_BASE_CLASS_NAME)) { + initServices(); +} + +// EnvManager::EnvManager( +// rclcpp::Executor::SharedPtr executor, +// const std::string & json_config_path, +// const std::string & node_name, +// const std::string & node_namespace, +// rclcpp::NodeOptions & options) +// : rclcpp::Node(node_name, node_namespace, options), +// m_executor(executor), +// m_loader(std::make_shared>( +// ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME)) +// { +// initServices(); +// } + +void EnvManager::initServices() { + using namespace std::placeholders; + + m_load_env_srv = create_service( + "~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2)); + m_configure_env_srv = + create_service( + "~/configure_env", + std::bind(&EnvManager::configureEnv_cb, this, _1, _2)); + m_unload_env_srv = create_service( + "~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2)); +} + +rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; } + +void 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); + response->ok = loadEnv(request->name, request->type).get() != nullptr; +} + +void EnvManager::configureEnv_cb( + const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, + env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) { + std::lock_guard guard(m_env_mutex); + 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); + response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS; +} + +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; + if (enviroment.env_ptr->init(enviroment.name, get_namespace()) == + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: + CallbackReturn::ERROR) { + RCLCPP_ERROR(get_logger(), + "Enviroment with name %s cannot been initialized", + enviroment.name.c_str()); + return nullptr; + } + + m_active_envs.push_back(enviroment); + m_executor->add_node( + enviroment.env_ptr->getNode()->get_node_base_interface()); + return enviroment.env_ptr; +} + +EnvInterfaceSharedPtr EnvManager::loadEnv(const std::string &env_name, + const std::string &env_class_type) { + EnvInterfaceSharedPtr loaded_env = nullptr; + RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str()); + try { + if (m_loader->isClassAvailable(env_class_type)) { + loaded_env = m_loader->createSharedInstance(env_class_type); + } else { + 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; + } + } catch (const pluginlib::PluginlibException &e) { + RCLCPP_ERROR( + get_logger(), + "Error happened during creation of enviroment '%s' with type '%s':\n%s", + env_name.c_str(), env_class_type.c_str(), e.what()); + return nullptr; + } + EnvSpec enviroment; + enviroment.name = env_name; + enviroment.type = env_class_type; + enviroment.env_ptr = loaded_env; + return addEnv(enviroment); +} + +EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) { + auto it = std::find_if( + m_active_envs.begin(), m_active_envs.end(), + [&env_name](const EnvSpec &env) { return env.name == env_name; }); + + if (it != m_active_envs.end()) { + m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface()); + m_active_envs.erase(it); + return EnvStateReturnType::SUCCESS; + } else { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + env_name.c_str()); + return EnvStateReturnType::ERROR; + } + return EnvStateReturnType::FAILURE; +} + +// EnvInterfaceSharedPtr +// EnvManager::loadEnv(const std::string env_name) +// { +// std::string env_class_type = "some_default_namespace::" + env_name; + +// return loadEnv(env_name, env_class_type); +// } + +EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) { + std::lock_guard guard(m_env_mutex); + + auto it = std::find_if( + m_active_envs.begin(), m_active_envs.end(), + [&env_name](const EnvSpec &env) { return env.name == env_name; }); + + if (it == m_active_envs.end()) { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + env_name.c_str()); + return EnvStateReturnType::FAILURE; + } + it->env_ptr->configure(); + // it->env_ptr->getNode()->configure(); + // it->env_ptr->activate(); + it->env_ptr->getNode()->activate(); + + return EnvStateReturnType::SUCCESS; +} + +} // namespace env_manager diff --git a/env_manager/env_manager/src/run_env_manager.cpp b/env_manager/env_manager/src/run_env_manager.cpp new file mode 100644 index 0000000..1506122 --- /dev/null +++ b/env_manager/env_manager/src/run_env_manager.cpp @@ -0,0 +1,21 @@ +#include "rclcpp/rclcpp.hpp" +#include "env_manager/env_manager.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + std::shared_ptr executor = std::make_shared(); + std::string manager_node_name = "env_manager"; + + auto em = std::make_shared(executor, manager_node_name); + + RCLCPP_INFO(em->get_logger(), "Env manager is starting"); + + em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment"); + em->configureEnv("gz_enviroment"); + + executor->add_node(em); + executor->spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/env_manager/env_manager_interfaces/CMakeLists.txt b/env_manager/env_manager_interfaces/CMakeLists.txt new file mode 100644 index 0000000..3bf6ce3 --- /dev/null +++ b/env_manager/env_manager_interfaces/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(env_manager_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + srv/ConfigureEnv.srv + srv/LoadEnv.srv + srv/UnloadEnv.srv + msg/EnvState.msg + DEPENDENCIES lifecycle_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/env_manager/env_manager_interfaces/LICENSE b/env_manager/env_manager_interfaces/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/env_manager/env_manager_interfaces/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/env_manager/env_manager_interfaces/msg/EnvState.msg b/env_manager/env_manager_interfaces/msg/EnvState.msg new file mode 100644 index 0000000..fa4c82e --- /dev/null +++ b/env_manager/env_manager_interfaces/msg/EnvState.msg @@ -0,0 +1,4 @@ +string name +string type +string plugin_name +lifecycle_msgs/State state diff --git a/env_manager/env_manager_interfaces/package.xml b/env_manager/env_manager_interfaces/package.xml new file mode 100644 index 0000000..4fb3920 --- /dev/null +++ b/env_manager/env_manager_interfaces/package.xml @@ -0,0 +1,27 @@ + + + + env_manager_interfaces + 0.0.0 + TODO: Package description + solid-sinusoid + Apache-2.0 + + ament_cmake + rosidl_default_generators + + builtin_interfaces + lifecycle_msgs + + builtin_interfaces + lifecycle_msgs + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + ament_cmake + + diff --git a/env_manager/env_manager_interfaces/srv/ConfigureEnv.srv b/env_manager/env_manager_interfaces/srv/ConfigureEnv.srv new file mode 100644 index 0000000..4292382 --- /dev/null +++ b/env_manager/env_manager_interfaces/srv/ConfigureEnv.srv @@ -0,0 +1,3 @@ +string name +--- +bool ok \ No newline at end of file diff --git a/env_manager/env_manager_interfaces/srv/LoadEnv.srv b/env_manager/env_manager_interfaces/srv/LoadEnv.srv new file mode 100644 index 0000000..d6d46cc --- /dev/null +++ b/env_manager/env_manager_interfaces/srv/LoadEnv.srv @@ -0,0 +1,4 @@ +string name +string type +--- +bool ok \ No newline at end of file diff --git a/env_manager/env_manager_interfaces/srv/UnloadEnv.srv b/env_manager/env_manager_interfaces/srv/UnloadEnv.srv new file mode 100644 index 0000000..4292382 --- /dev/null +++ b/env_manager/env_manager_interfaces/srv/UnloadEnv.srv @@ -0,0 +1,3 @@ +string name +--- +bool ok \ No newline at end of file diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt new file mode 100644 index 0000000..90ecb8d --- /dev/null +++ b/env_manager/gz_enviroment/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.8) +project(gz_enviroment) + +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 + ros_gz_bridge + env_interface + ignition-gazebo6 +) + +# 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(env_interface gz_enviroment.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + 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/LICENSE b/env_manager/gz_enviroment/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/env_manager/gz_enviroment/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/env_manager/gz_enviroment/gz_enviroment.xml b/env_manager/gz_enviroment/gz_enviroment.xml new file mode 100644 index 0000000..59d0901 --- /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 new file mode 100644 index 0000000..852e875 --- /dev/null +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -0,0 +1,47 @@ +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +// #include "ros_gz_bridge/convert.hpp" +#include +#include +#include +#include +#include +#include "gz/msgs/pose_v.pb.h" +#include "env_interface/env_interface.hpp" + +#include +#include +#include + +namespace gz_enviroment +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class GzEnviroment : public env_interface::EnvInterface +{ +public: + GzEnviroment(); + CallbackReturn on_init() override; + 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: + void onGzPoseSub(const gz::msgs::Pose_V& pose_v); + + bool doGzSpawn(); + +private: + std::unique_ptr m_tf2_broadcaster; + std::shared_ptr m_gz_node; + std::vector m_follow_frames; + std::string m_topic_name; + std::string m_service_spawn; + std::string m_world_name; + + std::string getGzWorldName(); + std::string createGzModelString(const std::vector& pose, const std::string& mesh_filepath, + const std::string& name); +}; +} // namespace gz_enviroment diff --git a/env_manager/gz_enviroment/package.xml b/env_manager/gz_enviroment/package.xml new file mode 100644 index 0000000..65f99ad --- /dev/null +++ b/env_manager/gz_enviroment/package.xml @@ -0,0 +1,25 @@ + + + + gz_enviroment + 0.0.0 + TODO: Package description + bill-finger + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_lifecycle + tf2_ros + tf2_msgs + ros_gz + env_interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp new file mode 100644 index 0000000..25d1d03 --- /dev/null +++ b/env_manager/gz_enviroment/src/gz_enviroment.cpp @@ -0,0 +1,180 @@ +#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 +#include +#include + +#include +#include + +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(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(); + 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(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", "monkey" }; + 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 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& pose, const std::string& mesh_filepath, + const std::string& name) +{ + std::string model_template = std::string("") + "" + + "" + "" + 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]) + "" + + "" + "file://" + mesh_filepath + + "" + "" + "" + + "file://" + mesh_filepath + "" + + "" + "" + "" + ""; + return model_template; +} + +} // namespace gz_enviroment + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment, env_interface::EnvInterface); \ No newline at end of file diff --git a/env_manager/include/component_manager/component_template.hpp b/env_manager/gz_enviroment/test.json similarity index 100% rename from env_manager/include/component_manager/component_template.hpp rename to env_manager/gz_enviroment/test.json diff --git a/env_manager/include/component_manager/client_component.hpp b/env_manager/include/component_manager/client_component.hpp deleted file mode 100644 index 07e34b2..0000000 --- a/env_manager/include/component_manager/client_component.hpp +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_ -#define ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_ - -#include "component_manager/visibility_control.h" - -#include "rclcpp/rclcpp.hpp" - -#include -#include -#include -#include - - -namespace env_manager -{ -namespace component_manager -{ - -using namespace std::chrono_literals; - -const std::string DEFAULT_CLIENT_NODE_NAME = "env_manager_client_node"; -const std::string DEFAULT_CLIENT_NAME = "client"; - -template -class ClientComponent: public rclcpp::Node -{ -public: - explicit ClientComponent(const rclcpp::NodeOptions& options) - : Node(DEFAULT_CLIENT_NODE_NAME, options) - { - _client = create_client(DEFAULT_CLIENT_NAME, 10); - } - - virtual void callback( - typename rclcpp::Client::SharedFuture future) = 0; - - void populate_request( - const std::shared_ptr& request) - { - while (!_client->wait_for_service(1s)) - { - if (rclcpp::ok()) - { - RCLCPP_ERROR(this->get_logger(), - "Client interrupted while waiting for service. Terminating..."); - } - } - - auto result_future = _client->async_send_request( - request, std::bind(&ClientComponent::callback, - this, std::placeholders::_1)); - } - -private: - typename rclcpp::Client::SharedPtr _client; -}; - -} // namespace component_manager -} // namespace env_manager - -#endif // ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/component_manager.hpp b/env_manager/include/component_manager/component_manager.hpp deleted file mode 100644 index 815044d..0000000 --- a/env_manager/include/component_manager/component_manager.hpp +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_ -#define COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_ - -#include "component_manager/visibility_control.h" -#include "config/options.hpp" - -#include -#include -#include -#include - -#include - - -namespace env_manager -{ -namespace component_manager -{ - -/** - * This class implements the system for managing and configuring loaded components. - * - * It is assumed that the loaded components are inherited from the classes - * provided in the library for each node type. - */ -class ComponentManager -{ -public: - ComponentManager(std::weak_ptr executor); - - void register_components( - const std::map &comps, - const std::map &nodes, - const std::string& ns); - - void remove_components_from_executor(); - - void remap_components_namespace(const std::string& ns); - -private: - std::weak_ptr _executor; - std::vector _loaders; - std::map _node_wrappers; - std::map _nodes; -}; - -} // namespace env_manager -} // namespace component_manager - -#endif // COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_ diff --git a/env_manager/include/component_manager/node_component.hpp b/env_manager/include/component_manager/node_component.hpp deleted file mode 100644 index 482b698..0000000 --- a/env_manager/include/component_manager/node_component.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ -#define ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ - -#include "component_manager/visibility_control.h" - -#include "rclcpp/node.hpp" - -namespace env_manager -{ -namespace component_manager -{ - -const std::string DEFAULT_NODE_NODE_NAME = "env_manager_node"; - -//TODO: interhit all another components from this node_component -class NodeComponent: public rclcpp::Node -{ -public: - explicit NodeComponent(const rclcpp::NodeOptions& options) - :Node(DEFAULT_NODE_NODE_NAME, options) - { - auto ret = rcutils_logging_set_logger_level( - get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL); - if (ret != RCUTILS_RET_OK) - { - RCLCPP_ERROR(get_logger(), - "Error setting severity: %s", rcutils_get_error_string().str); - rcutils_reset_error(); - } - } -}; - -} // namesapce component_manager -} // namesapce env_manager - -#endif // ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ diff --git a/env_manager/include/component_manager/publisher_component.hpp b/env_manager/include/component_manager/publisher_component.hpp deleted file mode 100644 index 5cec622..0000000 --- a/env_manager/include/component_manager/publisher_component.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_ -#define ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_ - -#include "component_manager/visibility_control.h" - -#include "rclcpp/rclcpp.hpp" - -namespace env_manager -{ -namespace component_manager -{ - -const std::string DEFAULT_PUB_NODE_NAME = "env_manager_pub_node"; -const std::string DEFAULT_PUB_TOPIC_NAME = "pub_topic"; - -template -class PublisherComponent: public rclcpp::Node -{ -public: - explicit PublisherComponent(const rclcpp::NodeOptions& options) - : Node(DEFAULT_PUB_NODE_NAME, options) - { - _pub = create_publisher(DEFAULT_PUB_TOPIC_NAME, 10); - auto ret = rcutils_logging_set_logger_level( - get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL); - if (ret != RCUTILS_RET_OK) - { - RCLCPP_ERROR(get_logger(), - "Error setting severity: %s", rcutils_get_error_string().str); - rcutils_reset_error(); - } - } - - void populate_publication(const MessageT& msg) - { - _pub->publish(std::move(msg)); - } - -private: - typename rclcpp::Publisher::SharedPtr _pub; -}; - -} // namespace component_manager -} // namespace env_manager - -#endif // ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/service_component.hpp b/env_manager/include/component_manager/service_component.hpp deleted file mode 100644 index 86bac7c..0000000 --- a/env_manager/include/component_manager/service_component.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_ -#define ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_ - -#include "component_manager/visibility_control.h" - -#include - -namespace env_manager -{ -namespace component_manager -{ - -const std::string DEFAULT_SERVICE_NDOE_NAME = "env_manager_service_node"; -const std::string DEFAULT_SERVICE_NAME = "service"; - -template -class ServiceComponent: public rclcpp::Node -{ -public: - explicit ServiceComponent(const rclcpp::NodeOptions& options) - : Node(DEFAULT_SERVICE_NDOE_NAME, options) - { - _service = create_service( - DEFAULT_SERVICE_NAME, std::bind( - &ServiceComponent::callback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - virtual void callback(std::shared_ptr request, - std::shared_ptr response) = 0; - -private: - typename rclcpp::Service::SharedPtr _service; -}; - -} // namespace component_manager -} // namespace env_manager - -#endif // ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/subscriber_component.hpp b/env_manager/include/component_manager/subscriber_component.hpp deleted file mode 100644 index 80fb873..0000000 --- a/env_manager/include/component_manager/subscriber_component.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_ -#define ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" - -namespace env_manager -{ -namespace component_manager -{ - -const std::string DEFAULT_SUB_NODE_NAME = "env_manager_sub_node"; -const std::string DEFAULT_SUB_TOPIC_NAME = "sub_topic"; - -template -class SubscriberComponent : public rclcpp::Node -{ -public: - explicit SubscriberComponent(const rclcpp::NodeOptions& options) - : Node(DEFAULT_SUB_NODE_NAME, options) - { - _sub = create_subscription( - DEFAULT_SUB_TOPIC_NAME, 10, this->callback); - auto ret = rcutils_logging_set_logger_level( - get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL); - if (ret != RCUTILS_RET_OK) - { - RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str); - rcutils_reset_error(); - } - } - - virtual void callback(const MessageT& msg) = 0; - -private: - typename rclcpp::Subscription::SharedPtr _sub; -}; - -} // namespace component_manager -} // namespace env_manager - -#endif // ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/visibility_control.h b/env_manager/include/component_manager/visibility_control.h deleted file mode 100644 index e26be49..0000000 --- a/env_manager/include/component_manager/visibility_control.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef COMPONENT_MANAGER__VISIBILITY_CONTROL_H_ -#define COMPONENT_MANAGER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define COMPONENT_MANAGER_EXPORT __attribute__ ((dllexport)) - #define COMPONENT_MANAGER_IMPORT __attribute__ ((dllimport)) - #else - #define COMPONENT_MANAGER_EXPORT __declspec(dllexport) - #define COMPONENT_MANAGER_IMPORT __declspec(dllimport) - #endif - #ifdef COMPONENT_MANAGER_BUILDING_LIBRARY - #define COMPONENT_MANAGER_PUBLIC COMPONENT_MANAGER_EXPORT - #else - #define COMPONENT_MANAGER_PUBLIC COMPONENT_MANAGER_IMPORT - #endif - #define COMPONENT_MANAGER_PUBLIC_TYPE COMPONENT_MANAGER_PUBLIC - #define COMPONENT_MANAGER_LOCAL -#else - #define COMPONENT_MANAGER_EXPORT __attribute__ ((visibility("default"))) - #define COMPONENT_MANAGER_IMPORT - #if __GNUC__ >= 4 - #define COMPONENT_MANAGER_PUBLIC __attribute__ ((visibility("default"))) - #define COMPONENT_MANAGER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define COMPONENT_MANAGER_PUBLIC - #define COMPONENT_MANAGER_LOCAL - #endif - #define COMPONENT_MANAGER_PUBLIC_TYPE -#endif - -#endif // COMPONENT_MANAGER__VISIBILITY_CONTROL_H_ diff --git a/env_manager/include/config/config.hpp.cmake b/env_manager/include/config/config.hpp.cmake deleted file mode 100644 index 708944f..0000000 --- a/env_manager/include/config/config.hpp.cmake +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef ENV_MANAGER_CONFIG_CONFIG_H_ -#define ENV_MANAGER_CONFIG_CONFIG_H_ - -namespace env_manager -{ -namespace config -{ - -constexpr char kConfigurationFilesDirectory[] = "@ENV_MANAGER_CONFIGURATION_FILES_DIRECTORY@"; -constexpr char kSourceDirectory[] = "@PROJECT_SOURCE_DIR@"; - -} // namespace config -} // namespace env_manager - -#endif // ENV_MANAGER_CONFIG_CONFIG_H_ diff --git a/env_manager/include/config/config_file_resolver.hpp b/env_manager/include/config/config_file_resolver.hpp deleted file mode 100644 index 28848d0..0000000 --- a/env_manager/include/config/config_file_resolver.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_ -#define ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_ - -#include "lua_file_resolver.hpp" - -namespace env_manager -{ -namespace config -{ - -class ConfigurationFileResolver : public FileResolver -{ -public: - explicit ConfigurationFileResolver( - const std::vector& cfg_files_dirs); - - std::string GetPath(const std::string& basename) const; - std::string GetContent(const std::string& basename) const; - -private: - std::vector _config_files_dirs; - -}; - -} // namespace config -} // namespace env_manager - -#endif // ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_ diff --git a/env_manager/include/config/lua_file_resolver.hpp b/env_manager/include/config/lua_file_resolver.hpp deleted file mode 100644 index 2c9d900..0000000 --- a/env_manager/include/config/lua_file_resolver.hpp +++ /dev/null @@ -1,129 +0,0 @@ -#ifndef ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_ -#define ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_ - -#include "lua.hpp" -#include -#include -#include -#include - -namespace env_manager -{ -namespace config -{ - -class FileResolver -{ -public: - virtual ~FileResolver() {} - virtual std::string GetPath(const std::string& basename) const = 0; - virtual std::string GetContent(const std::string& basename) const = 0; -}; - -class LuaParameterDictionary { - public: - // Constructs the dictionary from a Lua Table specification. - LuaParameterDictionary(const std::string& code, - std::unique_ptr file_resolver); - - LuaParameterDictionary(const LuaParameterDictionary&) = delete; - LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete; - - // Constructs a LuaParameterDictionary without reference counting. - static std::unique_ptr NonReferenceCounted( - const std::string& code, std::unique_ptr file_resolver); - - ~LuaParameterDictionary(); - - // Returns all available keys. - std::vector GetKeys() const; - - // Returns true if the key is in this dictionary. - bool HasKey(const std::string& key) const; - - // These methods CHECK() that the 'key' exists. - std::string GetString(const std::string& key); - double GetDouble(const std::string& key); - int GetInt(const std::string& key); - bool GetBool(const std::string& key); - std::unique_ptr GetDictionary(const std::string& key); - - // Gets an int from the dictionary and CHECK()s that it is non-negative. - int GetNonNegativeInt(const std::string& key); - - // Returns a string representation for this LuaParameterDictionary. - std::string ToString() const; - - // Returns the values of the keys '1', '2', '3' as the given types. - std::vector GetArrayValuesAsDoubles(); - std::vector GetArrayValuesAsStrings(); - std::vector> - GetArrayValuesAsDictionaries(); - - private: - enum class ReferenceCount { YES, NO }; - LuaParameterDictionary(const std::string& code, - ReferenceCount reference_count, - std::unique_ptr file_resolver); - - // For GetDictionary(). - LuaParameterDictionary(lua_State* L, ReferenceCount reference_count, - std::shared_ptr file_resolver); - - // Function that recurses to keep track of indent for ToString(). - std::string DoToString(const std::string& indent) const; - - // Pop the top of the stack and CHECKs that the type is correct. - double PopDouble() const; - int PopInt() const; - bool PopBool() const; - - // Pop the top of the stack and CHECKs that it is a string. The returned value - // is either quoted to be suitable to be read back by a Lua interpretor or - // not. - enum class Quoted { YES, NO }; - std::string PopString(Quoted quoted) const; - - // Creates a LuaParameterDictionary from the Lua table at the top of the - // stack, either with or without reference counting. - std::unique_ptr PopDictionary( - ReferenceCount reference_count) const; - - // CHECK() that 'key' is in the dictionary. - void CheckHasKey(const std::string& key) const; - - // CHECK() that 'key' is in this dictionary and reference it as being used. - void CheckHasKeyAndReference(const std::string& key); - - // If desired, this can be called in the destructor of a derived class. It - // will CHECK() that all keys defined in the configuration have been used - // exactly once and resets the reference counter. - void CheckAllKeysWereUsedExactlyOnceAndReset(); - - // Reads a file into a Lua string. - static int LuaRead(lua_State* L); - - // Handles inclusion of other Lua files and prevents double inclusion. - static int LuaInclude(lua_State* L); - - lua_State* L_; // The name is by convention in the Lua World. - int index_into_reference_table_; - - // This is shared with all the sub dictionaries. - const std::shared_ptr file_resolver_; - - // If true will check that all keys were used on destruction. - const ReferenceCount reference_count_; - - // This is modified with every call to Get* in order to verify that all - // parameters are read exactly once. - std::map reference_counts_; - - // List of all included files in order of inclusion. Used to prevent double - // inclusion. - std::vector included_files_; -}; -} // namespce config -} // namespace env_manager - -#endif // ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_ diff --git a/env_manager/include/config/options.hpp b/env_manager/include/config/options.hpp deleted file mode 100644 index 190c23f..0000000 --- a/env_manager/include/config/options.hpp +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef ENV_MANAGER_CONFIG_OPTIONS_HPP_ -#define ENV_MANAGER_CONFIG_OPTIONS_HPP_ - -#include "config/lua_file_resolver.hpp" - -#include - -namespace env_manager -{ -namespace config -{ - -struct ComponentOption -{ - static ComponentOption create_option( - LuaParameterDictionary* lua_parameter_dictionary); - std::string library; - std::string class_name; -}; - -struct EnvironmentOption -{ - static EnvironmentOption create_option( - LuaParameterDictionary* lua_parameter_dictionary); - std::string ns; - std::map components; -}; - -struct NodeOption -{ - enum NodeType - { - Publisher, - Subscriber, - Service, - Client, - Node - }; - - static NodeOption create_option( - LuaParameterDictionary* lua_parameter_dictionary); - std::string name; - std::string msg_type; - NodeType type; - static const std::unordered_map node_type_remap; -}; - -struct EnvManagerOption -{ - static EnvManagerOption create_option( - LuaParameterDictionary* lua_parameter_dictionary); - std::map environments; - std::map nodes; -}; - -} -} -#endif // ENV_MANAGER_CONFIG_OPTIONS_HPP_ diff --git a/env_manager/include/env_manager/environment_base.hpp b/env_manager/include/env_manager/environment_base.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/env_manager/include/env_manager/environment_manager.hpp b/env_manager/include/env_manager/environment_manager.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/env_manager/include/env_manager/environment_node_manager.hpp b/env_manager/include/env_manager/environment_node_manager.hpp deleted file mode 100644 index cb5bca6..0000000 --- a/env_manager/include/env_manager/environment_node_manager.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_ -#define ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_ - -#include "config/options.hpp" - -#include - -#include -#include - -extern "C" -{ -#include -#include -} - -namespace env_manager -{ - -class EnvironmentNodeManager -{ -public: - EnvironmentNodeManager(); - - ~EnvironmentNodeManager(); -}; - -} // namespace env_manager - -#endif // ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_ diff --git a/env_manager/planning_scene_manager/CMakeLists.txt b/env_manager/planning_scene_manager/CMakeLists.txt new file mode 100644 index 0000000..4a46988 --- /dev/null +++ b/env_manager/planning_scene_manager/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(planning_scene_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/env_manager/planning_scene_manager/LICENSE b/env_manager/planning_scene_manager/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/env_manager/planning_scene_manager/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp b/env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp new file mode 100644 index 0000000..a3023e8 --- /dev/null +++ b/env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp @@ -0,0 +1,29 @@ +#ifndef __PLANNING_SCENE_MANAGER_H__ +#define __PLANNING_SCENE_MANAGER_H__ + +#include "rclcpp/rclcpp.hpp" +#include "moveit/planning_scene_monitor/planning_scene_monitor.h" + +namespace planning_scene_manager +{ +class PlanningSceneManager : public planning_scene_monitor::PlanningSceneMonitor +{ +public: + PlanningSceneManager(); + + bool GetPlanningObject(); + bool SetPlanningObject(); + + bool SetPlanningScene(); + bool GetPlanningScene(); + + bool UpdatePlanningScene(); + +private: + +}; + + +} // namespace planning_scene_manager + +#endif // __PLANNING_SCENE_MANAGER_H__ \ No newline at end of file diff --git a/env_manager/package.xml b/env_manager/planning_scene_manager/package.xml similarity index 56% rename from env_manager/package.xml rename to env_manager/planning_scene_manager/package.xml index 4a1128c..ea47535 100644 --- a/env_manager/package.xml +++ b/env_manager/planning_scene_manager/package.xml @@ -1,16 +1,13 @@ - env_manager + planning_scene_manager 0.0.0 - env_manager - splinter1984 - Apache-2.0 + TODO: Package description + Bill Finger + Apache-2.0 - ament_cmake_ros - - boost - rcl + ament_cmake ament_lint_auto ament_lint_common diff --git a/env_manager/planning_scene_manager/src/planning_scene_manager.cpp b/env_manager/planning_scene_manager/src/planning_scene_manager.cpp new file mode 100644 index 0000000..8c9ab4a --- /dev/null +++ b/env_manager/planning_scene_manager/src/planning_scene_manager.cpp @@ -0,0 +1,31 @@ +#include "planning_scene_manager/planning_scene_manager.hpp" +#include "moveit_msgs/msg/collision_object.hpp" +#include "moveit/planning_scene_interface/planning_scene_interface.h" + +namespace planning_scene_manager +{ + +bool PlanningSceneManager::SetPlanningObject() +{ + moveit_msgs::msg::CollisionObject object; + std::shared_ptr pcm_; + planning_scene_monitor::LockedPlanningSceneRW scene(pcm_); + scene->removeAllCollisionObjects(); + RCLCPP_INFO(pnode_->get_logger(), "All object has been delete"); + return true; +} + +} // namespace planning_scene_manager + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + // Добавляем объект в сцену коллизии + node->SetPlanningObject(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/env_manager/src/component_manager/component_manager.cpp b/env_manager/src/component_manager/component_manager.cpp deleted file mode 100644 index 96bf858..0000000 --- a/env_manager/src/component_manager/component_manager.cpp +++ /dev/null @@ -1,159 +0,0 @@ -#include "component_manager/component_manager.hpp" -#include "component_manager/node_component.hpp" -#include "component_manager/publisher_component.hpp" -#include "component_manager/subscriber_component.hpp" -#include "component_manager/service_component.hpp" -#include "component_manager/client_component.hpp" -#include "glog/logging.h" - -#include "rcl/remap.h" -#include - -namespace env_manager -{ -namespace component_manager -{ - -ComponentManager::ComponentManager(std::weak_ptr executor) - : _executor(executor) -{ -} - -static rclcpp::NodeOptions create_options( - const config::NodeOption &node_opts, - const std::string &node_name, - const std::string &ns) -{ - rclcpp::NodeOptions opts; - std::vector args = {"--ros-args", - "--disable-rosout-logs", - "--disable-stdout-logs", - "--enable-external-lib-logs", - "--log-level", "WARN", - }; - args.push_back("-r"); - args.push_back("__ns:=/" + ns); - args.push_back("-r"); - args.push_back("__node:=" + node_name); - args.push_back("-r"); - - switch (node_opts.type) - { - case config::NodeOption::NodeType::Publisher: - args.push_back(DEFAULT_PUB_TOPIC_NAME + ":=" + node_opts.name); - break; - case config::NodeOption::NodeType::Subscriber: - args.push_back(DEFAULT_SUB_TOPIC_NAME + ":=" + node_opts.name); - break; - case config::NodeOption::NodeType::Service: - args.push_back(DEFAULT_SERVICE_NAME + ":=" + node_opts.name); - break; - case config::NodeOption::NodeType::Client: - args.push_back(DEFAULT_CLIENT_NAME + ":=" + node_opts.name); - break; - case config::NodeOption::NodeType::Node: - args.erase(args.end()); - break; - } - - opts.arguments(args); - - return opts; -} - -static rclcpp::NodeOptions create_default_options(const std::string &ns) -{ - rclcpp::NodeOptions opts; - opts.arguments({ - "--ros-args", "-r", std::string("__ns:=/" + ns), - "--disable-rosout-logs", - "--disable-stdout-logs", - "--enable-external-lib-logs", - "--log-level", "FATAL", - }); - - return opts; -} - -void ComponentManager::register_components( - const std::map &comps, - const std::map &nodes, - const std::string& ns) -{ - if (comps.empty()) - return; - - for (const auto &[name, comp]: comps) - { - auto class_name = "rclcpp_components::NodeFactoryTemplate<" + comp.class_name + ">"; - - LOG(INFO) << "Provide lib: " << comp.library << " namespace: " + ns; - auto loader = new class_loader::ClassLoader(comp.library); - auto classes = - loader->getAvailableClasses(); - - for (auto clazz: classes) - { - if (clazz != class_name) - continue; - rclcpp::NodeOptions opts = create_default_options(ns); - if (clazz == class_name) - { - auto node_opts = nodes.at(name); - opts = create_options(node_opts, name, ns); - } - LOG(INFO) << "Create instance of class: " << clazz; - auto node_factory = - loader->createInstance(clazz); - auto wrapper = node_factory->create_node_instance(opts); - auto node = wrapper.get_node_base_interface(); - - _node_wrappers.insert({name, wrapper}); - _nodes.insert({name, node}); - if (auto exec = _executor.lock()) - exec->add_node(node); - } - - _loaders.push_back(loader); - } -} - -void ComponentManager::remap_components_namespace(const std::string &ns) -{ - char* nms = const_cast(ns.c_str()); - for (auto& [name, wrapper]: _node_wrappers) - { - auto node = (rclcpp::Node*)wrapper.get_node_instance().get(); - auto opts = node->get_node_options(); - auto ret = rcl_remap_node_namespace( - &opts.get_rcl_node_options()->arguments, - NULL, node->get_name(), rcl_get_default_allocator(), - &nms); - - if (ret == RCL_RET_OK) - LOG(INFO) << "Succesfully remap node with ns: " + ns; - } - - //std::logic_error("Not implemented." + ns); -} - -void ComponentManager::remove_components_from_executor() -{ - if (_nodes.empty()) - { - LOG(WARNING) << "Unable to remove nodes from executor."; - return; - } - - if (auto exec = _executor.lock()) - { - for (const auto &[node_name, node]: _nodes) - { - LOG(INFO) << "Remove node '" << node_name << "' from executor."; - exec->remove_node(node); - } - } -} - -} // namespace env_manager -} // namespace component_manager diff --git a/env_manager/src/config/config_file_resolver.cpp b/env_manager/src/config/config_file_resolver.cpp deleted file mode 100644 index f945deb..0000000 --- a/env_manager/src/config/config_file_resolver.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include "config/config_file_resolver.hpp" - -#include -#include -#include - -#include "config/config.hpp" - -#include - -namespace env_manager -{ -namespace config -{ - -ConfigurationFileResolver::ConfigurationFileResolver( - const std::vector& cfg_files_dirs) - : _config_files_dirs(cfg_files_dirs) -{ - _config_files_dirs.push_back(kConfigurationFilesDirectory); -} - -std::string ConfigurationFileResolver::GetPath( - const std::string &basename) const -{ - for (const auto& path: _config_files_dirs) - { - const std::string filename = path + "/" + basename; - std::ifstream stream(filename.c_str()); - if (stream.good()) - { - LOG(INFO) << "found filename: " << filename; - return filename; - } - } - - LOG(FATAL) << "File '" << basename << "' was not found."; -} - -std::string ConfigurationFileResolver::GetContent( - const std::string &basename) const -{ - CHECK(!basename.empty()) << "File basename cannot be empty." << basename; - const std::string filename = GetPath(basename); - std::ifstream stream(filename.c_str()); - return std::string(std::istreambuf_iterator(stream), - std::istreambuf_iterator()); -} - -} // namespace config -} // namespace env_manager diff --git a/env_manager/src/config/lua_file_resolver.cpp b/env_manager/src/config/lua_file_resolver.cpp deleted file mode 100644 index 71ec231..0000000 --- a/env_manager/src/config/lua_file_resolver.cpp +++ /dev/null @@ -1,446 +0,0 @@ -#include "config/lua_file_resolver.hpp" -#include "glog/logging.h" - -#include -#include -#include -#include - -namespace env_manager -{ -namespace config -{ - -namespace { -// Replace the string at the top of the stack through a quoted version that Lua -// can read back. -void QuoteStringOnStack(lua_State* L) { - CHECK(lua_isstring(L, -1)) << "Top of stack is not a string value."; - int current_index = lua_gettop(L); - - // S: ... string - lua_pushglobaltable(L); // S: ... string globals - lua_getfield(L, -1, "string"); // S: ... string globals - lua_getfield(L, -1, - "format"); // S: ... string globals format - lua_pushstring(L, "%q"); // S: ... string globals format "%q" - lua_pushvalue(L, current_index); // S: ... string globals - // format "%q" string - - lua_call(L, 2, 1); // S: ... string globals quoted - lua_replace(L, current_index); // S: ... quoted globals - - lua_pop(L, 2); // S: ... quoted -} - -// Sets the given 'dictionary' as the value of the "this" key in Lua's registry -// table. -void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* dictionary) { - lua_pushstring(L, "this"); - lua_pushlightuserdata(L, dictionary); - lua_settable(L, LUA_REGISTRYINDEX); -} - -// Gets the 'dictionary' from the "this" key in Lua's registry table. -LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) { - lua_pushstring(L, "this"); - lua_gettable(L, LUA_REGISTRYINDEX); - void* return_value = lua_isnil(L, -1) ? nullptr : lua_touserdata(L, -1); - lua_pop(L, 1); - CHECK(return_value != nullptr); - return reinterpret_cast(return_value); -} - -// CHECK()s if a Lua method returned an error. -void CheckForLuaErrors(lua_State* L, int status) { - CHECK_EQ(status, 0) << lua_tostring(L, -1); -} - -// Returns 'a' if 'condition' is true, else 'b'. -int LuaChoose(lua_State* L) { - CHECK_EQ(lua_gettop(L), 3) << "choose() takes (condition, a, b)."; - CHECK(lua_isboolean(L, 1)) << "condition is not a boolean value."; - - const bool condition = lua_toboolean(L, 1); - if (condition) { - lua_pushvalue(L, 2); - } else { - lua_pushvalue(L, 3); - } - return 1; -} - -// Pushes a value to the Lua stack. -void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); } -void PushValue(lua_State* L, const std::string& key) { - lua_pushstring(L, key.c_str()); -} - -// Reads the value with the given 'key' from the Lua dictionary and pushes it to -// the top of the stack. -template -void GetValueFromLuaTable(lua_State* L, const T& key) { - PushValue(L, key); - lua_rawget(L, -2); -} - -// CHECK() that the topmost parameter on the Lua stack is a table. -void CheckTableIsAtTopOfStack(lua_State* L) { - CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!"; -} - -// Returns true if 'key' is in the table at the top of the Lua stack. -template -bool HasKeyOfType(lua_State* L, const T& key) { - CheckTableIsAtTopOfStack(L); - PushValue(L, key); - lua_rawget(L, -2); - const bool key_not_found = lua_isnil(L, -1); - lua_pop(L, 1); // Pop the item again. - return !key_not_found; -} - -// Iterates over the integer keys of the table at the top of the stack of 'L• -// and pushes the values one by one. 'pop_value' is expected to pop a value and -// put them into a C++ container. -void GetArrayValues(lua_State* L, const std::function& pop_value) { - int idx = 1; - while (true) { - GetValueFromLuaTable(L, idx); - if (lua_isnil(L, -1)) { - lua_pop(L, 1); - break; - } - pop_value(); - ++idx; - } -} - -} // namespace - -std::unique_ptr -LuaParameterDictionary::NonReferenceCounted( - const std::string& code, std::unique_ptr file_resolver) { - return std::unique_ptr(new LuaParameterDictionary( - code, ReferenceCount::NO, std::move(file_resolver))); -} - -LuaParameterDictionary::LuaParameterDictionary( - const std::string& code, std::unique_ptr file_resolver) - : LuaParameterDictionary(code, ReferenceCount::YES, - std::move(file_resolver)) {} - -LuaParameterDictionary::LuaParameterDictionary( - const std::string& code, ReferenceCount reference_count, - std::unique_ptr file_resolver) - : L_(luaL_newstate()), - index_into_reference_table_(-1), - file_resolver_(std::move(file_resolver)), - reference_count_(reference_count) { - CHECK_NOTNULL(L_); - SetDictionaryInRegistry(L_, this); - - luaL_openlibs(L_); - - lua_register(L_, "choose", LuaChoose); - lua_register(L_, "include", LuaInclude); - lua_register(L_, "read", LuaRead); - - CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str())); - CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0)); - CheckTableIsAtTopOfStack(L_); -} - -LuaParameterDictionary::LuaParameterDictionary( - lua_State* const L, ReferenceCount reference_count, - std::shared_ptr file_resolver) - : L_(lua_newthread(L)), - file_resolver_(std::move(file_resolver)), - reference_count_(reference_count) { - CHECK_NOTNULL(L_); - - // Make sure this is never garbage collected. - CHECK(lua_isthread(L, -1)); - index_into_reference_table_ = luaL_ref(L, LUA_REGISTRYINDEX); - - CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!"; - lua_xmove(L, L_, 1); // Moves the table and the coroutine over. - CheckTableIsAtTopOfStack(L_); -} - -LuaParameterDictionary::~LuaParameterDictionary() { - if (reference_count_ == ReferenceCount::YES) { - //CheckAllKeysWereUsedExactlyOnceAndReset(); - } - if (index_into_reference_table_ > 0) { - luaL_unref(L_, LUA_REGISTRYINDEX, index_into_reference_table_); - } else { - lua_close(L_); - } -} - -std::vector LuaParameterDictionary::GetKeys() const { - CheckTableIsAtTopOfStack(L_); - std::vector keys; - - lua_pushnil(L_); // Push the first key - while (lua_next(L_, -2) != 0) { - lua_pop(L_, 1); // Pop value, keep key. - if (!lua_isnumber(L_, -1)) { - keys.emplace_back(lua_tostring(L_, -1)); - } - } - return keys; -} - -bool LuaParameterDictionary::HasKey(const std::string& key) const { - return HasKeyOfType(L_, key); -} - -std::string LuaParameterDictionary::GetString(const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopString(Quoted::NO); -} - -std::string LuaParameterDictionary::PopString(Quoted quoted) const { - CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value."; - if (quoted == Quoted::YES) { - QuoteStringOnStack(L_); - } - - const std::string value = lua_tostring(L_, -1); - lua_pop(L_, 1); - return value; -} - -double LuaParameterDictionary::GetDouble(const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopDouble(); -} - -double LuaParameterDictionary::PopDouble() const { - CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value."; - const double value = lua_tonumber(L_, -1); - lua_pop(L_, 1); - return value; -} - -int LuaParameterDictionary::GetInt(const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopInt(); -} - -int LuaParameterDictionary::PopInt() const { - CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value."; - const int value = lua_tointeger(L_, -1); - lua_pop(L_, 1); - return value; -} - -bool LuaParameterDictionary::GetBool(const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopBool(); -} - -bool LuaParameterDictionary::PopBool() const { - CHECK(lua_isboolean(L_, -1)) << "Top of stack is not a boolean value."; - const bool value = lua_toboolean(L_, -1); - lua_pop(L_, 1); - return value; -} - -std::unique_ptr LuaParameterDictionary::GetDictionary( - const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopDictionary(reference_count_); -} - -std::unique_ptr LuaParameterDictionary::PopDictionary( - ReferenceCount reference_count) const { - CheckTableIsAtTopOfStack(L_); - std::unique_ptr value( - new LuaParameterDictionary(L_, reference_count, file_resolver_)); - // The constructor lua_xmove()s the value, no need to pop it. - CheckTableIsAtTopOfStack(L_); - return value; -} - -std::string LuaParameterDictionary::DoToString( - const std::string& indent) const { - std::string result = "{"; - bool dictionary_is_empty = true; - - const auto top_of_stack_to_string = [this, indent, - &dictionary_is_empty]() -> std::string { - dictionary_is_empty = false; - - const int value_type = lua_type(L_, -1); - switch (value_type) { - case LUA_TBOOLEAN: - return PopBool() ? "true" : "false"; - break; - case LUA_TSTRING: - return PopString(Quoted::YES); - break; - case LUA_TNUMBER: { - const double value = PopDouble(); - if (std::isinf(value)) { - return value < 0 ? "-math.huge" : "math.huge"; - } else { - return std::to_string(value); - } - } break; - case LUA_TTABLE: { - std::unique_ptr subdict( - PopDictionary(ReferenceCount::NO)); - return subdict->DoToString(indent + " "); - } break; - default: - LOG(FATAL) << "Unhandled type " << lua_typename(L_, value_type); - } - }; - - // Integer (array) keys. - for (int i = 1; i; ++i) { - GetValueFromLuaTable(L_, i); - if (lua_isnil(L_, -1)) { - lua_pop(L_, 1); - break; - } - result.append("\n"); - result.append(indent); - result.append(" "); - result.append(top_of_stack_to_string()); - result.append(","); - } - - // String keys. - std::vector keys = GetKeys(); - if (!keys.empty()) { - std::sort(keys.begin(), keys.end()); - for (const std::string& key : keys) { - GetValueFromLuaTable(L_, key); - result.append("\n"); - result.append(indent); - result.append(" "); - result.append(key); - result.append(" = "); - result.append(top_of_stack_to_string()); - result.append(","); - } - } - result.append("\n"); - result.append(indent); - result.append("}"); - - if (dictionary_is_empty) { - return "{}"; - } - return result; -} - -std::string LuaParameterDictionary::ToString() const { return DoToString(""); } - -std::vector LuaParameterDictionary::GetArrayValuesAsDoubles() { - std::vector values; - GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); }); - return values; -} - -std::vector> -LuaParameterDictionary::GetArrayValuesAsDictionaries() { - std::vector> values; - GetArrayValues(L_, [&values, this] { - values.push_back(PopDictionary(reference_count_)); - }); - return values; -} - -std::vector LuaParameterDictionary::GetArrayValuesAsStrings() { - std::vector values; - GetArrayValues(L_, - [&values, this] { values.push_back(PopString(Quoted::NO)); }); - return values; -} - -void LuaParameterDictionary::CheckHasKey(const std::string& key) const { - CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n" - << ToString(); -} - -void LuaParameterDictionary::CheckHasKeyAndReference(const std::string& key) { - CheckHasKey(key); - reference_counts_[key]++; -} - -void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() { - for (const auto& key : GetKeys()) { - CHECK_EQ(1, reference_counts_.count(key)) - << "Key '" << key << "' was used the wrong number of times."; - CHECK_EQ(1, reference_counts_.at(key)) - << "Key '" << key << "' was used the wrong number of times."; - } - reference_counts_.clear(); -} - -int LuaParameterDictionary::GetNonNegativeInt(const std::string& key) { - const int temp = GetInt(key); // Will increase reference count. - CHECK_GE(temp, 0) << temp << " is negative."; - return temp; -} - -// Lua function to run a script in the current Lua context. Takes the filename -// as its only argument. -int LuaParameterDictionary::LuaInclude(lua_State* L) { - CHECK_EQ(lua_gettop(L), 1); - CHECK(lua_isstring(L, -1)) << "include takes a filename."; - - LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L); - const std::string basename = lua_tostring(L, -1); - const std::string filename = - parameter_dictionary->file_resolver_->GetPath(basename); - if (std::find(parameter_dictionary->included_files_.begin(), - parameter_dictionary->included_files_.end(), - filename) != parameter_dictionary->included_files_.end()) { - std::string error_msg = - "Tried to include " + filename + - " twice. Already included files in order of inclusion: "; - for (const std::string& filename : parameter_dictionary->included_files_) { - error_msg.append(filename); - error_msg.append("\n"); - } - LOG(FATAL) << error_msg; - } - parameter_dictionary->included_files_.push_back(filename); - lua_pop(L, 1); - CHECK_EQ(lua_gettop(L), 0); - - const std::string content = - parameter_dictionary->file_resolver_->GetContent(basename); - CheckForLuaErrors( - L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str())); - CheckForLuaErrors(L, lua_pcall(L, 0, LUA_MULTRET, 0)); - - return lua_gettop(L); -} - -// Lua function to read a file into a string. -int LuaParameterDictionary::LuaRead(lua_State* L) { - CHECK_EQ(lua_gettop(L), 1); - CHECK(lua_isstring(L, -1)) << "read takes a filename."; - - LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L); - const std::string file_content = - parameter_dictionary->file_resolver_->GetContent( - lua_tostring(L, -1)); - lua_pushstring(L, file_content.c_str()); - return 1; -} - -} // namespace config -} // namespace env_manager diff --git a/env_manager/src/config/options.cpp b/env_manager/src/config/options.cpp deleted file mode 100644 index 2fbe53d..0000000 --- a/env_manager/src/config/options.cpp +++ /dev/null @@ -1,83 +0,0 @@ -#include "config/options.hpp" - -#include "glog/logging.h" - -namespace env_manager -{ -namespace config -{ - -ComponentOption ComponentOption::create_option( - LuaParameterDictionary *lua_parameter_dictionary) -{ - return ComponentOption{ - lua_parameter_dictionary->GetString("lib"), - lua_parameter_dictionary->GetString("class"), - }; -} - -EnvironmentOption EnvironmentOption::create_option( - LuaParameterDictionary *lua_parameter_dictionary) -{ - auto comps = - lua_parameter_dictionary->GetDictionary("components"); - std::map components; - for (const auto &comp: comps->GetKeys()) - components.insert({ - comp, ComponentOption::create_option(comps->GetDictionary(comp).get()) - }); - - return EnvironmentOption{ - lua_parameter_dictionary->GetString("namespace"), - components, - }; -} - -NodeOption NodeOption::create_option( - LuaParameterDictionary *lua_parameter_dictionary) -{ - return NodeOption{ - lua_parameter_dictionary->GetString("name"), - lua_parameter_dictionary->GetString("msg_type"), - node_type_remap.at(lua_parameter_dictionary->GetString("type")), - }; -} - -const std::unordered_map -NodeOption::node_type_remap = - { - {"Publisher", NodeOption::NodeType::Publisher}, - {"Subscriber", NodeOption::NodeType::Subscriber}, - {"Service", NodeOption::NodeType::Service}, - {"Client", NodeOption::NodeType::Client}, - {"Node", NodeOption::NodeType::Node}, - }; - -EnvManagerOption EnvManagerOption::create_option( - LuaParameterDictionary *lua_parameter_dictionary) -{ - - auto lua_env_dictionary = lua_parameter_dictionary->GetDictionary("environments"); - auto envs_dict = lua_env_dictionary->GetArrayValuesAsDictionaries(); - std::map environments; - for (const auto& env: envs_dict) - environments.insert({ - env->GetString("namespace"), EnvironmentOption::create_option(env.get()) - }); - - auto lua_node_dictionary = lua_parameter_dictionary->GetDictionary("nodes"); - auto nodes_keys = lua_node_dictionary->GetKeys(); - std::map nodes; - for (const auto& node: nodes_keys) - nodes.insert({ - node, NodeOption::create_option(lua_node_dictionary->GetDictionary(node).get()) - }); - - return EnvManagerOption{ - environments, - nodes, - }; -} - -} // namespace config -} // namespace env_manager diff --git a/env_manager/src/env_manager/environment_node_manager.cpp b/env_manager/src/env_manager/environment_node_manager.cpp deleted file mode 100644 index 7b95a3e..0000000 --- a/env_manager/src/env_manager/environment_node_manager.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "env_manager/environment_node_manager.hpp" - -namespace env_manager -{ - -} // namespace env_manager diff --git a/env_manager/src/main.cpp b/env_manager/src/main.cpp deleted file mode 100644 index 88f2456..0000000 --- a/env_manager/src/main.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include "component_manager/component_manager.hpp" -#include "config/config_file_resolver.hpp" -#include "config/lua_file_resolver.hpp" -#include "config/options.hpp" - -#include "config/config.hpp" - -#include "glog/logging.h" - -int main(int argc, const char* argv[]) -{ - google::InitGoogleLogging(argv[0]); - FLAGS_logtostderr = 1; - - rclcpp::init(argc, argv); - auto exec = std::make_shared(); - - std::vector dirs = {}; - auto cfg_resolver = - ::std::make_unique<::env_manager::config::ConfigurationFileResolver>( - std::vector{ - std::string(env_manager::config::kSourceDirectory) + - "/config" - } - ); - - auto code = cfg_resolver->GetContent("config.lua"); - //LOG(INFO) << code; - ::env_manager::config::LuaParameterDictionary lua_dict( - code, std::move(cfg_resolver) - ); - - auto env_manager_opts = - ::env_manager::config::EnvManagerOption::create_option( - &lua_dict - ); - - std::vector comp_mngrs; - - for (const auto& [env, opts]: env_manager_opts.environments) - { - LOG(INFO) << "Start to initialize environment: " << env; - env_manager::component_manager::ComponentManager cmg(exec); - cmg.register_components(opts.components, env_manager_opts.nodes, opts.ns); - //cmg.upload_components_to_executor(&exec); - comp_mngrs.push_back(std::move(cmg)); - } - - //std::string ns = "/"; - //comp_mngrs.begin()->remap_components_namespace(ns); - - exec->spin(); - - for (auto &cmg: comp_mngrs) - cmg.remove_components_from_executor(); - - rclcpp::shutdown(); - - return 0; -} diff --git a/rbs_bringup/launch/bringup.launch.py b/rbs_bringup/launch/bringup.launch.py index 3b6ef07..5bb560e 100644 --- a/rbs_bringup/launch/bringup.launch.py +++ b/rbs_bringup/launch/bringup.launch.py @@ -142,7 +142,7 @@ def generate_launch_description(): DeclareLaunchArgument("sim_fake", default_value="false", description="Mock contollers") ) declared_arguments.append( - DeclareLaunchArgument("env_manager", default_value="true", description="Launch env_manager?") + DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?") ) declared_arguments.append( DeclareLaunchArgument("launch_sim", default_value="true", description="Launch simulator (Gazebo)? Most general arg") diff --git a/rbs_simulation/launch/get_urdf.launch.py b/rbs_simulation/launch/get_urdf.launch.py index 52f61d6..d034a8d 100644 --- a/rbs_simulation/launch/get_urdf.launch.py +++ b/rbs_simulation/launch/get_urdf.launch.py @@ -20,7 +20,7 @@ def generate_launch_description(): "prefix": "", "sim_mujoco": "true", "simulation_controllers": str(initial_joint_controllers_file_path), - "with_gripper": "false" + "with_gripper": "true" }) robot_desc = doc.toprettyxml(indent=' ') diff --git a/rbs_simulation/launch/simulation.launch.py b/rbs_simulation/launch/simulation.launch.py index 46b5665..25dc634 100644 --- a/rbs_simulation/launch/simulation.launch.py +++ b/rbs_simulation/launch/simulation.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?") ) declared_arguments.append( - DeclareLaunchArgument("gazebo_gui", default_value="false", description="Launch env_manager?") + DeclareLaunchArgument("gazebo_gui", default_value="true", description="Launch env_manager?") ) sim_gazebo = LaunchConfiguration("sim_gazebo") diff --git a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp b/rbs_skill_servers/src/add_planning_scene_objects_service.cpp index aaf0a88..008107d 100644 --- a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp +++ b/rbs_skill_servers/src/add_planning_scene_objects_service.cpp @@ -69,4 +69,4 @@ int main(int argc, char **argv) { rclcpp::shutdown(); g_node = nullptr; return 0; -} \ No newline at end of file +} diff --git a/rbs_skill_servers/src/gripper_control_action_server.cpp b/rbs_skill_servers/src/gripper_control_action_server.cpp index 9f68288..74addd0 100644 --- a/rbs_skill_servers/src/gripper_control_action_server.cpp +++ b/rbs_skill_servers/src/gripper_control_action_server.cpp @@ -116,4 +116,4 @@ RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer); // rbs_skill_actions::GripperControlActionServer server; // rclcpp::spin(server); // return 0; -// } \ No newline at end of file +// } diff --git a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp index 1709f0f..f223160 100644 --- a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp +++ b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp @@ -178,4 +178,4 @@ int main(int argc, char **argv) { run_server.join(); return 0; -} \ No newline at end of file +} diff --git a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp index 2f61ed9..b7adb84 100644 --- a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp +++ b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp @@ -160,4 +160,4 @@ int main(int argc, char **argv) { run_server.join(); return 0; -} \ No newline at end of file +} diff --git a/rbs_skill_servers/src/move_topose_action_server.cpp b/rbs_skill_servers/src/move_topose_action_server.cpp index 4ed59bc..aba0780 100644 --- a/rbs_skill_servers/src/move_topose_action_server.cpp +++ b/rbs_skill_servers/src/move_topose_action_server.cpp @@ -172,4 +172,4 @@ int main(int argc, char **argv) { run_server.join(); return 0; -} \ No newline at end of file +}