#include "behaviortree_ros2/bt_service_node.hpp" #include "rcl_interfaces/msg/parameter.hpp" #include "rcl_interfaces/srv/set_parameters.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include #include using namespace BT; using namespace std::chrono_literals; using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState; class PoseEstimation : public RosServiceNode { public: PoseEstimation(const std::string &name, const NodeConfig &conf, const RosNodeParams ¶ms) : RosServiceNode(name, conf, params) { RCLCPP_INFO_STREAM(logger(), "Start node."); m_params_client = std::make_shared( node_.lock(), "/pose_estimation"); while (!m_params_client->wait_for_service(1s)) { if (!rclcpp::ok()) { RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting."); break; } RCLCPP_WARN(logger(), "service not available, waiting again..."); } // Get model name paramter from BT ports getInput("ObjectName", m_model_name); } bool setRequest(Request::SharedPtr &request) override { getInput("ReqKind", m_req_type); request->set__transition(transition_event(m_req_type)); return true; } NodeStatus onResponseReceived(const Response::SharedPtr &response) override { if (!response->success) { return NodeStatus::FAILURE; } return NodeStatus::SUCCESS; } static PortsList providedPorts() { return providedBasicPorts({ InputPort("ReqKind"), InputPort("ObjectName"), InputPort("ObjectPath"), }); } private: uint8_t m_transition_id{}; std::string m_model_name{}; std::string m_model_type{}; std::string m_req_type{}; std::shared_ptr m_params_client; std::vector m_params; rcl_interfaces::msg::Parameter m_param; lifecycle_msgs::msg::Transition transition_event(const std::string &req_type) { lifecycle_msgs::msg::Transition translation{}; // ParamSetter param_setter(m_params_client); if (req_type == "configure") { set_mesh_param(); // auto str_mesh_param = std::make_shared("model_name", m_model_name); // param_setter.setStrategy(str_mesh_param); // param_setter.setParam() m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE; } else if (req_type == "calibrate") { set_str_param(); m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE; } else if (req_type == "activate") { m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE; } else if (req_type == "deactivate") { m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE; } else if (req_type == "cleanup") { m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP; } // calibrate translation.set__id(m_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_str_param() { RCLCPP_INFO_STREAM(logger(), "Set string parameter: <" + m_model_name + ">"); std::vector params{ rclcpp::Parameter("model_name", m_model_name)}; m_params_client->set_parameters(params); } void set_mesh_param() { auto t_package_name = ament_index_cpp::get_package_share_directory("rbs_perception"); m_model_type = build_model_path(m_model_name, t_package_name); RCLCPP_INFO_STREAM(logger(), m_model_type); std::vector params{ rclcpp::Parameter("mesh_path", m_model_name)}; m_params_client->set_parameters(params); } }; CreateRosNodePlugin(PoseEstimation, "PoseEstimation");