00001 #include "movebase_node.h" 00002 #include "behaviortree_cpp/bt_factory.h" 00003 00004 // This function must be implemented in the .cpp file to create 00005 // a plugin that can be loaded at run-time 00006 BT_REGISTER_NODES(factory) 00007 { 00008 factory.registerNodeType<MoveBaseAction>("MoveBase"); 00009 } 00010 00011 BT::NodeStatus MoveBaseAction::tick() 00012 { 00013 Pose2D goal; 00014 if ( !getInput<Pose2D>("goal", goal)) 00015 { 00016 throw BT::RuntimeError("missing required input [goal]"); 00017 } 00018 00019 printf("[ MoveBase: STARTED ]. goal: x=%.f y=%.1f theta=%.2f\n", goal.x, goal.y, goal.theta); 00020 00021 _halt_requested.store(false); 00022 int count = 0; 00023 00024 // Pretend that "computing" takes 250 milliseconds. 00025 // It is up to you to check periodicall _halt_requested and interrupt 00026 // this tick() if it is true. 00027 while (!_halt_requested && count++ < 25) 00028 { 00029 SleepMS(10); 00030 } 00031 00032 std::cout << "[ MoveBase: FINISHED ]" << std::endl; 00033 return _halt_requested ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS; 00034 } 00035 00036 void MoveBaseAction::halt() 00037 { 00038 _halt_requested.store(true); 00039 }