root / quad2 / altitude_node / src / altitude_node.cpp @ 02975ec6
History | View | Annotate | Download (2.9 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 |
float last_height;
|
22 |
PID_control pid; |
23 |
ros::NodeHandle n; |
24 |
ros::Publisher pub; |
25 |
ros::Subscriber height_sub; |
26 |
ros::Subscriber goal_sub; |
27 |
ros::Subscriber p_sub; |
28 |
ros::Subscriber i_sub; |
29 |
ros::Subscriber d_sub; |
30 |
ros::Subscriber enable_sub; |
31 |
}; |
32 |
|
33 |
AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false), last_height(0) |
34 |
{ |
35 |
height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100, |
36 |
&AltitudeNode::height_cb, this);
|
37 |
goal_sub = n.subscribe<std_msgs::Float64>("/altitude_node/height_goal", 100, |
38 |
&AltitudeNode::goal_cb, this);
|
39 |
p_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/p", 100, |
40 |
&AltitudeNode::p_cb, this);
|
41 |
i_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/i", 100, |
42 |
&AltitudeNode::i_cb, this);
|
43 |
d_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/d", 100, |
44 |
&AltitudeNode::d_cb, this);
|
45 |
enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable", 100, |
46 |
&AltitudeNode::enable_cb, this);
|
47 |
pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100); |
48 |
|
49 |
/*double default_goal;
|
50 |
ros::param::param<double>("~default_goal", default_goal, 0);
|
51 |
pid.change_goal(default_goal);*/
|
52 |
} |
53 |
|
54 |
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc) |
55 |
{ |
56 |
last_height = fc->heightValue; |
57 |
if (enabled)
|
58 |
{ |
59 |
std_msgs::Float64 msg; |
60 |
msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec()); |
61 |
pub.publish(msg); |
62 |
} |
63 |
} |
64 |
|
65 |
void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal) |
66 |
{ |
67 |
pid.change_goal(goal->data); |
68 |
ROS_INFO("Goal: %f", goal->data);
|
69 |
} |
70 |
|
71 |
void AltitudeNode::p_cb(const std_msgs::Float64::ConstPtr& p) |
72 |
{ |
73 |
pid.change_p(p->data); |
74 |
ROS_INFO("K_p: %f", p->data);
|
75 |
} |
76 |
|
77 |
void AltitudeNode::i_cb(const std_msgs::Float64::ConstPtr& i) |
78 |
{ |
79 |
pid.change_i(i->data); |
80 |
ROS_INFO("K_i: %f", i->data);
|
81 |
} |
82 |
|
83 |
void AltitudeNode::d_cb(const std_msgs::Float64::ConstPtr& d) |
84 |
{ |
85 |
pid.change_d(d->data); |
86 |
ROS_INFO("K_d: %f", d->data);
|
87 |
} |
88 |
|
89 |
void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg) |
90 |
{ |
91 |
enabled = msg->data; |
92 |
if (enabled)
|
93 |
{ |
94 |
pid.change_goal(last_height); |
95 |
ROS_INFO("Altitude Hold: Enabled.");
|
96 |
ROS_INFO("Altitude: %f", last_height);
|
97 |
} |
98 |
else
|
99 |
{ |
100 |
ROS_INFO("Altitude Hold: Disabled.");
|
101 |
} |
102 |
} |
103 |
|
104 |
int main(int argc, char **argv) { |
105 |
ros::init(argc, argv, "altitude_node");
|
106 |
AltitudeNode an = AltitudeNode(); |
107 |
an.main_loop(); |
108 |
return 0; |
109 |
} |