root / quad2 / mikrokopter / src / node_control.cpp @ 98e35b23
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 |
} |