rosserial test

Dependencies:   CANnucleo mbed-rtos mbed ros_lib_indigo

main.cpp

Committer:
piliwilliam
Date:
2016-10-06
Revision:
0:a66fe2119e59

File content as of revision 0:a66fe2119e59:

#include "mbed.h"
#include "rtos.h"
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Empty.h>

ros::NodeHandle  nh;

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

std_msgs::String banana_msg;
ros::Publisher banana("banana", &banana_msg);

char hello[13] = "hello world!";
char bababanana[11] = "bababanana";

DigitalOut LEDs[4] = {
    DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
};

Serial serial1(PA_9, PA_10);

void messageCb(const std_msgs::String& str_msg)
{
    //myled = !myled;   // blink the led
    banana_msg.data = bababanana;
    banana.publish(&banana_msg);
    //str_msg.data = hello;
    //chatter.publish( &str_msg );
}

ros::Subscriber<std_msgs::String> sub("chatter", &messageCb);

void blink(void const *n) {
    //int i = 1;
    //serial1.putc(i);
    str_msg.data = hello;
    chatter.publish( &str_msg );
}

int main()
{
    nh.getHardware()->setBaud(1000000);
    nh.initNode();
    nh.advertise(chatter);
    nh.advertise(banana);
    nh.subscribe(sub);
    
    
    serial1.baud(115200);
    RtosTimer led_1_timer(blink, osTimerPeriodic, (void *)0);
    led_1_timer.start(1000);//1000ms
    //printf("\n\n*** RTOS basic example ***\n");
    //Thread thread(print_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    //Thread::wait(osWaitForever);
    
    while (true) {

        
        //temp_msg.data = 1;
        //led_1_timer.start(100);
        //chatter.publish( &str_msg );
        nh.spinOnce();
    }
    
}