root / mikrokopter / src / turn_to_target.cpp @ 7a6ed02d
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 |
} |