Statistics
| Branch: | Revision:

root / mikrokopter / mikrokopter / src / node_control.cpp @ 98711613

History | View | Annotate | Download (1.34 KB)

1
#include "ros/ros.h"
2
#include "std_msgs/Float64.h"
3
#include "mikrokopter/Velocity2D.h"
4
#include "nav_lib.h"
5

    
6
class NodeControl
7
{
8
public:
9
  NodeControl();
10
  void main_loop() {control.main_loop();}
11
  void cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n);
12
  void cb_yaw(const std_msgs::Float64::ConstPtr& n);
13
  void cb_thrust(const std_msgs::Float64::ConstPtr& n);
14

    
15
private:
16
  MikrokopterControl control;
17
  ros::NodeHandle n;
18
  ros::Subscriber sub_velocity;
19
  ros::Subscriber sub_yaw;
20
  ros::Subscriber sub_thrust;
21
};
22

    
23
NodeControl::NodeControl()
24
{
25
  sub_velocity = n.subscribe<mikrokopter::Velocity2D>("/mikrokopter/velocity",
26
      100, &NodeControl::cb_velocity, this);
27
  sub_yaw = n.subscribe<std_msgs::Float64>("/mikrokopter/yaw", 100,
28
      &NodeControl::cb_yaw, this);
29
  sub_thrust= n.subscribe<std_msgs::Float64>("/mikrokopter/thrust", 100,
30
      &NodeControl::cb_thrust, this);
31
}
32

    
33
void NodeControl::cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n)
34
{
35
  control.velocity_control(n->forward, n->lateral);
36
}
37
void NodeControl::cb_yaw(const std_msgs::Float64::ConstPtr& n)
38
{
39
  control.set_yaw(n->data);
40
}
41
void NodeControl::cb_thrust(const std_msgs::Float64::ConstPtr& n)
42
{
43
  control.set_thrust(n->data);
44
}
45

    
46
int main(int argc, char **argv) {
47
  ros::init(argc, argv, "node_control");
48
  NodeControl control = NodeControl();
49
  control.main_loop();
50
  return 0;
51
}