rosserial for Hydro
Fork of rosserial_mbed_lib by
Diff: tests/array_test.cpp
- Revision:
- 3:1cf99502f396
diff -r bb6bb835fde4 -r 1cf99502f396 tests/array_test.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tests/array_test.cpp Sat Nov 12 23:54:45 2011 +0000 @@ -0,0 +1,47 @@ +//#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 \ No newline at end of file