Personal fork
Fork of rosserial_mbed_lib by
Embed:
(wiki syntax)
Show/hide line numbers
array_test.cpp
00001 //#define COMPILE_ARRAY_CODE_RSOSSERIAL 00002 #ifdef COMPILE_ARRAY_CODE_RSOSSERIAL 00003 00004 /* 00005 * rosserial::geometry_msgs::PoseArray Test 00006 * Sums an array, publishes sum 00007 */ 00008 #include "mbed.h" 00009 #include <ros.h> 00010 #include <geometry_msgs/Pose.h> 00011 #include <geometry_msgs/PoseArray.h> 00012 00013 00014 ros::NodeHandle nh; 00015 00016 bool set_; 00017 DigitalOut myled(LED1); 00018 00019 geometry_msgs::Pose sum_msg; 00020 ros::Publisher p("sum", &sum_msg); 00021 00022 void messageCb(const geometry_msgs::PoseArray& msg) { 00023 sum_msg.position.x = 0; 00024 sum_msg.position.y = 0; 00025 sum_msg.position.z = 0; 00026 for (int i = 0; i < msg.poses_length; i++) { 00027 sum_msg.position.x += msg.poses[i].position.x; 00028 sum_msg.position.y += msg.poses[i].position.y; 00029 sum_msg.position.z += msg.poses[i].position.z; 00030 } 00031 myled = !myled; // blink the led 00032 } 00033 00034 ros::Subscriber<geometry_msgs::PoseArray> s("poses",messageCb); 00035 00036 int main() { 00037 nh.initNode(); 00038 nh.subscribe(s); 00039 nh.advertise(p); 00040 00041 while (1) { 00042 p.publish(&sum_msg); 00043 nh.spinOnce(); 00044 wait_ms(10); 00045 } 00046 } 00047 #endif
Generated on Tue Jul 12 2022 19:53:56 by 1.7.2