90 lines
3 KiB
C++
90 lines
3 KiB
C++
#include "behaviortree_ros2/bt_service_node.hpp"
|
|
#include "lifecycle_msgs/msg/transition.hpp"
|
|
#include "lifecycle_msgs/srv/change_state.hpp"
|
|
#include "rcl_interfaces/msg/parameter.hpp"
|
|
#include "rcl_interfaces/srv/set_parameters.hpp"
|
|
#include <behaviortree_cpp/tree_node.h>
|
|
#include <behaviortree_ros2/plugins.hpp>
|
|
#include <behaviortree_ros2/ros_node_params.hpp>
|
|
#include <memory>
|
|
#include <rclcpp/parameter_client.hpp>
|
|
|
|
using namespace BT;
|
|
using namespace std::chrono_literals;
|
|
using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;
|
|
|
|
class ObjectDetection : public RosServiceNode<ObjDetectionSrv> {
|
|
public:
|
|
ObjectDetection(const std::string &name, const NodeConfig &conf,
|
|
const RosNodeParams ¶ms)
|
|
: RosServiceNode<ObjDetectionSrv>(name, conf, params) {
|
|
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
|
|
node_.lock(), "/object_detection");
|
|
while (!m_params_client->wait_for_service(1s)) {
|
|
if (!rclcpp::ok()) {
|
|
RCLCPP_ERROR(logger(),
|
|
"Interrupted while waiting for the service. Exiting.");
|
|
break;
|
|
}
|
|
}
|
|
getInput("SettingPath", m_settings_path);
|
|
}
|
|
|
|
static PortsList providedPorts() {
|
|
return providedBasicPorts({
|
|
InputPort<std::string>("ReqKind"),
|
|
InputPort<std::string>("SettingPath"),
|
|
});
|
|
}
|
|
|
|
bool setRequest(Request::SharedPtr &request) override {
|
|
getInput("ReqKind", m_req_type);
|
|
auto transition = transition_event(m_req_type);
|
|
request->set__transition(transition);
|
|
return true;
|
|
}
|
|
|
|
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
|
|
if (!response->success) {
|
|
return NodeStatus::FAILURE;
|
|
}
|
|
return NodeStatus::SUCCESS;
|
|
}
|
|
|
|
private:
|
|
uint8_t transition_id_{};
|
|
std::string m_settings_path{};
|
|
// std::string _model_path{};
|
|
std::string m_req_type{};
|
|
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
|
|
std::vector<rcl_interfaces::msg::Parameter> _params;
|
|
rcl_interfaces::msg::Parameter m_param;
|
|
|
|
lifecycle_msgs::msg::Transition
|
|
transition_event(const std::string &req_type) {
|
|
lifecycle_msgs::msg::Transition translation{};
|
|
if (req_type == "configure") {
|
|
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;
|
|
}
|
|
translation.set__id(transition_id_);
|
|
return translation;
|
|
}
|
|
|
|
void set_str_param() {
|
|
RCLCPP_INFO_STREAM(logger(),
|
|
"Set string parameter: <" + m_settings_path + ">");
|
|
|
|
std::vector<rclcpp::Parameter> t_parameters{
|
|
rclcpp::Parameter("setting_path", m_settings_path)};
|
|
m_params_client->set_parameters(t_parameters);
|
|
}
|
|
};
|
|
|
|
CreateRosNodePlugin(ObjectDetection, "ObjectDetection");
|