runtime/rbs_bt_executor/src/PoseEstimation.cpp

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behavior_tree/BtService.hpp>

#include <behaviortree_cpp_v3/basic_types.h>
#include <string>

#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"

#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"

using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
class PoseEstimation : public BtService<PoseEstimationSrv> {
public:
  PoseEstimation(const std::string &name, const BT::NodeConfiguration &config)
      : BtService<PoseEstimationSrv>(name, config) {
    RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");

    _set_params_client =
        this->_node->create_client<rcl_interfaces::srv::SetParameters>(
            "/image_sub2/set_parameters");

    while (!_set_params_client->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(_node->get_logger(),
                     "Interrupted while waiting for the service. Exiting.");
        break;
      }
      RCLCPP_WARN(_node->get_logger(),
                  "service not available, waiting again...");
    }

    _model_name = getInput<std::string>("ObjectName").value();
  }

  PoseEstimationSrv::Request::SharedPtr populate_request() {
    auto request = std::make_shared<PoseEstimationSrv::Request>();
    _req_type = getInput<std::string>("ReqKind").value();
    request->set__transition(transition_event(_req_type));
    return request;
  }

  BT::NodeStatus
  handle_response(const PoseEstimationSrv::Response::SharedPtr response) {
    if (response->success) {
      return BT::NodeStatus::SUCCESS;
    }
    return BT::NodeStatus::FAILURE;
  }

  static BT::PortsList providedPorts() {
    return providedBasicPorts({
        BT::InputPort<std::string>("ReqKind"),
        BT::InputPort<std::string>("ObjectName"),
        BT::InputPort<std::string>("ObjectPath"),
    });
  }

private:
  uint8_t transition_id_{};
  std::string _model_name{};
  std::string _model_path{};
  std::string _req_type{};
  rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr
      _set_params_client;
  std::vector<rcl_interfaces::msg::Parameter> _params;
  rcl_interfaces::msg::Parameter _param;

  lifecycle_msgs::msg::Transition
  transition_event(const std::string &req_type) {
    lifecycle_msgs::msg::Transition translation{};
    if (req_type == "configure") {
      set_mesh_param();
      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
    } else if (req_type == "activate") {
      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
    } else if (req_type == "deactivate") {
      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
    } else if (req_type == "cleanup") {
      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
    }
    translation.set__id(transition_id_);
    return translation;
  }

  inline std::string build_model_path(const std::string &model_name,
                                      const std::string &package_path) {
    return package_path + "/config/" + model_name + ".ply";
  }

  inline std::string build_model_path(const std::string &model_path) {
    return model_path;
  }

  void set_mesh_param() {
    auto _package_name =
        ament_index_cpp::get_package_share_directory("rbs_perception");
    _model_path = build_model_path(_model_name, _package_name);
    RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);

    auto param_request =
        std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
    rcl_interfaces::msg::ParameterValue model_path_param_value;
    model_path_param_value.set__string_value(_model_path);
    _param.name = "mesh_path";
    _param.value = model_path_param_value;
    _params.push_back(_param);
    param_request->set__parameters(_params);

    auto response = send_request(_node, _set_params_client, param_request);
    RCLCPP_INFO(_node->get_logger(), "Response");

    for (auto &tt : response->results) {
      RCLCPP_INFO(_node->get_logger(), "%d \n", tt.successful);
    }
  }

  rcl_interfaces::srv::SetParameters::Response::SharedPtr send_request(
      rclcpp::Node::SharedPtr node,
      rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr client,
      rcl_interfaces::srv::SetParameters::Request::SharedPtr request) {
    auto result = client->async_send_request(request);
    if (rclcpp::spin_until_future_complete(node, result) ==
        rclcpp::FutureReturnCode::SUCCESS) {
      return result.get();
    } else {
      return nullptr;
    }
  }

};

#include "behaviortree_cpp_v3/bt_factory.h"

BT_REGISTER_NODES(factory) {
  factory.registerNodeType<PoseEstimation>("PoseEstimation");
}