nucho / rosserial_mbed_lib

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

Dependencies:  

Dependents:   rosserial_mbed robot_S2

tests/array_test.cpp

Committer:
nucho
Date:
2012-02-29
Revision:
4:684f39d0c346
Parent:
3:1cf99502f396

File content as of revision 4:684f39d0c346:

//#define COMPILE_ARRAY_CODE_RSOSSERIAL
#ifdef COMPILE_ARRAY_CODE_RSOSSERIAL

/*
 * rosserial::geometry_msgs::PoseArray Test
 * Sums an array, publishes sum
 */
#include "mbed.h"
#include <ros.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>


ros::NodeHandle nh;

bool set_;
DigitalOut myled(LED1);

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

void messageCb(const geometry_msgs::PoseArray& msg) {
    sum_msg.position.x = 0;
    sum_msg.position.y = 0;
    sum_msg.position.z = 0;
    for (int i = 0; i < msg.poses_length; i++) {
        sum_msg.position.x += msg.poses[i].position.x;
        sum_msg.position.y += msg.poses[i].position.y;
        sum_msg.position.z += msg.poses[i].position.z;
    }
    myled = !myled;   // blink the led
}

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

int main() {
    nh.initNode();
    nh.subscribe(s);
    nh.advertise(p);

    while (1) {
        p.publish(&sum_msg);
        nh.spinOnce();
        wait_ms(10);
    }
}
#endif