Statistics
| Branch: | Revision:

root / mikrokopter / mikrokopter / src / turn_to_target.cpp @ 98711613

History | View | Annotate | Download (1.13 KB)

1
#include "nav_lib.h"
2
#include <geometry_msgs/Point.h>
3
#include <math.h>
4

    
5
class TurnToTarget
6
{
7
public:
8
  TurnToTarget();
9
  void mainLoop();
10
  void pointCallback(const geometry_msgs::Point::ConstPtr& p);
11

    
12
private:
13
  MikrokopterControl control;
14
  ros::Publisher pub;
15
  ros::NodeHandle n;
16
  ros::Subscriber sub;
17
};
18

    
19
TurnToTarget::TurnToTarget()
20
{
21
  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
22
  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,
23
      &TurnToTarget::pointCallback, this);
24
}
25

    
26
void TurnToTarget::mainLoop()
27
{
28
  /*ros::Rate loop_rate(25);
29
  while(ros::ok())
30
  {
31
    ros::spinOnce();
32
    control.publish_on(pub);
33
    loop_rate.sleep();
34
  }*/
35
  control.main_loop();
36
}
37

    
38
void TurnToTarget::pointCallback(const geometry_msgs::Point::ConstPtr& p)
39
{
40
  // angle going CCW from forward in horizontal plane
41
  float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct
42
  // should be from [-100, 100]
43
  control.set_yaw(angle*100/M_PI);
44
}
45

    
46
int main(int argc, char **argv) {
47
  ros::init(argc, argv, "turn_to_target");
48
  TurnToTarget ttt = TurnToTarget();
49
  ttt.mainLoop();
50
  return 0;
51
}