00001 #ifndef MOVEBASE_BT_NODES_H 00002 #define MOVEBASE_BT_NODES_H 00003 00004 #include "behaviortree_cpp/behavior_tree.h" 00005 00006 // Custom type 00007 struct Pose2D 00008 { 00009 double x, y, theta; 00010 }; 00011 00012 inline void SleepMS(int ms) 00013 { 00014 std::this_thread::sleep_for(std::chrono::milliseconds(ms)); 00015 } 00016 00017 namespace BT 00018 { 00019 // This template specialization is needed only if you want 00020 // to AUTOMATICALLY convert a NodeParameter into a Pose2D 00021 // In other words, implement it if you want to be able to do: 00022 // 00023 // TreeNode::getInput<Pose2D>(key, ...) 00024 // 00025 template <> inline 00026 Pose2D convertFromString(StringView key) 00027 { 00028 // three real numbers separated by semicolons 00029 auto parts = BT::splitString(key, ';'); 00030 if (parts.size() != 3) 00031 { 00032 throw BT::RuntimeError("invalid input)"); 00033 } 00034 else 00035 { 00036 Pose2D output; 00037 output.x = convertFromString<double>(parts[0]); 00038 output.y = convertFromString<double>(parts[1]); 00039 output.theta = convertFromString<double>(parts[2]); 00040 return output; 00041 } 00042 } 00043 } // end namespace BT 00044 00045 // This is an asynchronous operation that will run in a separate thread. 00046 // It requires the input port "goal". 00047 00048 class MoveBaseAction : public BT::AsyncActionNode 00049 { 00050 public: 00051 // Any TreeNode with ports must have a constructor with this signature 00052 MoveBaseAction(const std::string& name, const BT::NodeConfiguration& config) 00053 : AsyncActionNode(name, config) 00054 { 00055 } 00056 00057 // It is mandatory to define this static method. 00058 static BT::PortsList providedPorts() 00059 { 00060 return{ BT::InputPort<Pose2D>("goal") }; 00061 } 00062 00063 BT::NodeStatus tick() override; 00064 00065 virtual void halt() override; 00066 00067 private: 00068 std::atomic_bool _halt_requested; 00069 }; 00070 00071 #endif // MOVEBASE_BT_NODES_H