49 lines
1.9 KiB
C++
49 lines
1.9 KiB
C++
|
|
#include "behaviortree_cpp/action_node.h"
|
|
|
|
namespace mg {
|
|
|
|
class CanChooser : public BT::SyncActionNode {
|
|
struct cans {
|
|
double x;
|
|
double y;
|
|
int orientation;
|
|
};
|
|
|
|
public:
|
|
CanChooser(const std::string& name, const BT::NodeConfig config) : BT::SyncActionNode(name, config) { }
|
|
|
|
static BT::PortsList providedPorts() {
|
|
// This action has a single input port called "message"
|
|
return { BT::InputPort<std::string>("canlist"),
|
|
BT::OutputPort<double>("x_loc"),
|
|
BT::OutputPort<double>("y_loc"),
|
|
BT::OutputPort<int>("orient") };
|
|
}
|
|
|
|
// You must override the virtual function tick()
|
|
BT::NodeStatus tick() override {
|
|
constexpr std::array<cans, 10> c{ { { 0.825, 1.425, 3 },
|
|
{ 0.35, 0.4, 2 },
|
|
{ 0.35, 1.325, 2 },
|
|
{ 0.775, 0.55, 1 },
|
|
{ 2.225, 0.55, 1 },
|
|
{ 2.65, 0.4, 0 },
|
|
{ 2.65, 0.4, 0 },
|
|
{ 2.175, 1.425, 3 },
|
|
{ 1.1, 0.65, 3 },
|
|
{ 1.9, 0.65, 3 } } };
|
|
|
|
int idx = 0;
|
|
// Todo iterate through all cans and use a passthrough function that returns current can state
|
|
std::istringstream ss(getInput<std::string>("canlist").value());
|
|
ss >> idx;
|
|
|
|
setOutput<double>("x_loc", c[idx].x);
|
|
setOutput<double>("y_loc", c[idx].y);
|
|
setOutput<int>("orient", c[idx].orientation);
|
|
|
|
return BT::NodeStatus::SUCCESS;
|
|
}
|
|
};
|
|
} |