Statistics
| Branch: | Revision:

root / quad2 / altitude_node / src / altitude_node.cpp @ 98e35b23

History | View | Annotate | Download (1.71 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 enable_cb(const std_msgs::Bool::ConstPtr& goal);
15

    
16
private:
17
  bool enabled;
18
  PID_control pid;
19
  ros::NodeHandle n;
20
  ros::Publisher pub;
21
  ros::Subscriber height_sub;
22
  ros::Subscriber goal_sub;
23
  ros::Subscriber enable_sub;
24
};
25

    
26
AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0), enabled(false)
27
{
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
  enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable",
33
      100, &AltitudeNode::enable_cb, this);
34
  pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);
35
}
36

    
37
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc)
38
{
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
  }
45
}
46

    
47
void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal)
48
{
49
  pid.change_goal(goal->data);
50
}
51

    
52
void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg)
53
{
54
  enabled = msg->data;
55
  if (enabled)
56
  {
57
    ROS_INFO("Altitude Hold: Enabled.");
58
  }
59
  else
60
  {
61
    ROS_INFO("Altitude Hold: Disabled.");
62
  }
63
}
64

    
65
int main(int argc, char **argv) {
66
  ros::init(argc, argv, "altitude_node");
67
  AltitudeNode an = AltitudeNode();
68
  an.main_loop();
69
  return 0;
70
}