robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

add graspPosesSrv

Browse source
This commit is contained in:
Splinter1984 2022-02-24 00:30:47 +08:00
parent a9105b3992
commit 718fd94d18
10 changed files with 57 additions and 50 deletions
Download patch file Download diff file Expand all files Collapse all files

4
robossembler_scene_monitor/scene_monitor/CMakeLists.txt
View file

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

0
robossembler_scene_monitor/scene_monitor/include/assembly_state_monitor.hpp
View file

3
robossembler_scene_monitor/scene_monitor/include/component.hpp
View file

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

15
robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp
View file

@ -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/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<GetPartSrv::Request> request,
std::shared_ptr<GetPartSrv::Response> response);
// void uploadComponent(const std::shared_ptr<UploadPartSrv::Request> request,
// std::shared_ptr<UploadPartSrv::Response> response);
// void deleteComponent(const std::shared_ptr<DeletePartSrv::Request> request,
// std::shared_ptr<DeletePartSrv::Response> response);
void getGraspPoses(const std::shared_ptr<GetGraspPosesSrv::Request> request,
std::shared_ptr<GetGraspPosesSrv::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<DeletePartSrv>::SharedPtr _delete_part_service;
rclcpp::Service<GetGraspPosesSrv>::SharedPtr _get_grasp_poses_service;
};
}

21
robossembler_scene_monitor/scene_monitor/src/component.cpp
View file

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

48
robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp
View file

@ -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");
// _upload_part_service = this->create_service<UploadPartSrv>("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<DeletePartSrv>("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<GetGraspPosesSrv>("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<UploadPartSrv::Request> request,
// std::shared_ptr<UploadPartSrv::Response> 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<GetGraspPosesSrv::Request> request,
std::shared_ptr<GetGraspPosesSrv::Response> 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<DeletePartSrv::Request> request,
// std::shared_ptr<DeletePartSrv::Response> 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)
{

5
robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt
View file

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

2
robossembler_scene_monitor/scene_monitor_interfaces/srv/DeletePart.srv → robossembler_scene_monitor/scene_monitor_interfaces/srv/GetGraspPoses.srv
View file

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

3
robossembler_scene_monitor/scene_monitor_interfaces/srv/GetPart.srv
View file

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

6
robossembler_scene_monitor/scene_monitor_interfaces/srv/UploadPart.srv
View file

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