From 718fd94d18fb84826c5399c777f945bd25daaf32 Mon Sep 17 00:00:00 2001 From: Splinter1984 Date: Thu, 24 Feb 2022 00:30:47 +0800 Subject: [PATCH] add graspPosesSrv --- .../scene_monitor/CMakeLists.txt | 4 +- .../include/assembly_state_monitor.hpp | 0 .../scene_monitor/include/component.hpp | 3 +- .../include/component_state_monitor.hpp | 15 ++---- .../scene_monitor/src/component.cpp | 21 +++++++- .../src/component_state_monitor.cpp | 48 ++++++++++--------- .../scene_monitor_interfaces/CMakeLists.txt | 5 +- .../srv/{DeletePart.srv => GetGraspPoses.srv} | 2 + .../scene_monitor_interfaces/srv/GetPart.srv | 3 +- .../srv/UploadPart.srv | 6 --- 10 files changed, 57 insertions(+), 50 deletions(-) delete mode 100644 robossembler_scene_monitor/scene_monitor/include/assembly_state_monitor.hpp rename robossembler_scene_monitor/scene_monitor_interfaces/srv/{DeletePart.srv => GetGraspPoses.srv} (86%) delete mode 100644 robossembler_scene_monitor/scene_monitor_interfaces/srv/UploadPart.srv diff --git a/robossembler_scene_monitor/scene_monitor/CMakeLists.txt b/robossembler_scene_monitor/scene_monitor/CMakeLists.txt index 98e8e3b..5d78f94 100644 --- a/robossembler_scene_monitor/scene_monitor/CMakeLists.txt +++ b/robossembler_scene_monitor/scene_monitor/CMakeLists.txt @@ -49,9 +49,7 @@ endforeach() add_executable(component_state_monitor_node src/component_state_monitor_node.cpp) ament_target_dependencies(component_state_monitor_node ${dependencies}) -target_link_libraries(component_state_monitor_node - ${plugin_libs} -) +target_link_libraries(component_state_monitor_node ${plugin_libs}) # install(DIRECTORY launch # DESTINATION share/${PROJECT_NAME} diff --git a/robossembler_scene_monitor/scene_monitor/include/assembly_state_monitor.hpp b/robossembler_scene_monitor/scene_monitor/include/assembly_state_monitor.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/robossembler_scene_monitor/scene_monitor/include/component.hpp b/robossembler_scene_monitor/scene_monitor/include/component.hpp index 74bd20c..b90d04e 100644 --- a/robossembler_scene_monitor/scene_monitor/include/component.hpp +++ b/robossembler_scene_monitor/scene_monitor/include/component.hpp @@ -37,7 +37,8 @@ public: ComponentState getCurrentState() const; std::string getFrameName() const; - scene_monitor_interfaces::msg::GraspPose getGraspPose() const; + scene_monitor_interfaces::msg::GraspPose getGraspPose(const std::string &grasp_pose_name) const; + std::vector getGraspPoses() const; geometry_msgs::msg::Pose getPlacementPose() const; 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 bbfa027..36dafcd 100644 --- a/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp +++ b/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp @@ -6,16 +6,14 @@ #include #include "scene_monitor_interfaces/srv/get_part.hpp" -#include "scene_monitor_interfaces/srv/upload_part.hpp" -#include "scene_monitor_interfaces/srv/delete_part.hpp" +#include "scene_monitor_interfaces/srv/get_grasp_poses.hpp" #include "component.hpp" namespace component_state_monitor { using GetPartSrv = scene_monitor_interfaces::srv::GetPart; -// using UploadPartSrv = scene_monitor_interfaces::srv::UploadPart; -// using DeletePartSrv = scene_monitor_interfaces::srv::DeletePart; +using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses; class ComponentStateMonitor: public rclcpp::Node { @@ -30,15 +28,12 @@ private: void getComponent(const std::shared_ptr request, std::shared_ptr response); - // void uploadComponent(const std::shared_ptr request, - // std::shared_ptr response); - // void deleteComponent(const std::shared_ptr request, - // std::shared_ptr response); + void getGraspPoses(const std::shared_ptr request, + std::shared_ptr response); std::map _loaded_components; rclcpp::Service::SharedPtr _get_part_service; - // rclcpp::Service::SharedPtr _upload_part_service; - // rclcpp::Service::SharedPtr _delete_part_service; + rclcpp::Service::SharedPtr _get_grasp_poses_service; }; } \ No newline at end of file diff --git a/robossembler_scene_monitor/scene_monitor/src/component.cpp b/robossembler_scene_monitor/scene_monitor/src/component.cpp index b12facc..7a0c684 100644 --- a/robossembler_scene_monitor/scene_monitor/src/component.cpp +++ b/robossembler_scene_monitor/scene_monitor/src/component.cpp @@ -22,9 +22,26 @@ namespace component_state_monitor return _frame_name; } - scene_monitor_interfaces::msg::GraspPose Component::getGraspPose() const + scene_monitor_interfaces::msg::GraspPose Component::getGraspPose(const std::string &grasp_pose_name) const { - return _grasp_poses.at("grasp-pose-1"); + if (_grasp_poses.find(grasp_pose_name) == _grasp_poses.end()) + { + RCLCPP_WARN(rclcpp::get_logger("component"), "GraspPose with name (%s) not found", grasp_pose_name.c_str()); + return scene_monitor_interfaces::msg::GraspPose(); + } + + return _grasp_poses.at(grasp_pose_name); + } + + std::vector Component::getGraspPoses() const + { + std::vector grasp_poses; + for (auto &pose: _grasp_poses) + { + grasp_poses.push_back(pose.second); + } + + return grasp_poses; } geometry_msgs::msg::Pose Component::getPlacementPose() const 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 13816af..cf9ded3 100644 --- a/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp +++ b/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp @@ -5,6 +5,7 @@ #include #include "component_state_monitor.hpp" +#include "nlohmann_json/json.hpp" namespace component_state_monitor { @@ -15,15 +16,10 @@ namespace component_state_monitor std::bind(&ComponentStateMonitor::getComponent, this, std::placeholders::_1, std::placeholders::_2)); - // RCLCPP_INFO(this->get_logger(), "Initialize [upload_part] service"); - // _upload_part_service = this->create_service("scene_monitor/upload_part", - // std::bind(&ComponentStateMonitor::uploadComponent, this, - // std::placeholders::_1, std::placeholders::_2)); - - // RCLCPP_INFO(this->get_logger(), "Initialize [delete_part] service"); - // _delete_part_service = this->create_service("scene_monitor/delete_part", - // std::bind(&ComponentStateMonitor::deleteComponent, this, - // std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(this->get_logger(), "Initialize [get_grasp_poses] service"); + _get_grasp_poses_service = this->create_service("scene_monitor/get_grasp_poses", + std::bind(&ComponentStateMonitor::getGraspPoses, this, + std::placeholders::_1, std::placeholders::_2)); std::string package_name = "sdf_models"; initializeComponents(package_name); @@ -41,13 +37,13 @@ namespace component_state_monitor RCLCPP_INFO(this->get_logger(), "[get_part] found frame (%s) in component_state_monitor", request->frame_id.c_str()); auto component = _loaded_components.at(request->frame_id); response->frame_id = component.getFrameName(); - response->grasp_pose = component.getGraspPose(); + response->grasp_poses = component.getGraspPoses(); response->placement_pose = component.getPlacementPose(); response->success = true; response->message = "successfully received a response from the service [get_part] with frame (" + response->frame_id + ")"; } else { - RCLCPP_WARN(this->get_logger(), "[get_psrt] Frame with name (%s) not loaded in component_state_monitor", response->frame_id.c_str()); + RCLCPP_WARN(this->get_logger(), "[get_part] 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"; } @@ -55,19 +51,25 @@ namespace component_state_monitor RCLCPP_INFO(this->get_logger(), "[get_part] Sending back response"); } - // void ComponentStateMonitor::uploadComponent(const std::shared_ptr request, - // std::shared_ptr response) - // { - // RCLCPP_INFO(this->get_logger(), "[upload_part] Incoming request"); - // RCLCPP_INFO(this->get_logger(), "[upload_part] Sending back response"); - // } + void ComponentStateMonitor::getGraspPoses(const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO(this->get_logger(), "[get_grasp_poses] Incoming request"); + if (_loaded_components.find(request->frame_id) != _loaded_components.end()) + { + auto component = _loaded_components.at(request->frame_id); + response->grasp_poses = component.getGraspPoses(); + response->success = true; + response->message = "succesfully received a response from service [get_grasp_poses]"; + } else { - // void ComponentStateMonitor::deleteComponent(const std::shared_ptr request, - // std::shared_ptr response) - // { - // RCLCPP_INFO(this->get_logger(), "[delete_part] Incoming request"); - // RCLCPP_INFO(this->get_logger(), "[delete_part] Sending back response"); - // } + RCLCPP_WARN(this->get_logger(), "[get_grasp_poses] 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_poses] Sending back response"); + } void ComponentStateMonitor::initializeComponents(const std::string& package_name) { diff --git a/robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt b/robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt index efc2aa8..e4e13df 100644 --- a/robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt +++ b/robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt @@ -8,7 +8,7 @@ endif() # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -25,9 +25,8 @@ set(msg_files ) set(srv_files + "srv/GetGraspPoses.srv" "srv/GetPart.srv" - "srv/UploadPart.srv" - "srv/DeletePart.srv" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/robossembler_scene_monitor/scene_monitor_interfaces/srv/DeletePart.srv b/robossembler_scene_monitor/scene_monitor_interfaces/srv/GetGraspPoses.srv similarity index 86% rename from robossembler_scene_monitor/scene_monitor_interfaces/srv/DeletePart.srv rename to robossembler_scene_monitor/scene_monitor_interfaces/srv/GetGraspPoses.srv index d947894..597febf 100644 --- a/robossembler_scene_monitor/scene_monitor_interfaces/srv/DeletePart.srv +++ b/robossembler_scene_monitor/scene_monitor_interfaces/srv/GetGraspPoses.srv @@ -1,5 +1,7 @@ string frame_id --- #result definition +GraspPose[] grasp_poses + bool success # indicate successful run of triggered service string message # informational, e.g. for error messages \ No newline at end of file diff --git a/robossembler_scene_monitor/scene_monitor_interfaces/srv/GetPart.srv b/robossembler_scene_monitor/scene_monitor_interfaces/srv/GetPart.srv index bca3844..f449e2c 100644 --- a/robossembler_scene_monitor/scene_monitor_interfaces/srv/GetPart.srv +++ b/robossembler_scene_monitor/scene_monitor_interfaces/srv/GetPart.srv @@ -2,9 +2,8 @@ string frame_id --- #result definition string frame_id -string material geometry_msgs/Pose placement_pose -GraspPose grasp_pose +GraspPose[] grasp_poses bool success # indicate successful run of triggered service string message # informational, e.g. for error messages diff --git a/robossembler_scene_monitor/scene_monitor_interfaces/srv/UploadPart.srv b/robossembler_scene_monitor/scene_monitor_interfaces/srv/UploadPart.srv deleted file mode 100644 index 399d7e2..0000000 --- a/robossembler_scene_monitor/scene_monitor_interfaces/srv/UploadPart.srv +++ /dev/null @@ -1,6 +0,0 @@ -string frame_id -string material ---- -#result definition -bool success # indicate successful run of triggered service -string message # informational, e.g. for error messages \ No newline at end of file