root / quad2 / arduino / src / ros_lib / tests / array_test / array_test.pde @ c1426757
History | View | Annotate | Download (909 Bytes)
1 | c1426757 | Tom Mullins | /* |
---|---|---|---|
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 | } |