test1

Dependencies:   ros_lib_indigo mbed

main.cpp

Committer:
fibokmutt
Date:
2020-02-09
Revision:
1:8080f7a0c35d
Parent:
0:17fd7572aedb

File content as of revision 1:8080f7a0c35d:

#include"mbed.h"
#include <ros.h>
#include <std_msgs/String.h>
Timer t;

ros::NodeHandle  nh;
std_msgs::String str_msg;
ros::Publisher pub("LL2ROS", &str_msg);
char msg_string[10];
Serial debug(D1,D0); //UART2_TX and UART2_RX
DigitalOut led = LED1;

void matlab_msg( const std_msgs::String& msg)
{
    debug.printf("MATLAB response time: %lu \n", t.read_ms());
}

void python_msg( const std_msgs::String& msg)
{
    debug.printf("PYTHON response time: %lu \n", t.read_ms());
}

ros::Subscriber<std_msgs::String> sub1("MATLAB2LL", matlab_msg);
ros::Subscriber<std_msgs::String> sub2("PYTHON2LL", python_msg);

int main()
{
    nh.initNode();
    nh.advertise(pub);
    nh.subscribe(sub1);
    nh.subscribe(sub2);
    debug.baud(115200);
    debug.printf("ready \n");
    t.start();
    while (1) {
        if (t.read_ms() >  1000) {
            t.reset();
            debug.printf("send datat to ROS %lu \n", t.read_ms());
            str_msg.data="data to ROS";
            pub.publish( &str_msg );
            nh.spinOnce();
            led = !led;
        }
    }
}