movebase_node.h
Go to the documentation of this file.
1 #ifndef MOVEBASE_BT_NODES_H
2 #define MOVEBASE_BT_NODES_H
3 
5 
6 // Custom type
7 struct Pose2D
8 {
9  double x, y, theta;
10 };
11 
12 inline void SleepMS(int ms)
13 {
14  std::this_thread::sleep_for(std::chrono::milliseconds(ms));
15 }
16 
17 namespace BT
18 {
19 // This template specialization is needed only if you want
20 // to AUTOMATICALLY convert a NodeParameter into a Pose2D
21 // In other words, implement it if you want to be able to do:
22 //
23 // TreeNode::getInput<Pose2D>(key, ...)
24 //
25 template <> inline
27 {
28  // three real numbers separated by semicolons
29  auto parts = BT::splitString(key, ';');
30  if (parts.size() != 3)
31  {
32  throw BT::RuntimeError("invalid input)");
33  }
34  else
35  {
36  Pose2D output;
37  output.x = convertFromString<double>(parts[0]);
38  output.y = convertFromString<double>(parts[1]);
39  output.theta = convertFromString<double>(parts[2]);
40  return output;
41  }
42 }
43 } // end namespace BT
44 
45 // This is an asynchronous operation that will run in a separate thread.
46 // It requires the input port "goal".
47 
49 {
50  public:
51  // Any TreeNode with ports must have a constructor with this signature
52  MoveBaseAction(const std::string& name, const BT::NodeConfiguration& config)
53  : AsyncActionNode(name, config)
54  {
55  }
56 
57  // It is mandatory to define this static method.
59  {
60  return{ BT::InputPort<Pose2D>("goal") };
61  }
62 
63  BT::NodeStatus tick() override;
64 
65  virtual void halt() override;
66 
67  private:
68  std::atomic_bool _halt_requested;
69 };
70 
71 #endif // MOVEBASE_BT_NODES_H
The AsyncActionNode uses a different thread where the action will be executed.
Definition: action_node.h:105
void SleepMS(int ms)
Definition: movebase_node.h:12
double x
Definition: movebase_node.h:9
std::vector< StringView > splitString(const StringView &strToSplit, char delimeter)
nonstd::string_view StringView
Definition: basic_types.h:50
double theta
Definition: movebase_node.h:9
std::atomic_bool _halt_requested
Definition: movebase_node.h:68
MoveBaseAction(const std::string &name, const BT::NodeConfiguration &config)
Definition: movebase_node.h:52
std::unordered_map< std::string, PortInfo > PortsList
Definition: basic_types.h:316
NodeStatus
Definition: basic_types.h:35
double y
Definition: movebase_node.h:9
Pose2D convertFromString(StringView key)
Definition: movebase_node.h:26
static BT::PortsList providedPorts()
Definition: movebase_node.h:58
double convertFromString< double >(StringView str)


behaviortree_cpp
Author(s): Michele Colledanchise, Davide Faconti
autogenerated on Sat Jun 8 2019 18:04:05