nucho / Mbed 2 deprecated rosserial_mbed

This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial. This program contains an example.

Dependencies:   rosserial_mbed_lib mbed Servo

tests/array_test.cpp@0:06fc856e99ca, 2011-08-19 (annotated)

Committer:
nucho
Date:
Fri Aug 19 09:06:16 2011 +0000
Revision:
0:06fc856e99ca

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


nucho

0:06fc856e99ca


     1

//#define COMPILE_ARRAY_CODE_RSOSSERIAL





nucho

0:06fc856e99ca


     2

#ifdef COMPILE_ARRAY_CODE_RSOSSERIAL





nucho

0:06fc856e99ca


     3







nucho

0:06fc856e99ca


     4

/*





nucho

0:06fc856e99ca


     5

 * rosserial::geometry_msgs::PoseArray Test





nucho

0:06fc856e99ca


     6

 * Sums an array, publishes sum





nucho

0:06fc856e99ca


     7

 */





nucho

0:06fc856e99ca


     8

#include "mbed.h"





nucho

0:06fc856e99ca


     9

#include <ros.h>





nucho

0:06fc856e99ca


    10

#include <geometry_msgs/Pose.h>





nucho

0:06fc856e99ca


    11

#include <geometry_msgs/PoseArray.h>





nucho

0:06fc856e99ca


    12







nucho

0:06fc856e99ca


    13







nucho

0:06fc856e99ca


    14

ros::NodeHandle nh;





nucho

0:06fc856e99ca


    15







nucho

0:06fc856e99ca


    16

bool set_;





nucho

0:06fc856e99ca


    17

DigitalOut myled(LED1);





nucho

0:06fc856e99ca


    18







nucho

0:06fc856e99ca


    19

geometry_msgs::Pose sum_msg;





nucho

0:06fc856e99ca


    20

ros::Publisher p("sum", &sum_msg);





nucho

0:06fc856e99ca


    21







nucho

0:06fc856e99ca


    22

void messageCb(const geometry_msgs::PoseArray& msg) {





nucho

0:06fc856e99ca


    23

    sum_msg.position.x = 0;





nucho

0:06fc856e99ca


    24

    sum_msg.position.y = 0;





nucho

0:06fc856e99ca


    25

    sum_msg.position.z = 0;





nucho

0:06fc856e99ca


    26

    for (int i = 0; i < msg.poses_length; i++) {





nucho

0:06fc856e99ca


    27

        sum_msg.position.x += msg.poses[i].position.x;





nucho

0:06fc856e99ca


    28

        sum_msg.position.y += msg.poses[i].position.y;





nucho

0:06fc856e99ca


    29

        sum_msg.position.z += msg.poses[i].position.z;





nucho

0:06fc856e99ca


    30

    }





nucho

0:06fc856e99ca


    31

    myled = !myled;   // blink the led





nucho

0:06fc856e99ca


    32

}





nucho

0:06fc856e99ca


    33







nucho

0:06fc856e99ca


    34

ros::Subscriber<geometry_msgs::PoseArray> s("poses",messageCb);





nucho

0:06fc856e99ca


    35







nucho

0:06fc856e99ca


    36

int main() {





nucho

0:06fc856e99ca


    37

    nh.initNode();





nucho

0:06fc856e99ca


    38

    nh.subscribe(s);





nucho

0:06fc856e99ca


    39

    nh.advertise(p);





nucho

0:06fc856e99ca


    40







nucho

0:06fc856e99ca


    41

    while (1) {





nucho

0:06fc856e99ca


    42

        p.publish(&sum_msg);





nucho

0:06fc856e99ca


    43

        nh.spinOnce();





nucho

0:06fc856e99ca


    44

        wait_ms(10);





nucho

0:06fc856e99ca


    45

    }





nucho

0:06fc856e99ca


    46

}





nucho

0:06fc856e99ca


    47

#endif