main.cpp

00001 #include "mbed.h"
00002 #include <ros.h>
00003 #include <std_msgs/Int16.h>
00004 
00005 //--------- GLOBAL VARIABLES LOCAL TO STM32 MAIN -----------
00006 DigitalOut my_led(LED1);
00007 
00008 int data_recieved     = 0;
00009 
00010 //--------  END GLOBAL VARIABLES DECLARATION ---------------
00011 
00012 
00013 //--------- ROS NODE CONFIGURATION -------------------------
00014 ros::NodeHandle nh;
00015 
00016 // Ros message data containers
00017 std_msgs::Int16 recieve_msg;
00018 std_msgs::Int16 transmit_msg;
00019 
00020 // Set up Publishers
00021 ros::Publisher acknowledge_publisher("acknowledge", &transmit_msg);
00022             
00023 // Set up Subscribers
00024 void reciever_callback( const std_msgs::Int16& msg)
00025 {
00026     // Assign msg.data to local variables
00027     data_recieved = msg.data;
00028 }
00029 
00030 ros::Subscriber<std_msgs::Int16> reciever_subscriber("test", reciever_callback);
00031 
00032 
00033 //---------- END CONFIGURATION  ----------------------------
00034 
00035 int main() {
00036     
00037     // Initialize Node and publisher/subscriber
00038     nh.initNode();
00039     nh.advertise(acknowledge_publisher);
00040     nh.subscribe(reciever_subscriber);
00041     
00042     while(1) {
00043         
00044         // Use local variables for computations here
00045         
00046         
00047         // Publish any data necessary to be fed back to pc
00048         transmit_msg.data = data_recieved;
00049         acknowledge_publisher.publish(&transmit_msg);
00050         
00051         // Spin ROS Node -- get data from callbacks
00052         nh.spinOnce();
00053         wait_ms(5);
00054     }
00055 }