test1
Dependencies: ros_lib_indigo mbed
Revision 1:8080f7a0c35d, committed 2020-02-09
- Comitter:
- fibokmutt
- Date:
- Sun Feb 09 04:07:51 2020 +0000
- Parent:
- 0:17fd7572aedb
- Commit message:
- test1;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Apr 19 21:11:17 2016 +0000 +++ b/main.cpp Sun Feb 09 04:07:51 2020 +0000 @@ -1,30 +1,45 @@ -/* - * rosserial Publisher Example - * Prints "hello world!" - */ - #include"mbed.h" #include <ros.h> #include <std_msgs/String.h> +Timer t; ros::NodeHandle nh; - std_msgs::String str_msg; -ros::Publisher chatter("chatter", &str_msg); - -char hello[13] = "hello world!"; - +ros::Publisher pub("LL2ROS", &str_msg); +char msg_string[10]; +Serial debug(D1,D0); //UART2_TX and UART2_RX DigitalOut led = LED1; -int main() { +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(chatter); - + nh.advertise(pub); + nh.subscribe(sub1); + nh.subscribe(sub2); + debug.baud(115200); + debug.printf("ready \n"); + t.start(); while (1) { - led = !led; - str_msg.data = hello; - chatter.publish( &str_msg ); - nh.spinOnce(); - wait_ms(1000); + 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; + } } } \ No newline at end of file