add file exist conditions

robossembler_scene_monitor/scene_monitor/CMakeLists.txt

@@ -20,44 +20,33 @@ find_package(nlohmann_json REQUIRED)
 
 set(dependencies
     rclcpp
-    # rclcpp_action
     geometry_msgs
     ament_index_cpp
-    # tf2_ros
-    # rclcpp_components
     nlohmann_json
     scene_monitor_interfaces
 )
 
 include_directories(include)
 
-add_library(component SHARED src/component.cpp)
+add_library(component_state_lib SHARED
-list(APPEND plugin_libs component)
+            src/component.cpp
-
+            src/component_state_monitor.cpp
-add_library(component_state_monitor SHARED src/component_state_monitor.cpp)
-list(APPEND plugin_libs component_state_monitor)
-
-target_link_libraries(
-    component_state_monitor
-    component
 )
-
+ament_target_dependencies(component_state_lib ${dependencies})
-foreach(plugin ${plugin_libs})
-    ament_target_dependencies(${plugin} ${dependencies})
-endforeach()
 
 add_executable(component_state_monitor_node src/component_state_monitor_node.cpp)
+target_link_libraries(component_state_monitor_node component_state_lib)
 ament_target_dependencies(component_state_monitor_node ${dependencies})
 
-target_link_libraries(component_state_monitor_node ${plugin_libs})
+install(TARGETS
-
+  component_state_lib
-# install(DIRECTORY launch
+  ARCHIVE DESTINATION lib
-#     DESTINATION share/${PROJECT_NAME}
+  LIBRARY DESTINATION lib
-# )
+  RUNTIME DESTINATION bin
+)
 
 install(TARGETS 
     component_state_monitor_node
-    ${plugin_libs}
     DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -66,6 +55,4 @@ if(BUILD_TESTING)
     ament_lint_auto_find_test_dependencies()
 endif()
 
-# ament_export_dependencies(${dependencies})
-
 ament_package()

robossembler_scene_monitor/scene_monitor/include/component.hpp

@@ -40,6 +40,8 @@ public:
     scene_monitor_interfaces::msg::GraspPose getGraspPose(const std::string &grasp_pose_name) const;
     std::vector<scene_monitor_interfaces::msg::GraspPose> getGraspPoses() const;
     geometry_msgs::msg::Pose getPlacementPose() const;
+
+    void setFrameName(const std::string &frame_name);
     
     void initializeFromJson(json &input);
     /*

robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp

@@ -6,6 +6,7 @@
 #include <rclcpp/timer.hpp>
 
 #include "scene_monitor_interfaces/srv/get_part.hpp"
+#include "scene_monitor_interfaces/srv/get_grasp_pose.hpp"
 #include "scene_monitor_interfaces/srv/get_grasp_poses.hpp"
 
 #include "component.hpp"
@@ -13,6 +14,7 @@
 namespace component_state_monitor
 {
 using GetPartSrv    = scene_monitor_interfaces::srv::GetPart;
+using GetGraspPoseSrv = scene_monitor_interfaces::srv::GetGraspPose;
 using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses;
 
 class ComponentStateMonitor: public rclcpp::Node
@@ -30,9 +32,12 @@ private:
         std::shared_ptr<GetPartSrv::Response> response);
     void getGraspPoses(const std::shared_ptr<GetGraspPosesSrv::Request> request,
         std::shared_ptr<GetGraspPosesSrv::Response> response);
+    void getGraspPose(const std::shared_ptr<GetGraspPoseSrv::Request> request,
+        std::shared_ptr<GetGraspPoseSrv::Response> response);
 
     std::map<std::string, Component> _loaded_components;
     rclcpp::Service<GetPartSrv>::SharedPtr _get_part_service;
+    rclcpp::Service<GetGraspPoseSrv>::SharedPtr _get_grasp_pose_service;
     rclcpp::Service<GetGraspPosesSrv>::SharedPtr _get_grasp_poses_service;
 
 };

robossembler_scene_monitor/scene_monitor/src/component.cpp

@@ -44,6 +44,11 @@ namespace component_state_monitor
         return grasp_poses;
     }
 
+    void Component::setFrameName(const std::string &frame_name)
+    {
+        _frame_name = frame_name;
+    }
+
     geometry_msgs::msg::Pose Component::getPlacementPose() const
     {
         return _placement;
@@ -103,6 +108,8 @@ namespace component_state_monitor
 
             _grasp_poses.insert({grasp_pose.label, grasp_pose});
         }
+
+        _current_state = ComponentState::Initialized;
     }
 
 }

robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp

@@ -5,7 +5,6 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
 #include "component_state_monitor.hpp"
-#include "nlohmann_json/json.hpp"
 
 namespace component_state_monitor
 {
@@ -21,6 +20,11 @@ namespace component_state_monitor
                                         std::bind(&ComponentStateMonitor::getGraspPoses, this, 
                                         std::placeholders::_1, std::placeholders::_2));
 
+        RCLCPP_INFO(this->get_logger(), "Initialize [get_grasp_pose] service");
+        _get_grasp_pose_service = this->create_service<GetGraspPoseSrv>("scene_monitor/get_grasp_pose", 
+                                        std::bind(&ComponentStateMonitor::getGraspPose, this, 
+                                        std::placeholders::_1, std::placeholders::_2));
+
         std::string package_name = "sdf_models";
         initializeComponents(package_name);
     }
@@ -71,6 +75,26 @@ namespace component_state_monitor
         RCLCPP_INFO(this->get_logger(), "[get_grasp_poses] Sending back response");
     }
 
+    void ComponentStateMonitor::getGraspPose(const std::shared_ptr<GetGraspPoseSrv::Request> request,
+        std::shared_ptr<GetGraspPoseSrv::Response> response)
+    {
+        RCLCPP_INFO(this->get_logger(), "[get_grasp_pose] Incoming request");
+        if (_loaded_components.find(request->frame_id) != _loaded_components.end())
+        {
+            auto component = _loaded_components.at(request->frame_id);
+            response->grasp_pose = component.getGraspPose(request->grasp_pose_name);
+            response->success = true;
+            response->message = "succesfully received a response from service [get_grasp_pose]";
+        } else {
+
+            RCLCPP_WARN(this->get_logger(), "[get_grasp_pose] Frame with name (%s) not loaded in component_state_monitor", request->frame_id.c_str());
+            response->success = false;
+            response->message = "could not found frame (%s) in component_state_monitor";
+        }
+
+        RCLCPP_INFO(this->get_logger(), "[get_grasp_pose] Sending back response");
+    }
+
     void ComponentStateMonitor::initializeComponents(const std::string& package_name)
     {
         RCLCPP_INFO(this->get_logger(), "Start read package (%s)", package_name.c_str());
@@ -82,21 +106,42 @@ namespace component_state_monitor
 
         std::string package_share_directory = ament_index_cpp::get_package_share_directory(package_name);
         std::filesystem::path models_path = package_share_directory + "/models/";
+        
+        if (!std::filesystem::exists(models_path))
+        {
+            RCLCPP_WARN(this->get_logger(), "Package (%s) does not contain '/models/' directory", package_name.c_str());
+            return;
+        }
+
         for (const auto &entry: std::filesystem::directory_iterator(models_path))
         {
-            RCLCPP_INFO(this->get_logger(), "Start read frame (%s)", entry.path().c_str());
+            std::string frame_directory_name = entry.path().string();
-            std::filesystem::path json_path = entry.path().string() + "/frames.json";
+            std::string frame_name = frame_directory_name.substr(frame_directory_name.find_last_of("/")+1, frame_directory_name.size()-1);
-            RCLCPP_INFO(this->get_logger(), "Start read frame metadata (%s)", json_path.c_str());
+
+            RCLCPP_INFO(this->get_logger(), "Start read frame (%s)", frame_name.c_str());
+            std::filesystem::path json_path = entry.path().string() + "/frames.json";
 
-            std::ifstream json_file(json_path);
-            auto input = json::parse(json_file);
             Component component;
 
-            component.initializeFromJson(input);
+            if (!std::filesystem::exists(json_path))
+            {
+                RCLCPP_WARN(this->get_logger(), "Package (%s) with frame (%s) does not contain 'frames.json' file", package_name.c_str(), frame_name.c_str());
+                RCLCPP_INFO(this->get_logger(), "Initialize empty component with frame name (%s)", frame_name.c_str());
+
+                component.setFrameName(frame_name);
+            } else {
+
+                RCLCPP_INFO(this->get_logger(), "Start read frame metadata (%s)", json_path.c_str());
+
+                std::ifstream json_file(json_path);
+                auto input = json::parse(json_file);
+
+                component.initializeFromJson(input);
+            }
 
             _loaded_components.insert({component.getFrameName(), component});
 
-            RCLCPP_INFO(this->get_logger(), "Successfully initialize component");
+            RCLCPP_INFO(this->get_logger(), "Successfully initialize component (%s)", component.getFrameName().c_str());
         }
     }
 }

robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt

@@ -25,6 +25,7 @@ set(msg_files
 )
 
 set(srv_files
+    "srv/GetGraspPose.srv"
     "srv/GetGraspPoses.srv"
     "srv/GetPart.srv"
 )

robossembler_scene_monitor/scene_monitor_interfaces/srv/GetGraspPose.srv

@@ -0,0 +1,8 @@
+string frame_id
+string grasp_pose_name
+---
+#result definition
+GraspPose grasp_pose
+
+bool success   # indicate successful run of triggered service
+string message # informational, e.g. for error messages