00001 #include "behaviortree_cpp/bt_factory.h" 00002 #include "behaviortree_cpp/loggers/bt_cout_logger.h" 00003 00009 // This is my custom type. We won't know how to read this from a string, 00010 // unless we implement convertFromString<Point3D>() 00011 struct Point3D { double x,y,z; }; 00012 00013 // We want to create an ActionNode that calls the method MyLegacyMoveTo::go 00014 class MyLegacyMoveTo 00015 { 00016 public: 00017 bool go(Point3D goal) 00018 { 00019 printf("Going to: %f %f %f\n", goal.x, goal.y, goal.z); 00020 return true; // true means success in my legacy code 00021 } 00022 }; 00023 00024 // Similarly to the previous tutorials, we need to implement this parsing method, 00025 // providing a specialization of BT::convertFromString 00026 namespace BT 00027 { 00028 template <> Point3D convertFromString(StringView key) 00029 { 00030 // three real numbers separated by semicolons 00031 auto parts = BT::splitString(key, ';'); 00032 if (parts.size() != 3) 00033 { 00034 throw RuntimeError("invalid input)"); 00035 } 00036 else 00037 { 00038 Point3D output; 00039 output.x = convertFromString<double>(parts[0]); 00040 output.y = convertFromString<double>(parts[1]); 00041 output.z = convertFromString<double>(parts[2]); 00042 return output; 00043 } 00044 } 00045 } // end anmespace BT 00046 00047 00048 // clang-format off 00049 static const char* xml_text = R"( 00050 00051 <root> 00052 <BehaviorTree> 00053 <MoveTo goal="-1;3;0.5" /> 00054 </BehaviorTree> 00055 </root> 00056 )"; 00057 00058 // clang-format on 00059 00060 int main() 00061 { 00062 using namespace BT; 00063 00064 MyLegacyMoveTo move_to; 00065 00066 // Here we use a lambda that captures the reference of move_to 00067 auto MoveToWrapperWithLambda = [&move_to](TreeNode& parent_node) -> NodeStatus 00068 { 00069 Point3D goal; 00070 // thanks to paren_node, you can access easily the inpyt and output ports. 00071 parent_node.getInput("goal", goal); 00072 00073 bool res = move_to.go( goal ); 00074 // convert bool to NodeStatus 00075 return res ? NodeStatus::SUCCESS : NodeStatus::FAILURE; 00076 }; 00077 00078 BehaviorTreeFactory factory; 00079 00080 // Register the lambda with BehaviorTreeFactory::registerSimpleAction 00081 00082 PortsList ports = { BT::InputPort<Point3D>("goal") }; 00083 factory.registerSimpleAction("MoveTo", MoveToWrapperWithLambda, ports ); 00084 00085 auto tree = factory.createTreeFromText(xml_text); 00086 00087 tree.root_node->executeTick(); 00088 00089 return 0; 00090 } 00091 00092 /* Expected output: 00093 00094 Going to: -1.000000 3.000000 0.500000 00095 00096 */