Statistics
| Branch: | Revision:

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