root / mikrokopter / mikrokopter / src / turn_to_target.cpp @ 98711613
History | View | Annotate | Download (1.13 KB)
1 | 06a5548b | Tom Mullins | #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 | 7a6ed02d | Tom Mullins | //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
|
22 | 06a5548b | Tom Mullins | sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100, |
23 | &TurnToTarget::pointCallback, this);
|
||
24 | } |
||
25 | |||
26 | void TurnToTarget::mainLoop()
|
||
27 | { |
||
28 | 7a6ed02d | Tom Mullins | /*ros::Rate loop_rate(25);
|
29 | 06a5548b | Tom Mullins | while(ros::ok())
|
30 | {
|
||
31 | ros::spinOnce();
|
||
32 | control.publish_on(pub);
|
||
33 | loop_rate.sleep();
|
||
34 | 7a6ed02d | Tom Mullins | }*/
|
35 | control.main_loop(); |
||
36 | 06a5548b | Tom Mullins | } |
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 | } |