root / quad2 / altitude_node / src / altitude_node.cpp @ 122a57d8
History | View | Annotate | Download (1.82 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 | 7bc485ed | Tom Mullins | void enable_cb(const std_msgs::Bool::ConstPtr& goal); |
15 | 737bd480 | Tom Mullins | |
16 | private:
|
||
17 | 7bc485ed | Tom Mullins | bool enabled;
|
18 | 737bd480 | Tom Mullins | PID_control pid; |
19 | ros::NodeHandle n; |
||
20 | ros::Publisher pub; |
||
21 | ros::Subscriber height_sub; |
||
22 | ros::Subscriber goal_sub; |
||
23 | 7bc485ed | Tom Mullins | ros::Subscriber enable_sub; |
24 | 737bd480 | Tom Mullins | }; |
25 | |||
26 | 7bc485ed | Tom Mullins | AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0), enabled(false) |
27 | 737bd480 | Tom Mullins | { |
28 | height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug",
|
||
29 | 100, &AltitudeNode::height_cb, this); |
||
30 | goal_sub = n.subscribe<std_msgs::Float64>("/mikrokopter/height_goal",
|
||
31 | 100, &AltitudeNode::goal_cb, this); |
||
32 | 7bc485ed | Tom Mullins | enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable",
|
33 | 100, &AltitudeNode::enable_cb, this); |
||
34 | 737bd480 | Tom Mullins | pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100); |
35 | 122a57d8 | Tom Mullins | |
36 | float default_goal;
|
||
37 | ros::param::param<float>("~default_goal", default_goal, 0); |
||
38 | pid.change_goal(default_goal); |
||
39 | 737bd480 | Tom Mullins | } |
40 | |||
41 | void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc) |
||
42 | { |
||
43 | 7bc485ed | Tom Mullins | if (enabled)
|
44 | { |
||
45 | std_msgs::Float64 msg; |
||
46 | msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec()); |
||
47 | pub.publish(msg); |
||
48 | } |
||
49 | 737bd480 | Tom Mullins | } |
50 | |||
51 | void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal) |
||
52 | { |
||
53 | pid.change_goal(goal->data); |
||
54 | } |
||
55 | |||
56 | 7bc485ed | Tom Mullins | void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg) |
57 | { |
||
58 | enabled = msg->data; |
||
59 | 98e35b23 | Alex Zirbel | if (enabled)
|
60 | { |
||
61 | ROS_INFO("Altitude Hold: Enabled.");
|
||
62 | } |
||
63 | else
|
||
64 | { |
||
65 | ROS_INFO("Altitude Hold: Disabled.");
|
||
66 | } |
||
67 | 7bc485ed | Tom Mullins | } |
68 | |||
69 | 737bd480 | Tom Mullins | int main(int argc, char **argv) { |
70 | ros::init(argc, argv, "altitude_node");
|
||
71 | AltitudeNode an = AltitudeNode(); |
||
72 | an.main_loop(); |
||
73 | return 0; |
||
74 | } |