Revision 7bc485ed
ID | 7bc485ed1e7e508acaedf3dada02bec443ac9856 |
Added /altitude_node/enable topic to disable PID
Also, added node_control.cpp and Velocity2D.msg, which should have been
commited before but apparently weren't.
mikrokopter/altitude_node/src/altitude_node.cpp | ||
---|---|---|
1 | 1 |
#include "ros/ros.h" |
2 |
#include "std_msgs/Float64.h" |
|
3 | 2 |
#include "mikrokopter/FcDebug.h" |
3 |
#include "std_msgs/Float64.h" |
|
4 |
#include "std_msgs/Bool.h" |
|
4 | 5 |
#include "../../../control/pid_control.h" |
5 | 6 |
|
6 | 7 |
class AltitudeNode |
... | ... | |
10 | 11 |
void main_loop() {ros::spin();} |
11 | 12 |
void height_cb(const mikrokopter::FcDebug::ConstPtr& fc); |
12 | 13 |
void goal_cb(const std_msgs::Float64::ConstPtr& goal); |
14 |
void enable_cb(const std_msgs::Bool::ConstPtr& goal); |
|
13 | 15 |
|
14 | 16 |
private: |
17 |
bool enabled; |
|
15 | 18 |
PID_control pid; |
16 | 19 |
ros::NodeHandle n; |
17 | 20 |
ros::Publisher pub; |
18 | 21 |
ros::Subscriber height_sub; |
19 | 22 |
ros::Subscriber goal_sub; |
23 |
ros::Subscriber enable_sub; |
|
20 | 24 |
}; |
21 | 25 |
|
22 |
AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0) |
|
26 |
AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0), enabled(false)
|
|
23 | 27 |
{ |
24 | 28 |
height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug", |
25 | 29 |
100, &AltitudeNode::height_cb, this); |
26 | 30 |
goal_sub = n.subscribe<std_msgs::Float64>("/mikrokopter/height_goal", |
27 | 31 |
100, &AltitudeNode::goal_cb, this); |
32 |
enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable", |
|
33 |
100, &AltitudeNode::enable_cb, this); |
|
28 | 34 |
pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100); |
29 | 35 |
} |
30 | 36 |
|
31 | 37 |
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc) |
32 | 38 |
{ |
33 |
std_msgs::Float64 msg; |
|
34 |
msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec()); |
|
35 |
pub.publish(msg); |
|
39 |
if (enabled) |
|
40 |
{ |
|
41 |
std_msgs::Float64 msg; |
|
42 |
msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec()); |
|
43 |
pub.publish(msg); |
|
44 |
} |
|
36 | 45 |
} |
37 | 46 |
|
38 | 47 |
void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal) |
... | ... | |
40 | 49 |
pid.change_goal(goal->data); |
41 | 50 |
} |
42 | 51 |
|
52 |
void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg) |
|
53 |
{ |
|
54 |
enabled = msg->data; |
|
55 |
} |
|
56 |
|
|
43 | 57 |
int main(int argc, char **argv) { |
44 | 58 |
ros::init(argc, argv, "altitude_node"); |
45 | 59 |
AltitudeNode an = AltitudeNode(); |
Also available in: Unified diff