hello_world_tcp.cpp 1.6 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465
  1. #include <rtthread.h>
  2. #include <ros.h>
  3. #include <std_msgs/String.h>
  4. static ros::NodeHandle nh;
  5. static std_msgs::String str_msg;
  6. static ros::Publisher chatter("chatter", &str_msg);
  7. static char hello_msg[13] = "hello world!";
  8. static void rosserial_thread_entry(void *parameter)
  9. {
  10. // Please make sure you have network connection first
  11. // Set ip address and port
  12. nh.getHardware()->setConnection("192.168.199.100", 11411);
  13. nh.initNode();
  14. nh.advertise(chatter);
  15. while (1)
  16. {
  17. if ( nh.connected() )
  18. {
  19. str_msg.data = hello_msg;
  20. chatter.publish( &str_msg );
  21. }
  22. /*
  23. else
  24. {
  25. rt_kprintf("[rosserial] Not Connected \n");
  26. }
  27. */
  28. nh.spinOnce();
  29. rt_thread_delay(RT_TICK_PER_SECOND);
  30. }
  31. }
  32. static void rosserial_hello_world_tcp_example(int argc,char *argv[])
  33. {
  34. rt_thread_t thread = rt_thread_create("rosserial", rosserial_thread_entry, RT_NULL, 2048, 25, 10);
  35. if(thread != RT_NULL)
  36. {
  37. rt_thread_startup(thread);
  38. rt_kprintf("[rosserial] New thread rosserial\n");
  39. }
  40. else
  41. {
  42. rt_kprintf("[rosserial] Failed to create thread rosserial\n");
  43. }
  44. }
  45. MSH_CMD_EXPORT(rosserial_hello_world_tcp_example, roserial hello world example with TCP);
  46. // If you are using Keil, you can ignore everything below
  47. // This is required
  48. // If you'd like to compile with scons which uses arm-none-eabi-gcc
  49. // extern "C" void __cxa_pure_virtual()
  50. // {
  51. // while (1);
  52. // }
  53. // Moreover, you need to add:
  54. // CXXFLAGS = CFLAGS + ' -fno-rtti'
  55. // in rtconfig.py for arm-none-eabi-gcc