root / quad2 / altitude_node / src / altitude_node.cpp @ 734b51b9
History | View | Annotate | Download (2.75 KB)
1 |
#include "ros/ros.h" |
---|---|
2 |
#include "mikrokopter/FcDebug.h" |
3 |
#include "std_msgs/Float64.h" |
4 |
#include "std_msgs/Bool.h" |
5 |
#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 |
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 |
void enable_cb(const std_msgs::Bool::ConstPtr& goal); |
18 |
|
19 |
private:
|
20 |
bool enabled;
|
21 |
PID_control pid; |
22 |
ros::NodeHandle n; |
23 |
ros::Publisher pub; |
24 |
ros::Subscriber height_sub; |
25 |
ros::Subscriber goal_sub; |
26 |
ros::Subscriber p_sub; |
27 |
ros::Subscriber i_sub; |
28 |
ros::Subscriber d_sub; |
29 |
ros::Subscriber enable_sub; |
30 |
}; |
31 |
|
32 |
AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false) |
33 |
{ |
34 |
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 |
pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100); |
47 |
|
48 |
double default_goal;
|
49 |
ros::param::param<double>("~default_goal", default_goal, 0); |
50 |
pid.change_goal(default_goal); |
51 |
} |
52 |
|
53 |
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc) |
54 |
{ |
55 |
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 |
} |
62 |
|
63 |
void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal) |
64 |
{ |
65 |
pid.change_goal(goal->data); |
66 |
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 |
} |
86 |
|
87 |
void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg) |
88 |
{ |
89 |
enabled = msg->data; |
90 |
if (enabled)
|
91 |
{ |
92 |
ROS_INFO("Altitude Hold: Enabled.");
|
93 |
} |
94 |
else
|
95 |
{ |
96 |
ROS_INFO("Altitude Hold: Disabled.");
|
97 |
} |
98 |
} |
99 |
|
100 |
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 |
} |