root / quad2 / arduino / src / ros_lib / tests / array_test / array_test.pde @ c1426757
History | View | Annotate | Download (909 Bytes)
1 |
/* |
---|---|
2 |
* rosserial::geometry_msgs::PoseArray Test |
3 |
* Sums an array, publishes sum |
4 |
*/ |
5 |
|
6 |
#include <ros.h> |
7 |
#include <geometry_msgs/Pose.h> |
8 |
#include <geometry_msgs/PoseArray.h> |
9 |
|
10 |
|
11 |
ros::NodeHandle nh; |
12 |
|
13 |
|
14 |
bool set_; |
15 |
|
16 |
|
17 |
geometry_msgs::Pose sum_msg; |
18 |
ros::Publisher p("sum", &sum_msg); |
19 |
|
20 |
void messageCb(const geometry_msgs::PoseArray& msg){ |
21 |
sum_msg.position.x = 0; |
22 |
sum_msg.position.y = 0; |
23 |
sum_msg.position.z = 0; |
24 |
for(int i = 0; i < msg.poses_length; i++) |
25 |
{ |
26 |
sum_msg.position.x += msg.poses[i].position.x; |
27 |
sum_msg.position.y += msg.poses[i].position.y; |
28 |
sum_msg.position.z += msg.poses[i].position.z; |
29 |
} |
30 |
digitalWrite(13, HIGH-digitalRead(13)); // blink the led |
31 |
} |
32 |
|
33 |
ros::Subscriber<geometry_msgs::PoseArray> s("poses",messageCb); |
34 |
|
35 |
void setup() |
36 |
{ |
37 |
pinMode(13, OUTPUT); |
38 |
nh.initNode(); |
39 |
nh.subscribe(s); |
40 |
nh.advertise(p); |
41 |
} |
42 |
|
43 |
void loop() |
44 |
{ |
45 |
p.publish(&sum_msg); |
46 |
nh.spinOnce(); |
47 |
delay(10); |
48 |
} |
49 |
|