Quadrotor

#include "ros/ros.h"

#include "mikrokopter/FcDebug.h"

#include "std_msgs/Float64.h"

#include "std_msgs/Bool.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);

  void enable_cb(const std_msgs::Bool::ConstPtr& goal);

private:

  bool enabled;

  PID_control pid;

  ros::NodeHandle n;

  ros::Publisher pub;

  ros::Subscriber height_sub;

  ros::Subscriber goal_sub;

  ros::Subscriber enable_sub;

};

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

{

  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);

  enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable",

      100, &AltitudeNode::enable_cb, this);

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

  float default_goal;

  ros::param::param<float>("~default_goal", default_goal, 0);

  pid.change_goal(default_goal);

}

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

{

  if (enabled)

  {

    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);

}

void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg)

{

  enabled = msg->data;

  if (enabled)

  {

    ROS_INFO("Altitude Hold: Enabled.");

  }

  else

  {

    ROS_INFO("Altitude Hold: Disabled.");

  }

}

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

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

  AltitudeNode an = AltitudeNode();

  an.main_loop();

  return 0;

}