movebase_node.cpp
Go to the documentation of this file.
1 #include "movebase_node.h"
3 
4 // This function must be implemented in the .cpp file to create
5 // a plugin that can be loaded at run-time
7 {
8  factory.registerNodeType<MoveBaseAction>("MoveBase");
9 }
10 
12 {
13  Pose2D goal;
14  if ( !getInput<Pose2D>("goal", goal))
15  {
16  throw BT::RuntimeError("missing required input [goal]");
17  }
18 
19  printf("[ MoveBase: STARTED ]. goal: x=%.f y=%.1f theta=%.2f\n", goal.x, goal.y, goal.theta);
20 
21  _halt_requested.store(false);
22  int count = 0;
23 
24  // Pretend that "computing" takes 250 milliseconds.
25  // It is up to you to check periodicall _halt_requested and interrupt
26  // this tick() if it is true.
27  while (!_halt_requested && count++ < 25)
28  {
29  SleepMS(10);
30  }
31 
32  std::cout << "[ MoveBase: FINISHED ]" << std::endl;
34 }
35 
37 {
38  _halt_requested.store(true);
39 }
BT::NodeStatus tick() override
Method to be implemented by the user.
void SleepMS(int ms)
Definition: movebase_node.h:12
double x
Definition: movebase_node.h:9
BT_REGISTER_NODES(factory)
double theta
Definition: movebase_node.h:9
std::atomic_bool _halt_requested
Definition: movebase_node.h:68
static volatile int count
Definition: minitrace.cpp:55
NodeStatus
Definition: basic_types.h:35
double y
Definition: movebase_node.h:9
virtual void halt() override


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