Statistics
| Branch: | Revision:

root / quad2 / altitude_node / src / altitude_node.cpp @ 734b51b9

History | View | Annotate | Download (2.75 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
  PID_control pid;
22
  ros::NodeHandle n;
23
  ros::Publisher pub;
24
  ros::Subscriber height_sub;
25
  ros::Subscriber goal_sub;
26
  ros::Subscriber p_sub;
27
  ros::Subscriber i_sub;
28
  ros::Subscriber d_sub;
29
  ros::Subscriber enable_sub;
30
};
31

    
32
AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false)
33
{
34
  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
  pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);
47
  
48
  double default_goal;
49
  ros::param::param<double>("~default_goal", default_goal, 0);
50
  pid.change_goal(default_goal);
51
}
52

    
53
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc)
54
{
55
  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
}
62

    
63
void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal)
64
{
65
  pid.change_goal(goal->data);
66
  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
}
86

    
87
void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg)
88
{
89
  enabled = msg->data;
90
  if (enabled)
91
  {
92
    ROS_INFO("Altitude Hold: Enabled.");
93
  }
94
  else
95
  {
96
    ROS_INFO("Altitude Hold: Disabled.");
97
  }
98
}
99

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