runtime/rbs_bt_executor/src/PoseEstimation.cpp

125 lines
4.2 KiB
C++

#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 <behaviortree_ros2/plugins.hpp>
#include <memory>
using namespace BT;
using namespace std::chrono_literals;
using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
class PoseEstimation : public RosServiceNode<PoseEstimationSrv> {
public:
PoseEstimation(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<PoseEstimationSrv>(name, conf, params) {
RCLCPP_INFO_STREAM(logger(), "Start node.");
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
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<std::string>("ReqKind"),
InputPort<std::string>("ObjectName"),
InputPort<std::string>("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<rclcpp::AsyncParametersClient> m_params_client;
std::vector<rcl_interfaces::msg::Parameter> 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<SetParamShareDirectoryStrategy>("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<rclcpp::Parameter> 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<rclcpp::Parameter> params{
rclcpp::Parameter("mesh_path", m_model_name)};
m_params_client->set_parameters(params);
}
};
CreateRosNodePlugin(PoseEstimation, "PoseEstimation");