Quadrotor

#include "ros/ros.h"

#include "std_msgs/Float64.h"

#include "mikrokopter/Velocity2D.h"

#include "nav_lib.h"

class NodeControl

{

public:

  NodeControl();

  void main_loop() {control.main_loop();}

  void cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n);

  void cb_yaw(const std_msgs::Float64::ConstPtr& n);

  void cb_thrust(const std_msgs::Float64::ConstPtr& n);

private:

  MikrokopterControl control;

  ros::NodeHandle n;

  ros::Subscriber sub_velocity;

  ros::Subscriber sub_yaw;

  ros::Subscriber sub_thrust;

};

NodeControl::NodeControl()

{

  sub_velocity = n.subscribe<mikrokopter::Velocity2D>("/mikrokopter/velocity",

      100, &NodeControl::cb_velocity, this);

  sub_yaw = n.subscribe<std_msgs::Float64>("/mikrokopter/yaw", 100,

      &NodeControl::cb_yaw, this);

  sub_thrust= n.subscribe<std_msgs::Float64>("/mikrokopter/thrust", 100,

      &NodeControl::cb_thrust, this);

}

void NodeControl::cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n)

{

  control.velocity_control(n->forward, n->lateral);

}

void NodeControl::cb_yaw(const std_msgs::Float64::ConstPtr& n)

{

  control.set_yaw(n->data);

}

void NodeControl::cb_thrust(const std_msgs::Float64::ConstPtr& n)

{

  control.set_thrust(n->data);

}

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

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

  NodeControl control = NodeControl();

  control.main_loop();

  return 0;

}