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

add file exist conditions

Browse source
This commit is contained in:
Splinter1984 2022-02-24 02:18:59 +08:00
parent 718fd94d18
commit 97bbf46c6d
10 changed files with 87 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

35
robossembler_scene_monitor/scene_monitor/CMakeLists.txt
View file

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

0
robossembler_scene_monitor/scene_monitor/monitor_utils/tf2/tf2_broadcaster.cpp → robossembler_scene_monitor/scene_monitor/include/assembly_state_monitor.hpp
View file

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

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

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

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

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

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

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

@ -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::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());
}
}
}

0
robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/include/pose_estimation.hpp → robossembler_scene_monitor/scene_monitor/utils/pose_estimation/include/pose_estimation.hpp
View file

0
robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/src/pose_estimation.cpp → robossembler_scene_monitor/scene_monitor/utils/pose_estimation/src/pose_estimation.cpp
View file

1
robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt
View file

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

8
robossembler_scene_monitor/scene_monitor_interfaces/srv/GetGraspPose.srv Normal file
View file

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