47 lines
1.5 KiB
C++
47 lines
1.5 KiB
C++
#include "behaviortree_ros2/bt_service_node.hpp"
|
|
|
|
#include "behaviortree_ros2/plugins.hpp"
|
|
#include "std_srvs/srv/set_bool.hpp"
|
|
#include <behaviortree_cpp/basic_types.h>
|
|
#include <memory>
|
|
#include <rclcpp/logging.hpp>
|
|
#include <string>
|
|
|
|
using ToggleVacuumGripperService = std_srvs::srv::SetBool;
|
|
using namespace BT;
|
|
|
|
class ToggleVacuumGrippper : public RosServiceNode<ToggleVacuumGripperService> {
|
|
public:
|
|
ToggleVacuumGrippper(const std::string &name, const NodeConfig &conf,
|
|
const RosNodeParams ¶ms)
|
|
: RosServiceNode<ToggleVacuumGripperService>(name, conf, params) {
|
|
|
|
RCLCPP_INFO(this->logger(), "Starting ToggleVacuumGrippper");
|
|
}
|
|
|
|
static PortsList providedPorts() {
|
|
return providedBasicPorts({InputPort<bool>("enable")});
|
|
}
|
|
|
|
bool setRequest(Request::SharedPtr &request) override {
|
|
RCLCPP_INFO(this->logger(), "Starting send request");
|
|
if (!getInput("enable", request->data)) {
|
|
RCLCPP_ERROR(this->node_.lock()->get_logger(),
|
|
"Failed to get sending data from input port");
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
|
|
if (!response->success) {
|
|
RCLCPP_ERROR(this->logger(), "Response indicates failure.");
|
|
return NodeStatus::FAILURE;
|
|
}
|
|
|
|
return NodeStatus::SUCCESS;
|
|
}
|
|
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
|
|
};
|
|
|
|
CreateRosNodePlugin(ToggleVacuumGrippper, "ToggleVacuumGrippper");
|