robossembler/runtime
8
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

correct merge

Browse source
This commit is contained in:
Splinter1984 2022-03-21 23:20:15 +08:00
parent f18ccccd7e
commit e6aecabde6
36 changed files with 1040 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

33
robossembler_scene_monitor/printer/include/printer_base.hpp Normal file
View file

@ -0,0 +1,33 @@
#pragma once
#include <string>
#include <rclcpp/rclcpp.hpp>
// #include <geometry_msgs/msg/pose.hpp>
namespace printer
{
template <class Derived>
class PrinterBase: public rclcpp::Node
{
inline const Derived& derived() const& noexcept
{
return *static_cast<const Derived*>(this);
}
public:
explicit PrinterBase(const rclcpp::NodeOptions& options);
virtual ~PrinterBase() = default;
// virtual void print(const std::string& frame_name,
// const geometry_msgs::msg::Pose& placement_pose,
// const std::string& package_name);
// virtual ~Printer();
protected:
std::string _printer_name;
// geometry_msgs::msg::Pose _printer_pose;
};
} // namespace printer

15
robossembler_scene_monitor/printer/src/gz_printer_plugin.cpp Normal file
View file

@ -0,0 +1,15 @@
#include "printer_base.hpp"
namespace printer_plugins
{
class GZPrinter : public printer_base::Printer
{
public:
void init(const std::string &printer_name) override
{
this->_printer_name = printer_name;
}
};
}// namespace printer_plugins

14
robossembler_scene_monitor/scene_monitor/CMakeLists.txt
View file

@ -13,12 +13,21 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(ament_index_cpp REQUIRED)
# find_package(tf2_ros REQUIRED)
# find_package(visualization_msgs REQUIRED)
find_package(gazebo_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(scene_monitor_interfaces REQUIRED)
find_package(nlohmann_json REQUIRED)
set(dependencies
# visualization_msgs
std_msgs
tf2
tf2_ros
gazebo_msgs
rclcpp
geometry_msgs
ament_index_cpp
@ -29,6 +38,8 @@ set(dependencies
include_directories(include)
add_library(component_state_lib SHARED
src/component_pose_estimation.cpp
src/component_spawner.cpp
src/component.cpp
src/component_state_monitor.cpp
)
@ -41,6 +52,7 @@ ament_target_dependencies(component_state_monitor_node ${dependencies})
install(DIRECTORY
launch
config
DESTINATION share/${PROJECT_NAME}
)

11
robossembler_scene_monitor/scene_monitor/config/component_state_node.param.yaml Normal file
View file

@ -0,0 +1,11 @@
component_state_monitor_node:
ros__parameters:
models-package: "sdf_models"
printers_names: ["printerA"]
printers:
printerA:
position: [0.5, 0.5, 0.0]
orientation: [0.0, 0.0, 0.0, 0.0]
# printerB:
# position: [2.0, 2.0, 2.0]
# orientation: [2.0, 2.0, 2.0, 2.0]

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

@ -1,7 +1,9 @@
#pragma once
#include <string>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include "scene_monitor_interfaces/msg/grasp_pose.hpp"
#include "nlohmann_json/json.hpp"
@ -19,6 +21,7 @@ Description:
Posted:
- model is posted with another
*/
enum ComponentState
{
Initialized,
@ -45,7 +48,6 @@ public:
void updateState(const ComponentState &state);
private:
std::string _frame_name;
@ -53,6 +55,7 @@ private:
geometry_msgs::msg::Pose _placement;
std::map<std::string, scene_monitor_interfaces::msg::GraspPose> _grasp_poses;
geometry_msgs::msg::Pose _current_pose;
};
}

23
robossembler_scene_monitor/scene_monitor/include/component_pose_estimation.hpp Normal file
View file

@ -0,0 +1,23 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <gazebo_msgs/srv/get_entity_state.hpp>
namespace component_state_monitor
{
using GetEntityStateSrv = gazebo_msgs::srv::GetEntityState;
class ComponentPoseEstimation: public rclcpp::Node
{
public:
ComponentPoseEstimation();
geometry_msgs::msg::Pose framePose(const std::string& frame_id);
private:
rclcpp::Client<GetEntityStateSrv>::SharedPtr _client;
geometry_msgs::msg::Pose _pose;
};
}// end namespace component_state_monitor

36
robossembler_scene_monitor/scene_monitor/include/component_spawner.hpp Normal file
View file

@ -0,0 +1,36 @@
#pragma once
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <gazebo_msgs/srv/spawn_entity.hpp>
#include "component.hpp"
namespace component_state_monitor
{
using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
class ComponentSpawner: public rclcpp::Node
{
public:
ComponentSpawner();
~ComponentSpawner();
bool spawnComponent(const component_state_monitor::Component& component,
const geometry_msgs::msg::Pose& printer_pose,
const std::string& printer_name);
void handleResponse(SpawnEntitySrv::Response::SharedPtr response);
private:
rclcpp::Client<SpawnEntitySrv>::SharedPtr _spawn_component_client;
bool _result;
};
} // namespace component_state_monitor

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

@ -4,18 +4,28 @@
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/service.hpp>
#include <rclcpp/timer.hpp>
#include <gazebo_msgs/srv/spawn_entity.hpp>
#include <tf2_ros/static_transform_broadcaster_node.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <visualization_msgs/msg/marker_array.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 "scene_monitor_interfaces/srv/print_part.hpp"
#include "component.hpp"
#include "component_spawner.hpp"
#include "component_pose_estimation.hpp"
namespace component_state_monitor
{
using GetPartSrv = scene_monitor_interfaces::srv::GetPart;
using GetPartSrv = scene_monitor_interfaces::srv::GetPart;
using GetGraspPoseSrv = scene_monitor_interfaces::srv::GetGraspPose;
using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses;
using PrintPartSrv = scene_monitor_interfaces::srv::PrintPart;
class ComponentStateMonitor: public rclcpp::Node
{
@ -35,13 +45,29 @@ private:
std::shared_ptr<GetGraspPosesSrv::Response> response);
void getGraspPose(const std::shared_ptr<GetGraspPoseSrv::Request> request,
std::shared_ptr<GetGraspPoseSrv::Response> response);
void printComponent(const std::shared_ptr<PrintPartSrv::Request> request,
std::shared_ptr<PrintPartSrv::Response> response);
void makeTransform(const Component& component, const std::string& parent);
void makeTransform(const std::string& frame_name, const geometry_msgs::msg::Pose& frame_pose,
const std::string& parent);
void updateVizMarker(const Component& component);
void updateComponents();
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;
rclcpp::Service<PrintPartSrv>::SharedPtr _print_part_service;
std::string _models_package;
std::map<std::string, geometry_msgs::msg::Pose> _loaded_printers;
ComponentSpawner _component_spawner_node;
ComponentPoseEstimation _pose_estimation_node;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> _tf2_broadcaster;
std::shared_ptr<tf2_ros::TransformListener> _transform_listener;
std::unique_ptr<tf2_ros::Buffer> _tf2_buffer;
rclcpp::TimerBase::SharedPtr _timer;
};

27
robossembler_scene_monitor/scene_monitor/launch/scene_monitor.launch.py
View file

@ -1,17 +1,26 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
ld = LaunchDescription()
component_state_params = os.path.join(
get_package_share_directory('robossembler_scene_monitor'),
'config', 'component_state_node.param.yaml')
component_state_monitor_node = Node(
package="robossembler_scene_monitor",
executable="component_state_monitor_node",
name="component_state_monitor_node",
output="screen",
emulate_tty=True,
parameters=[
{"models-package": "sdf_models"}
]
)
package="robossembler_scene_monitor",
executable="component_state_monitor_node",
name="component_state_monitor_node",
output="screen",
emulate_tty=True,
parameters=[
component_state_params
]
)
ld.add_action(component_state_monitor_node)
return ld

7
robossembler_scene_monitor/scene_monitor/package.xml
View file

@ -10,12 +10,15 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<!-- <depend>tf2_ros</depend> -->
<!-- <depend>rclcpp_action</depend> -->
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>std_msgs</depend>
<!-- <depend>visualization_msgs</depend> -->
<depend>geometry_msgs</depend>
<!-- <depend>action_msgs</depend> -->
<depend>scene_monitor_interfaces</depend>
<depend>nlohmann_json</depend>
<depend>printer_plugins</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

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

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

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

@ -95,7 +95,6 @@ namespace component_state_monitor
return;
}
_frame_name = input.at("label");
_placement = construct_pose(input.at("placement"));
auto grasp_poses = input.at("features").at("grasp-poses");

55
robossembler_scene_monitor/scene_monitor/src/component_pose_estimation.cpp Normal file
View file

@ -0,0 +1,55 @@
#include "component_pose_estimation.hpp"
namespace component_state_monitor
{
using namespace std::chrono_literals;
ComponentPoseEstimation::ComponentPoseEstimation(): Node("component_pose_estimation_node")
{
_client = create_client<GetEntityStateSrv>("/get_entity_state");
while (!_client->wait_for_service(1s))
{
if (!rclcpp::ok())
{
RCLCPP_WARN(this->get_logger(), "Interruped while waiting for the server.");
rclcpp::shutdown();
return;
}
RCLCPP_WARN(this->get_logger(), "Server not available, waiting again...");
}
}
geometry_msgs::msg::Pose ComponentPoseEstimation::framePose(const std::string& frame_id)
{
auto request = std::make_shared<GetEntityStateSrv::Request>();
request->name = frame_id;
RCLCPP_INFO(this->get_logger(), "Provided service call with frame name [%s]", request->name.c_str());
auto future = _client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(this->get_logger(), "Could not build future");
return geometry_msgs::msg::Pose();
}
auto result = future.get();
if (!result->success)
{
RCLCPP_WARN(this->get_logger(), "Get bad response from service call");
return geometry_msgs::msg::Pose();
}
_pose = result->state.pose;
RCLCPP_INFO(this->get_logger(), "Get success response status");
RCLCPP_INFO(this->get_logger(), "update frame (%s) with \npose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f",
frame_id.c_str(), _pose.position.x, _pose.position.y, _pose.position.z,
_pose.orientation.x, _pose.orientation.y, _pose.orientation.z, _pose.orientation.w);
return _pose;
}
}// end namespace component_state_monitor

109
robossembler_scene_monitor/scene_monitor/src/component_spawner.cpp Normal file
View file

@ -0,0 +1,109 @@
#include "component_spawner.hpp"
#include <filesystem>
#include <fstream>
#include <sstream>
#include <ament_index_cpp/get_package_share_directory.hpp>
namespace component_state_monitor
{
using namespace std::chrono_literals;
ComponentSpawner::ComponentSpawner(): Node("component_spawner_node")
{
_spawn_component_client = create_client<SpawnEntitySrv>("/spawn_entity");
while (!_spawn_component_client->wait_for_service(1s))
{
if (!rclcpp::ok())
{
RCLCPP_WARN(this->get_logger(), "Interruped while waiting for the server.");
rclcpp::shutdown();
return;
}
RCLCPP_WARN(this->get_logger(), "Server not available, waiting again...");
}
}
ComponentSpawner::~ComponentSpawner() {}
bool ComponentSpawner::spawnComponent(const component_state_monitor::Component& component,
const geometry_msgs::msg::Pose& printer_pose,
const std::string& package_name)
{
auto request = std::make_shared<SpawnEntitySrv::Request>();
request->name = component.getFrameName();
RCLCPP_INFO(this->get_logger(), "Provided service call with frame name (%s)", request->name.c_str());
std::string package_share_directory = ament_index_cpp::get_package_share_directory(package_name);
std::filesystem::path sdf_path = package_share_directory + "/models/" + component.getFrameName() + "/model.sdf";
RCLCPP_INFO(this->get_logger(), "Start read frame (%s) frame_path (%s)", component.getFrameName().c_str(), sdf_path.c_str());
std::ifstream sdf_file(sdf_path);
std::stringstream buffer;
if (sdf_file.is_open()) {
buffer << sdf_file.rdbuf();
sdf_file.close();
} else {
RCLCPP_WARN(this->get_logger(), "Failed while try to open file (%s)", sdf_path.c_str());
_result = false;
return _result;
}
std::string xml = buffer.str();
geometry_msgs::msg::Pose spawn_pose;
auto placement_pose = component.getPlacementPose();
spawn_pose.position.x = printer_pose.position.x + placement_pose.position.x;
spawn_pose.position.y = printer_pose.position.y + placement_pose.position.y;
spawn_pose.position.z = printer_pose.position.z + placement_pose.position.z;
spawn_pose.orientation.x = printer_pose.orientation.x + placement_pose.orientation.x;
spawn_pose.orientation.y = printer_pose.orientation.y + placement_pose.orientation.y;
spawn_pose.orientation.z = printer_pose.orientation.z + placement_pose.orientation.z;
spawn_pose.orientation.w = printer_pose.orientation.w + placement_pose.orientation.w;
request->initial_pose = spawn_pose;
request->xml = xml;
RCLCPP_INFO(this->get_logger(), "Connecting to '/spawn_entity' service...");
auto future = _spawn_component_client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(this->get_logger(), "Could not build future");
return false;
}
auto result = future.get();
if (!result->success)
{
RCLCPP_WARN(this->get_logger(), "Get bad response from service call");
return false;
}
return true;
}
void ComponentSpawner::handleResponse(SpawnEntitySrv::Response::SharedPtr response)
{
RCLCPP_INFO(this->get_logger(), "Handle Response call");
if (!response->success)
{
RCLCPP_WARN(this->get_logger(), "Get failure response from service with msg: `%s`", response->status_message.c_str());
_result = false;
return;
}
RCLCPP_INFO(this->get_logger(), "Get success response status with msg: `%s`", response->status_message.c_str());
_result= true;
}
}

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

@ -13,6 +13,7 @@ namespace component_state_monitor
ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor_node")
{
this->declare_parameter<std::string>("models-package", "sdf_models");
this->declare_parameter<std::vector<std::string>>("printers_names", std::vector<std::string>{});
RCLCPP_INFO(this->get_logger(), "Initialize [get_part] service");
_get_part_service = this->create_service<GetPartSrv>("scene_monitor/get_part",
@ -29,15 +30,124 @@ namespace component_state_monitor
std::bind(&ComponentStateMonitor::getGraspPose, this,
std::placeholders::_1, std::placeholders::_2));
RCLCPP_INFO(this->get_logger(), "Initialize [print_part] service");
_print_part_service = this->create_service<PrintPartSrv>("scene_monitor/print_part",
std::bind(&ComponentStateMonitor::printComponent, this,
std::placeholders::_1, std::placeholders::_2));
_tf2_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
_tf2_buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
_transform_listener = std::make_shared<tf2_ros::TransformListener>(*_tf2_buffer);
initializeParameters();
initializeComponents(_models_package);
// _viz_marker_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("scene_monitor_viz_markers", 10);
_timer = this->create_wall_timer(
20ms, std::bind(&ComponentStateMonitor::updateComponents, this)
);
}
void ComponentStateMonitor::updateComponents()
{
for (const auto& component: _loaded_components)
{
if (component.second.getCurrentState() == ComponentState::Printed)
{
makeTransform(component.second, "world");
auto grasp_poses = component.second.getGraspPoses();
for (const auto& grasp_pose: grasp_poses)
{
makeTransform(grasp_pose.label, grasp_pose.placement_pose, component.first);
}
}
}
}
void ComponentStateMonitor::makeTransform(const Component& component, const std::string& parent)
{
rclcpp::Time now = this->get_clock()->now();
geometry_msgs::msg::TransformStamped t;
t.header.stamp = now;
t.header.frame_id = parent;
t.child_frame_id = component.getFrameName();
auto fp = _pose_estimation_node.framePose(component.getFrameName());
t.transform.translation.x = fp.position.x;
t.transform.translation.y = fp.position.y;
t.transform.translation.z = fp.position.z;
t.transform.rotation.x = fp.orientation.x;
t.transform.rotation.y = fp.orientation.y;
t.transform.rotation.z = fp.orientation.z;
t.transform.rotation.w = fp.orientation.w;
_tf2_broadcaster->sendTransform(t);
}
void ComponentStateMonitor::makeTransform(const std::string& frame_name, const geometry_msgs::msg::Pose& frame_pose,
const std::string& parent)
{
rclcpp::Time now = this->get_clock()->now();
geometry_msgs::msg::TransformStamped t;
t.header.stamp = now;
t.header.frame_id = parent;
t.child_frame_id = frame_name;
t.transform.translation.x = frame_pose.position.x;
t.transform.translation.y = frame_pose.position.y;
t.transform.translation.z = frame_pose.position.z;
t.transform.rotation.x = frame_pose.orientation.x;
t.transform.rotation.y = frame_pose.orientation.y;
t.transform.rotation.z = frame_pose.orientation.z;
t.transform.rotation.w = frame_pose.orientation.w;
_tf2_broadcaster->sendTransform(t);
}
void ComponentStateMonitor::initializeParameters()
{
this->get_parameter("models-package", _models_package);
RCLCPP_INFO(this->get_logger(), "Initialize with models-package (%s)", _models_package.c_str());
std::vector<std::string> printers;
this->get_parameter("printers_names", printers);
for (const auto& printer: printers)
{
std::vector<double> position;
std::vector<double> orientation;
this->declare_parameter<std::vector<double>>("printers." + printer + ".position", std::vector<double>{0.0, 0.0, 0.0});
this->declare_parameter<std::vector<double>>("printers." + printer + ".orientation", std::vector<double>{0.0, 0.0, 0.0, 0.0});
this->get_parameter("printers." + printer + ".position", position);
RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) position (%f, %f, %f)",
printer.c_str(), position.at(0), position.at(1), position.at(2));
this->get_parameter("printers." + printer + ".orientation", orientation);
RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) orientation (%f, %f, %f, %f)",
printer.c_str(), orientation.at(0), orientation.at(1), orientation.at(2), orientation.at(3));
geometry_msgs::msg::Pose tmp_pose;
tmp_pose.position.x = position.at(0);
tmp_pose.position.y = position.at(1);
tmp_pose.position.z = position.at(2);
tmp_pose.orientation.x = orientation.at(0);
tmp_pose.orientation.y = orientation.at(1);
tmp_pose.orientation.y = orientation.at(2);
tmp_pose.orientation.z = orientation.at(3);
_loaded_printers.insert({printer, tmp_pose});
// makeTransform(printer, tmp_pose, "world");
}
}
ComponentStateMonitor::~ComponentStateMonitor() {}
@ -70,6 +180,7 @@ namespace component_state_monitor
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);
@ -90,22 +201,100 @@ namespace component_state_monitor
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]";
if (component.getCurrentState() == ComponentState::Printed)
{
// std_msgs::msg::Header header;
// header.frame_id = request->frame_id;
// geometry_msgs::msg::PoseStamped target_pose;
// target_pose.header = header;
// target_pose.pose = component.getGraspPose(header.frame_id).placement_pose;
// std::string world = "world";
// geometry_msgs::msg::PoseStamped target_pose_req =
// _tf2_buffer->transform(target_pose, world);
geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
"world", request->grasp_pose_name, tf2::TimePointZero
);
auto grasp_pose = component.getGraspPose(request->grasp_pose_name);
grasp_pose.placement_pose.position.x = transformStamped.transform.translation.x;
grasp_pose.placement_pose.position.y = transformStamped.transform.translation.y;
grasp_pose.placement_pose.position.z = transformStamped.transform.translation.z;
grasp_pose.placement_pose.orientation.x = transformStamped.transform.rotation.x;
grasp_pose.placement_pose.orientation.y = transformStamped.transform.rotation.y;
grasp_pose.placement_pose.orientation.z = transformStamped.transform.rotation.z;
grasp_pose.placement_pose.orientation.w = transformStamped.transform.rotation.w;
response->grasp_pose = grasp_pose;
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 printed", request->frame_id.c_str());
response->success = false;
response->message = "Frame not printed";
}
} 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";
response->message = "could not found frame in component_state_monitor";
}
RCLCPP_INFO(this->get_logger(), "[get_grasp_pose] Sending back response");
}
void ComponentStateMonitor::printComponent(const std::shared_ptr<PrintPartSrv::Request> request,
std::shared_ptr<PrintPartSrv::Response> response)
{
RCLCPP_INFO(this->get_logger(), "[print_part] Incoming request");
if (_loaded_components.find(request->frame_id) != _loaded_components.end())
{
auto component = _loaded_components.at(request->frame_id);
if (_loaded_printers.find(request->printer_id) != _loaded_printers.end())
{
auto printer_pose = _loaded_printers.at(request->printer_id);
if (_component_spawner_node.spawnComponent(component, printer_pose, _models_package))
{
RCLCPP_INFO(this->get_logger(), "[print_part] Succesfully spawn frame (%s) at printer (%s)",
request->frame_id.c_str(), request->printer_id.c_str());
response->success = true;
response->message = "succesfully received a response from service [print_part]";
_loaded_components.at(request->frame_id).updateState(ComponentState::Printed);
} else {
RCLCPP_WARN(this->get_logger(), "[print_part] Failed to spawn frame (%s) at printer (%s)",
request->frame_id.c_str(), request->printer_id.c_str());
response->success = false;
response->message = "printer not available";
}
} else {
RCLCPP_WARN(this->get_logger(), "[print_part] Failed to spawn frame (%s) at printer (%s)",
request->frame_id.c_str(), request->printer_id.c_str());
response->success = false;
response->message = "could not found printer in component_state_monitor";
}
} else {
RCLCPP_WARN(this->get_logger(), "[print_part] Failed to spawn frame (%s) at printer (%s)",
request->frame_id.c_str(), request->printer_id.c_str());
response->success = false;
response->message = "could not found frame in component_state_monitor";
}
RCLCPP_INFO(this->get_logger(), "[print_part] Sending back response");
}
void ComponentStateMonitor::initializeComponents(const std::string& package_name)
{
RCLCPP_INFO(this->get_logger(), "Start read package (%s)", package_name.c_str());
@ -129,10 +318,9 @@ namespace component_state_monitor
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";
Component component;
Component component(frame_name);
if (!std::filesystem::exists(json_path))
{
@ -150,9 +338,10 @@ namespace component_state_monitor
component.initializeFromJson(input);
}
component.updateState(ComponentState::Initialized);
_loaded_components.insert({component.getFrameName(), component});
RCLCPP_INFO(this->get_logger(), "Successfully initialize component (%s)", component.getFrameName().c_str());
}
}
}
} // end namespace component_state_monitor

2
robossembler_scene_monitor/scene_monitor/src/component_state_monitor_node.cpp
View file

@ -6,7 +6,7 @@
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor exec;
rclcpp::executors::MultiThreadedExecutor exec;
auto component_state_monitor_node = std::make_shared<component_state_monitor::ComponentStateMonitor>();

31
robossembler_scene_monitor/scene_monitor_components/include/printer/printer_component_base.hpp Normal file
View file

@ -0,0 +1,31 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/client.hpp>
#include <gazebo_msgs/srv/spawn_entity.hpp>
namespace printer
{
using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
class PrinterBase: public rclcpp::Node
{
public:
explicit PrinterBase(const rclcpp::NodeOptions& options);
// virtual ~PrinterBase() = default;
void PrintComponent(const std::string& printer_id,
const std::string& component_id);
void handleResponse(SpawnEntitySrv::Response::SharedPtr& response);
protected:
rclcpp::Client<SpawnEntitySrv>::SharedPtr _client;
rclcpp::TimerBase::SharedPtr _timer;
};
}// end namespace printer

36
robossembler_scene_monitor/scene_monitor_components/src/printer_component.cpp Normal file
View file

@ -0,0 +1,36 @@
#include "printer/printer_component_base.hpp"
namespace printer
{
using namespace std::chrono_literals;
PrinterBase::PrinterBase(const rclcpp::NodeOptions& options): Node("printer_component", options)
{
_client = create_client<SpawnEntitySrv>("printer_client");
while (!_client->wait_for_service(1s))
{
if (!rclcpp::ok())
{
RCLCPP_WARN(this->get_logger(), "Interruped while waiting for the server.");
rclcpp::shutdown();
return;
}
RCLCPP_WARN(this->get_logger(), "Server not available, waiting again...");
}
}
void PrinterBase::PrintComponent(const std::string& printer_id,
const std::string& component_id)
{
}
} // end namespace printer
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(printer::PrinterBase)

8
robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt
View file

@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@ -19,12 +19,14 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
set(msg_files
"msg/GraspPose.msg"
)
set(srv_files
"srv/PrintPart.srv"
"srv/GetGraspPose.srv"
"srv/GetGraspPoses.srv"
"srv/GetPart.srv"
@ -33,7 +35,7 @@ set(srv_files
rosidl_generate_interfaces(${PROJECT_NAME}
${msg_files}
${srv_files}
DEPENDENCIES geometry_msgs
DEPENDENCIES geometry_msgs std_msgs
)
if(BUILD_TESTING)

2
robossembler_scene_monitor/scene_monitor_interfaces/package.xml
View file

@ -11,6 +11,8 @@
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

6
robossembler_scene_monitor/scene_monitor_interfaces/srv/PrintPart.srv Normal file
View file

@ -0,0 +1,6 @@
string frame_id
string printer_id
---
#result definition
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages