root / quad2 / mikrokopter / src / node_control.cpp @ 98e35b23
History | View | Annotate | Download (1.34 KB)
1 | 7bc485ed | Tom Mullins | #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 | } |