Motion planner (MoveIt) and Task planner (Plansys) integration

env_manager/config/environments/ground_true.lua

@@ -2,18 +2,13 @@
 GROUND_TRUE = {
     namespace = "ground_true",
     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"
         },
+        scene_component = {
+            lib = "libign_detect_object_component.so",
+            class = "scene_component::IgnDetectObjectService"
+        },
     }
 }

env_manager/config/nodes.lua

@@ -1,18 +1,12 @@
 NODES = {
-    --[[
-    talker_node = {
-        name = "talker",
-        type = "Publisher",
-        msg_type = "std_msgs/String",
-    },
-    service_node = {
-        name = "add_two_ints",
-        type = "Service",
-        msg_type = "example_interfaces/AddTwoInts"
-    },]]--
     camera_node = {
         name = "camera_node",
         type = "Publisher",
         msg_type = "sensor_msgs/Image"
     },
+    scene_component = {
+        name = "scene_component",
+        type = "Service",
+        msg_type = "component_interfaces/DetectObject"
+    },
 }

env_manager/include/component_manager/node_component.hpp

@@ -0,0 +1,36 @@
+#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__ 

env_manager/include/config/options.hpp

@@ -33,7 +33,8 @@ struct NodeOption
         Publisher,
         Subscriber,
         Service,
-        Client
+        Client,
+        Node
     };
 
     static NodeOption create_option(

env_manager/package.xml

@@ -3,9 +3,9 @@
 <package format="3">
   <name>env_manager</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>env_manager</description>
   <maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
-  <license>TODO: License declaration</license>
+ <license>Apache-2.0</license> 
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 

env_manager/src/component_manager/component_manager.cpp

@@ -1,4 +1,5 @@
 #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"
@@ -50,6 +51,9 @@ static rclcpp::NodeOptions create_options(
         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);
@@ -87,8 +91,11 @@ void ComponentManager::register_components(
         auto loader = new class_loader::ClassLoader(comp.library);
         auto classes = 
             loader->getAvailableClasses<rclcpp_components::NodeFactory>();
+
         for (auto clazz: classes)
         {
+            if (clazz != class_name)
+                continue;
             rclcpp::NodeOptions opts = create_default_options(ns);
             if (clazz == class_name)
             {

env_manager/src/config/options.cpp

@@ -50,6 +50,7 @@ NodeOption::node_type_remap =
         {"Subscriber", NodeOption::NodeType::Subscriber},
         {"Service", NodeOption::NodeType::Service},
         {"Client", NodeOption::NodeType::Client},
+        {"Node", NodeOption::NodeType::Node},
     };
 
 EnvManagerOption EnvManagerOption::create_option(