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 | } |