updating BT for queue work
This commit is contained in:
parent
a2f1f94ecd
commit
a86ce63d2a
4 changed files with 118 additions and 6 deletions
|
@ -36,6 +36,12 @@ list(APPEND plugin_libs is_in_pose)
|
|||
add_library(twist_command_with_condition SHARED plugins/twist_cmd.cpp)
|
||||
list(APPEND plugin_libs twist_command_with_condition)
|
||||
|
||||
add_library(takeon_task SHARED plugins/takeon_task.cpp)
|
||||
list(APPEND plugin_libs takeon_task)
|
||||
|
||||
add_library(task_completed SHARED plugins/task_completed.cpp)
|
||||
list(APPEND plugin_libs task_completed)
|
||||
|
||||
foreach(bt_plugin ${plugin_libs})
|
||||
ament_target_dependencies(${bt_plugin} ${dependencies})
|
||||
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
|
||||
|
|
54
rbs_mill_assist/bt/plugins/takeon_task.cpp
Normal file
54
rbs_mill_assist/bt/plugins/takeon_task.cpp
Normal file
|
@ -0,0 +1,54 @@
|
|||
#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");
|
40
rbs_mill_assist/bt/plugins/task_completed.cpp
Normal file
40
rbs_mill_assist/bt/plugins/task_completed.cpp
Normal file
|
@ -0,0 +1,40 @@
|
|||
#include "behaviortree_ros2/bt_service_node.hpp"
|
||||
#include "behaviortree_ros2/plugins.hpp"
|
||||
#include "rbs_utils_interfaces/srv/task_from_queue.hpp"
|
||||
|
||||
using TaskFromQueue = rbs_utils_interfaces::srv::TaskFromQueue;
|
||||
using namespace BT;
|
||||
|
||||
class TaskCompleted : public RosServiceNode<TaskFromQueue> {
|
||||
public:
|
||||
TaskCompleted(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({});
|
||||
}
|
||||
|
||||
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 = "completed";
|
||||
return true;
|
||||
}
|
||||
|
||||
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
|
||||
if (!response->ok) {
|
||||
RCLCPP_WARN(this->node_.lock()->get_logger(),
|
||||
"[%s] Not completed", name().c_str());
|
||||
return NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->node_.lock()->get_logger(),
|
||||
"[%s] Task completed", name().c_str());
|
||||
return NodeStatus::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
CreateRosNodePlugin(TaskCompleted, "TaskCompleted");
|
|
@ -3,14 +3,21 @@
|
|||
<BehaviorTree ID="Main">
|
||||
|
||||
<Sequence>
|
||||
<!-- <Action ID="GetTask" name="" data="{task}" num_cycles="{num_cycles}"/> -->
|
||||
<!-- <Action ID="AddTask" tasks_csv="..." service_name="queue/add_tasks"/> -->
|
||||
|
||||
<Fallback>
|
||||
<Repeat num_cycles="1">
|
||||
<Sequence>
|
||||
<!-- Preparing -->
|
||||
|
||||
<Action ID="GetSlotGraspPoses" from_pose="slot_1"
|
||||
<Action ID="TakeonTask" from_pose="{from_pose}" service_name="queue/takeon_task"/>
|
||||
<!-- <ReactiveFallback>
|
||||
<Action ID="TakeonTask" service_name="queue/takeon_task" from_pose="{from_pose}"/>
|
||||
<Delay delay_msec=1000>
|
||||
</Delay>
|
||||
</ReactiveFallback> -->
|
||||
|
||||
<Action ID="GetSlotGraspPoses" from_pose="{from_pose}"
|
||||
relative_to="base_link"
|
||||
pregrasp="{pregrasp}"
|
||||
grasp="{grasp}"
|
||||
|
@ -73,6 +80,8 @@
|
|||
postgrasp="{postgrasp}"
|
||||
/>
|
||||
|
||||
<Action ID="TaskCompleted" service_name="queue/task_completed"/>
|
||||
|
||||
<!-- Ending -->
|
||||
</Sequence>
|
||||
</Repeat>
|
||||
|
@ -103,10 +112,13 @@
|
|||
|
||||
<Action ID="NotifyOperator" editable="true" />
|
||||
|
||||
<Action ID="GetTask" editable="true">
|
||||
<input_port name="name"/>
|
||||
<input_port name="data"/>
|
||||
<input_port name="num_cycles"/>
|
||||
<Action ID="TakeonTask" editable="true">
|
||||
<input_port name="from_pose"/>
|
||||
<input_port name="service_name"/>
|
||||
</Action>
|
||||
|
||||
<Action ID="TaskCompleted" editable="true">
|
||||
<input_port name="service_name"/>
|
||||
</Action>
|
||||
|
||||
<Action ID="SendTask" editable="true" />
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue