Quadrotor

#include "nav_lib.h"

#include <geometry_msgs/Point.h>

#include <math.h>

class TurnToTarget

{

public:

  TurnToTarget();

  void mainLoop();

  void pointCallback(const geometry_msgs::Point::ConstPtr& p);

private:

  MikrokopterControl control;

  ros::Publisher pub;

  ros::NodeHandle n;

  ros::Subscriber sub;

};

TurnToTarget::TurnToTarget()

{

  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);

  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,

      &TurnToTarget::pointCallback, this);

}

void TurnToTarget::mainLoop()

{

  /*ros::Rate loop_rate(25);

  while(ros::ok())

  {

    ros::spinOnce();

    control.publish_on(pub);

    loop_rate.sleep();

  }*/

  control.main_loop();

}

void TurnToTarget::pointCallback(const geometry_msgs::Point::ConstPtr& p)

{

  // angle going CCW from forward in horizontal plane

  float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct

  // should be from [-100, 100]

  control.set_yaw(angle*100/M_PI);

}

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

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

  TurnToTarget ttt = TurnToTarget();

  ttt.mainLoop();

  return 0;

}