runtime/robossembler_scene_monitor/scene_monitor/src/component.cpp

#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 std::string &grasp_pose_name) const
    {
        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;
    }

    void Component::setFrameName(const std::string &frame_name)
    {
        _frame_name = frame_name;
    }

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

        _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});
        }

        _current_state = ComponentState::Initialized;
    }

}