From 97bbf46c6d1d143c539c001e175302741d820ce4 Mon Sep 17 00:00:00 2001 From: Splinter1984 Date: Thu, 24 Feb 2022 02:18:59 +0800 Subject: [PATCH] add file exist conditions --- .../scene_monitor/CMakeLists.txt | 35 ++++------- .../assembly_state_monitor.hpp} | 0 .../scene_monitor/include/component.hpp | 2 + .../include/component_state_monitor.hpp | 5 ++ .../scene_monitor/src/component.cpp | 7 +++ .../src/component_state_monitor.cpp | 61 ++++++++++++++++--- .../include/pose_estimation.hpp | 0 .../pose_estimation/src/pose_estimation.cpp | 0 .../scene_monitor_interfaces/CMakeLists.txt | 1 + .../srv/GetGraspPose.srv | 8 +++ 10 files changed, 87 insertions(+), 32 deletions(-) rename robossembler_scene_monitor/scene_monitor/{monitor_utils/tf2/tf2_broadcaster.cpp => include/assembly_state_monitor.hpp} (100%) rename robossembler_scene_monitor/scene_monitor/{plugins => utils}/pose_estimation/include/pose_estimation.hpp (100%) rename robossembler_scene_monitor/scene_monitor/{plugins => utils}/pose_estimation/src/pose_estimation.cpp (100%) create mode 100644 robossembler_scene_monitor/scene_monitor_interfaces/srv/GetGraspPose.srv diff --git a/robossembler_scene_monitor/scene_monitor/CMakeLists.txt b/robossembler_scene_monitor/scene_monitor/CMakeLists.txt index 5d78f94..e76072f 100644 --- a/robossembler_scene_monitor/scene_monitor/CMakeLists.txt +++ b/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) -list(APPEND plugin_libs component) - -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 +add_library(component_state_lib SHARED + src/component.cpp + src/component_state_monitor.cpp ) - -foreach(plugin ${plugin_libs}) - ament_target_dependencies(${plugin} ${dependencies}) -endforeach() +ament_target_dependencies(component_state_lib ${dependencies}) 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(DIRECTORY launch -# DESTINATION share/${PROJECT_NAME} -# ) +install(TARGETS + component_state_lib + ARCHIVE DESTINATION lib + 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() \ No newline at end of file diff --git a/robossembler_scene_monitor/scene_monitor/monitor_utils/tf2/tf2_broadcaster.cpp b/robossembler_scene_monitor/scene_monitor/include/assembly_state_monitor.hpp similarity index 100% rename from robossembler_scene_monitor/scene_monitor/monitor_utils/tf2/tf2_broadcaster.cpp rename to robossembler_scene_monitor/scene_monitor/include/assembly_state_monitor.hpp diff --git a/robossembler_scene_monitor/scene_monitor/include/component.hpp b/robossembler_scene_monitor/scene_monitor/include/component.hpp index b90d04e..edb55aa 100644 --- a/robossembler_scene_monitor/scene_monitor/include/component.hpp +++ b/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 getGraspPoses() const; geometry_msgs::msg::Pose getPlacementPose() const; + + void setFrameName(const std::string &frame_name); void initializeFromJson(json &input); /* diff --git a/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp b/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp index 36dafcd..24e20a0 100644 --- a/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp +++ b/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp @@ -6,6 +6,7 @@ #include #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 response); void getGraspPoses(const std::shared_ptr request, std::shared_ptr response); + void getGraspPose(const std::shared_ptr request, + std::shared_ptr response); std::map _loaded_components; rclcpp::Service::SharedPtr _get_part_service; + rclcpp::Service::SharedPtr _get_grasp_pose_service; rclcpp::Service::SharedPtr _get_grasp_poses_service; }; diff --git a/robossembler_scene_monitor/scene_monitor/src/component.cpp b/robossembler_scene_monitor/scene_monitor/src/component.cpp index 7a0c684..7cc87ea 100644 --- a/robossembler_scene_monitor/scene_monitor/src/component.cpp +++ b/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; } } \ No newline at end of file diff --git a/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp b/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp index cf9ded3..f3a1a4e 100644 --- a/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp +++ b/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp @@ -5,7 +5,6 @@ #include #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("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 request, + std::shared_ptr 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::filesystem::path json_path = entry.path().string() + "/frames.json"; - RCLCPP_INFO(this->get_logger(), "Start read frame metadata (%s)", json_path.c_str()); + std::string frame_directory_name = entry.path().string(); + 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 (%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()); } } } \ No newline at end of file diff --git a/robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/include/pose_estimation.hpp b/robossembler_scene_monitor/scene_monitor/utils/pose_estimation/include/pose_estimation.hpp similarity index 100% rename from robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/include/pose_estimation.hpp rename to robossembler_scene_monitor/scene_monitor/utils/pose_estimation/include/pose_estimation.hpp diff --git a/robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/src/pose_estimation.cpp b/robossembler_scene_monitor/scene_monitor/utils/pose_estimation/src/pose_estimation.cpp similarity index 100% rename from robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/src/pose_estimation.cpp rename to robossembler_scene_monitor/scene_monitor/utils/pose_estimation/src/pose_estimation.cpp diff --git a/robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt b/robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt index e4e13df..7160727 100644 --- a/robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt +++ b/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" ) diff --git a/robossembler_scene_monitor/scene_monitor_interfaces/srv/GetGraspPose.srv b/robossembler_scene_monitor/scene_monitor_interfaces/srv/GetGraspPose.srv new file mode 100644 index 0000000..4d23640 --- /dev/null +++ b/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 \ No newline at end of file