54 lines
1.7 KiB
C++
54 lines
1.7 KiB
C++
#include "nlohmann/json.hpp"
|
|
#include "behaviortree_ros2/bt_service_node.hpp"
|
|
#include "behaviortree_ros2/plugins.hpp"
|
|
#include "rbs_utils_interfaces/srv/task_from_queue.hpp"
|
|
#include <string>
|
|
#include <sstream>
|
|
#include <iostream>
|
|
|
|
using TaskFromQueue = rbs_utils_interfaces::srv::TaskFromQueue;
|
|
using namespace BT;
|
|
|
|
class TakeonTask : public RosServiceNode<TaskFromQueue> {
|
|
public:
|
|
TakeonTask(const std::string &name, const NodeConfig &conf, const RosNodeParams ¶ms)
|
|
: RosServiceNode<TaskFromQueue>(name, conf, params) {
|
|
|
|
RCLCPP_INFO(this->logger(), "Starting TakeonTask");
|
|
}
|
|
|
|
static PortsList providedPorts() {
|
|
return providedBasicPorts(
|
|
{OutputPort<std::string>("from_pose")});
|
|
}
|
|
|
|
bool setRequest(Request::SharedPtr &request) override {
|
|
RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
|
|
name().c_str(), this->service_name_.c_str());
|
|
request->mode = "takeon";
|
|
return true;
|
|
}
|
|
|
|
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
|
|
if (!response->ok) {
|
|
RCLCPP_WARN(this->node_.lock()->get_logger(),
|
|
"[%s] No tasks in queue", name().c_str());
|
|
return NodeStatus::FAILURE;
|
|
}
|
|
auto json_task = nlohmann::json::parse(response->task);
|
|
int TaskId = json_task["TaskId"];
|
|
int slot = json_task["obj_id"];
|
|
|
|
std::ostringstream oss;
|
|
oss << "slot_" << slot;
|
|
const char *x = oss.str().c_str();
|
|
|
|
setOutput("from_pose", x);
|
|
|
|
RCLCPP_INFO(this->node_.lock()->get_logger(),
|
|
"[%s] Task takeon: Id = %d, slot: %d", name().c_str(), TaskId, slot);
|
|
return NodeStatus::SUCCESS;
|
|
}
|
|
};
|
|
|
|
CreateRosNodePlugin(TakeonTask, "TakeonTask");
|