91 lines
2.5 KiB
C++
91 lines
2.5 KiB
C++
|
#include "component.hpp"
|
||
|
|
#include <rclcpp/rclcpp.hpp>
|
||
|
|
|
||
|
|
namespace component_state_monitor
|
||
|
|
{
|
||
|
|
Component::Component() {}
|
||
|
|
|
||
|
|
Component::Component(const std::string& frame_name): _frame_name(frame_name) {}
|
||
|
|
|
||
|
|
void Component::updateState(const ComponentState& state)
|
||
|
|
{
|
||
|
|
_current_state = state;
|
||
|
|
}
|
||
|
|
|
||
|
|
ComponentState Component::getCurrentState() const
|
||
|
|
{
|
||
|
|
return _current_state;
|
||
|
|
}
|
||
|
|
|
||
|
|
std::string Component::getFrameName() const
|
||
|
|
{
|
||
|
|
return _frame_name;
|
||
|
|
}
|
||
|
|
|
||
|
|
scene_monitor_interfaces::msg::GraspPose Component::getGraspPose() const
|
||
|
|
{
|
||
|
|
return _grasp_poses.at("grasp-pose-1");
|
||
|
|
}
|
||
|
|
|
||
|
|
geometry_msgs::msg::Pose Component::getPlacementPose() const
|
||
|
|
{
|
||
|
|
return _placement;
|
||
|
|
}
|
||
|
|
|
||
|
|
inline geometry_msgs::msg::Point construct_point(const json& point)
|
||
|
|
{
|
||
|
|
geometry_msgs::msg::Point result_point;
|
||
|
|
|
||
|
|
result_point.x = point.at("x");
|
||
|
|
result_point.y = point.at("y");
|
||
|
|
result_point.z = point.at("z");
|
||
|
|
|
||
|
|
return result_point;
|
||
|
|
}
|
||
|
|
|
||
|
|
inline geometry_msgs::msg::Quaternion construct_quaternion(const json& quaternion)
|
||
|
|
{
|
||
|
|
geometry_msgs::msg::Quaternion result_quaternion;
|
||
|
|
|
||
|
|
result_quaternion.x = quaternion.at("x");
|
||
|
|
result_quaternion.y = quaternion.at("y");
|
||
|
|
result_quaternion.z = quaternion.at("z");
|
||
|
|
result_quaternion.w = quaternion.at("w");
|
||
|
|
|
||
|
|
return result_quaternion;
|
||
|
|
}
|
||
|
|
|
||
|
|
inline geometry_msgs::msg::Pose construct_pose(const json& pose)
|
||
|
|
{
|
||
|
|
geometry_msgs::msg::Pose result_pose;
|
||
|
|
|
||
|
|
result_pose.position = construct_point(pose.at("position"));
|
||
|
|
result_pose.orientation = construct_quaternion(pose.at("orientation"));
|
||
|
|
|
||
|
|
return result_pose;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Component::initializeFromJson(json &input)
|
||
|
|
{
|
||
|
|
if (input.empty())
|
||
|
|
{
|
||
|
|
RCLCPP_WARN(rclcpp::get_logger("component"), "Provided empty json for initialize Component");
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
_frame_name = input.at("label");
|
||
|
|
_placement = construct_pose(input.at("placement"));
|
||
|
|
|
||
|
|
auto grasp_poses = input.at("features").at("grasp-poses");
|
||
|
|
for (auto it = grasp_poses.begin(); it != grasp_poses.end(); ++it)
|
||
|
|
{
|
||
|
|
scene_monitor_interfaces::msg::GraspPose grasp_pose;
|
||
|
|
grasp_pose.label = it.key();
|
||
|
|
grasp_pose.placement_pose = construct_pose(it.value().at("placement"));
|
||
|
|
grasp_pose.distance = it.value().at("distance");
|
||
|
|
|
||
|
|
_grasp_poses.insert({grasp_pose.label, grasp_pose});
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|