118 lines
4 KiB
C++
118 lines
4 KiB
C++
#include <ament_index_cpp/get_package_share_directory.hpp>
|
|
#include <behavior_tree/BtService.hpp>
|
|
|
|
#include <behaviortree_cpp_v3/basic_types.h>
|
|
#include <rclcpp/logging.hpp>
|
|
#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 = std::make_shared<rclcpp::AsyncParametersClient>(_node, "/pose_estimation");
|
|
|
|
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{};
|
|
std::shared_ptr<rclcpp::AsyncParametersClient> _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 == "calibrate") {
|
|
set_str_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;
|
|
}
|
|
//calibrate
|
|
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_str_param() {
|
|
RCLCPP_INFO_STREAM(_node->get_logger(), "Set string parameter: <" + _model_name + ">");
|
|
|
|
std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("mesh_path", _model_name)};
|
|
_set_params_client->set_parameters(_parameters);
|
|
}
|
|
|
|
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);
|
|
|
|
std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("mesh_path", _model_path)};
|
|
_set_params_client->set_parameters(_parameters);
|
|
}
|
|
|
|
};
|
|
|
|
#include "behaviortree_cpp_v3/bt_factory.h"
|
|
|
|
BT_REGISTER_NODES(factory) {
|
|
factory.registerNodeType<PoseEstimation>("PoseEstimation");
|
|
}
|