root / quad2 / altitude_node / src / altitude_node.cpp @ 734b51b9
History | View | Annotate | Download (2.75 KB)
1 | 737bd480 | Tom Mullins | #include "ros/ros.h" |
---|---|---|---|
2 | #include "mikrokopter/FcDebug.h" |
||
3 | 7bc485ed | Tom Mullins | #include "std_msgs/Float64.h" |
4 | #include "std_msgs/Bool.h" |
||
5 | 737bd480 | Tom Mullins | #include "../../../control/pid_control.h" |
6 | |||
7 | class AltitudeNode |
||
8 | { |
||
9 | public:
|
||
10 | AltitudeNode(); |
||
11 | void main_loop() {ros::spin();}
|
||
12 | void height_cb(const mikrokopter::FcDebug::ConstPtr& fc); |
||
13 | void goal_cb(const std_msgs::Float64::ConstPtr& goal); |
||
14 | 734b51b9 | Tom Mullins | void p_cb(const std_msgs::Float64::ConstPtr& p); |
15 | void i_cb(const std_msgs::Float64::ConstPtr& i); |
||
16 | void d_cb(const std_msgs::Float64::ConstPtr& d); |
||
17 | 7bc485ed | Tom Mullins | void enable_cb(const std_msgs::Bool::ConstPtr& goal); |
18 | 737bd480 | Tom Mullins | |
19 | private:
|
||
20 | 7bc485ed | Tom Mullins | bool enabled;
|
21 | 737bd480 | Tom Mullins | PID_control pid; |
22 | ros::NodeHandle n; |
||
23 | ros::Publisher pub; |
||
24 | ros::Subscriber height_sub; |
||
25 | ros::Subscriber goal_sub; |
||
26 | 734b51b9 | Tom Mullins | ros::Subscriber p_sub; |
27 | ros::Subscriber i_sub; |
||
28 | ros::Subscriber d_sub; |
||
29 | 7bc485ed | Tom Mullins | ros::Subscriber enable_sub; |
30 | 737bd480 | Tom Mullins | }; |
31 | |||
32 | 734b51b9 | Tom Mullins | AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false) |
33 | 737bd480 | Tom Mullins | { |
34 | 734b51b9 | Tom Mullins | height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100, |
35 | &AltitudeNode::height_cb, this);
|
||
36 | goal_sub = n.subscribe<std_msgs::Float64>("/altitude_node/height_goal", 100, |
||
37 | &AltitudeNode::goal_cb, this);
|
||
38 | p_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/p", 100, |
||
39 | &AltitudeNode::p_cb, this);
|
||
40 | i_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/i", 100, |
||
41 | &AltitudeNode::i_cb, this);
|
||
42 | d_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/d", 100, |
||
43 | &AltitudeNode::d_cb, this);
|
||
44 | enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable", 100, |
||
45 | &AltitudeNode::enable_cb, this);
|
||
46 | 737bd480 | Tom Mullins | pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100); |
47 | 122a57d8 | Tom Mullins | |
48 | 734b51b9 | Tom Mullins | double default_goal;
|
49 | ros::param::param<double>("~default_goal", default_goal, 0); |
||
50 | 122a57d8 | Tom Mullins | pid.change_goal(default_goal); |
51 | 737bd480 | Tom Mullins | } |
52 | |||
53 | void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc) |
||
54 | { |
||
55 | 7bc485ed | Tom Mullins | if (enabled)
|
56 | { |
||
57 | std_msgs::Float64 msg; |
||
58 | msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec()); |
||
59 | pub.publish(msg); |
||
60 | } |
||
61 | 737bd480 | Tom Mullins | } |
62 | |||
63 | void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal) |
||
64 | { |
||
65 | pid.change_goal(goal->data); |
||
66 | 734b51b9 | Tom Mullins | ROS_INFO("Goal: %f", goal->data);
|
67 | } |
||
68 | |||
69 | void AltitudeNode::p_cb(const std_msgs::Float64::ConstPtr& p) |
||
70 | { |
||
71 | pid.change_p(p->data); |
||
72 | ROS_INFO("K_p: %f", p->data);
|
||
73 | } |
||
74 | |||
75 | void AltitudeNode::i_cb(const std_msgs::Float64::ConstPtr& i) |
||
76 | { |
||
77 | pid.change_i(i->data); |
||
78 | ROS_INFO("K_i: %f", i->data);
|
||
79 | } |
||
80 | |||
81 | void AltitudeNode::d_cb(const std_msgs::Float64::ConstPtr& d) |
||
82 | { |
||
83 | pid.change_d(d->data); |
||
84 | ROS_INFO("K_d: %f", d->data);
|
||
85 | 737bd480 | Tom Mullins | } |
86 | |||
87 | 7bc485ed | Tom Mullins | void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg) |
88 | { |
||
89 | enabled = msg->data; |
||
90 | 98e35b23 | Alex Zirbel | if (enabled)
|
91 | { |
||
92 | ROS_INFO("Altitude Hold: Enabled.");
|
||
93 | } |
||
94 | else
|
||
95 | { |
||
96 | ROS_INFO("Altitude Hold: Disabled.");
|
||
97 | } |
||
98 | 7bc485ed | Tom Mullins | } |
99 | |||
100 | 737bd480 | Tom Mullins | int main(int argc, char **argv) { |
101 | ros::init(argc, argv, "altitude_node");
|
||
102 | AltitudeNode an = AltitudeNode(); |
||
103 | an.main_loop(); |
||
104 | return 0; |
||
105 | } |