Quadrotor

#include "ros/ros.h"

#include "std_msgs/Float64.h"

#include "mikrokopter/FcDebug.h"

#include "../../../control/pid_control.h"

class AltitudeNode

{

public:

  AltitudeNode();

  void main_loop() {ros::spin();}

  void height_cb(const mikrokopter::FcDebug::ConstPtr& fc);

  void goal_cb(const std_msgs::Float64::ConstPtr& goal);

private:

  PID_control pid;

  ros::NodeHandle n;

  ros::Publisher pub;

  ros::Subscriber height_sub;

  ros::Subscriber goal_sub;

};

AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0)

{

  height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug",

      100, &AltitudeNode::height_cb, this);

  goal_sub = n.subscribe<std_msgs::Float64>("/mikrokopter/height_goal",

      100, &AltitudeNode::goal_cb, this);

  pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);

}

void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc)

{

  std_msgs::Float64 msg;

  msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec());

  pub.publish(msg);

}

void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal)

{

  pid.change_goal(goal->data);

}

int main(int argc, char **argv) {

  ros::init(argc, argv, "altitude_node");

  AltitudeNode an = AltitudeNode();

  an.main_loop();

  return 0;

}