add graspPosesSrv

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
+target_link_libraries(component_state_monitor_node ${plugin_libs})
-    ${plugin_libs}
-)
 
 # install(DIRECTORY launch
 #     DESTINATION share/${PROJECT_NAME}

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<scene_monitor_interfaces::msg::GraspPose> getGraspPoses() const;
     geometry_msgs::msg::Pose getPlacementPose() const;
     
     void initializeFromJson(json &input);

robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp

@@ -6,16 +6,14 @@
 #include <rclcpp/timer.hpp>
 
 #include "scene_monitor_interfaces/srv/get_part.hpp"
-#include "scene_monitor_interfaces/srv/upload_part.hpp"
+#include "scene_monitor_interfaces/srv/get_grasp_poses.hpp"
-#include "scene_monitor_interfaces/srv/delete_part.hpp"
 
 #include "component.hpp"
 
 namespace component_state_monitor
 {
 using GetPartSrv    = scene_monitor_interfaces::srv::GetPart;
-// using UploadPartSrv = scene_monitor_interfaces::srv::UploadPart;
+using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses;
-// using DeletePartSrv = scene_monitor_interfaces::srv::DeletePart;
 
 class ComponentStateMonitor: public rclcpp::Node
 {
@@ -30,15 +28,12 @@ private:
 
     void getComponent(const std::shared_ptr<GetPartSrv::Request> request,
         std::shared_ptr<GetPartSrv::Response> response);
-    // void uploadComponent(const std::shared_ptr<UploadPartSrv::Request> request,
+    void getGraspPoses(const std::shared_ptr<GetGraspPosesSrv::Request> request,
-    //     std::shared_ptr<UploadPartSrv::Response> response);
+        std::shared_ptr<GetGraspPosesSrv::Response> response);
-    // void deleteComponent(const std::shared_ptr<DeletePartSrv::Request> request,
-    //     std::shared_ptr<DeletePartSrv::Response> response);
 
     std::map<std::string, Component> _loaded_components;
     rclcpp::Service<GetPartSrv>::SharedPtr _get_part_service;
-    // rclcpp::Service<UploadPartSrv>::SharedPtr _upload_part_service;
+    rclcpp::Service<GetGraspPosesSrv>::SharedPtr _get_grasp_poses_service;
-    // rclcpp::Service<DeletePartSrv>::SharedPtr _delete_part_service;
 
 };
 }

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<scene_monitor_interfaces::msg::GraspPose> Component::getGraspPoses() const
+    {
+        std::vector<scene_monitor_interfaces::msg::GraspPose> grasp_poses;
+        for (auto &pose: _grasp_poses)
+        {
+            grasp_poses.push_back(pose.second);
+        }
+
+        return grasp_poses;
     }
 
     geometry_msgs::msg::Pose Component::getPlacementPose() const

robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp

@@ -5,6 +5,7 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
 #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");
+        RCLCPP_INFO(this->get_logger(), "Initialize [get_grasp_poses] service");
-        // _upload_part_service = this->create_service<UploadPartSrv>("scene_monitor/upload_part", 
+        _get_grasp_poses_service = this->create_service<GetGraspPosesSrv>("scene_monitor/get_grasp_poses", 
-        //                                 std::bind(&ComponentStateMonitor::uploadComponent, this, 
+                                        std::bind(&ComponentStateMonitor::getGraspPoses, this, 
-        //                                 std::placeholders::_1, std::placeholders::_2));
+                                        std::placeholders::_1, std::placeholders::_2));
-
-        // RCLCPP_INFO(this->get_logger(), "Initialize [delete_part] service");
-        // _delete_part_service = this->create_service<DeletePartSrv>("scene_monitor/delete_part", 
-        //                                 std::bind(&ComponentStateMonitor::deleteComponent, 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<UploadPartSrv::Request> request,
+    void ComponentStateMonitor::getGraspPoses(const std::shared_ptr<GetGraspPosesSrv::Request> request,
-    //         std::shared_ptr<UploadPartSrv::Response> response)
+        std::shared_ptr<GetGraspPosesSrv::Response> response)
-    // {
+    {
-    //     RCLCPP_INFO(this->get_logger(), "[upload_part] Incoming request");
+        RCLCPP_INFO(this->get_logger(), "[get_grasp_poses] Incoming request");
-    //     RCLCPP_INFO(this->get_logger(), "[upload_part] Sending back response");
+        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<DeletePartSrv::Request> request,
+            RCLCPP_WARN(this->get_logger(), "[get_grasp_poses] Frame with name (%s) not loaded in component_state_monitor", request->frame_id.c_str());
-    //         std::shared_ptr<DeletePartSrv::Response> response)
+            response->success = false;
-    // {
+            response->message = "could not found frame (%s) in component_state_monitor";
-    //     RCLCPP_INFO(this->get_logger(), "[delete_part] Incoming request");
+        }
-    //     RCLCPP_INFO(this->get_logger(), "[delete_part] Sending back response");
+
-    // }
+        RCLCPP_INFO(this->get_logger(), "[get_grasp_poses] Sending back response");
+    }
 
     void ComponentStateMonitor::initializeComponents(const std::string& package_name)
     {

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}

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

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

robossembler_scene_monitor/scene_monitor_interfaces/srv/UploadPart.srv

@@ -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