Add working example

Author: lukicdarkoo

examples/micro-ros_publisher/micro-ros_publisher.ino

@@ -18,31 +18,37 @@ rcl_timer_t timer;
 
 #define LED_PIN 13
 
-#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
-#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
-
+#define RCCHECK(fn)              \
+  {                              \
+    rcl_ret_t temp_rc = fn;      \
+    if ((temp_rc != RCL_RET_OK)) \
+    {                            \
+      error_loop();              \
+    }                            \
+  }
+#define RCSOFTCHECK(fn)          \
+  {                              \
+    rcl_ret_t temp_rc = fn;      \
+    if ((temp_rc != RCL_RET_OK)) \
+    {                            \
+      error_loop();              \
+    }                            \
+  }
 
-void error_loop(){
-  while(1){
+void error_loop()
+{
+  while (1)
+  {
     digitalWrite(LED_PIN, !digitalRead(LED_PIN));
     delay(100);
   }
 }
 
-void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
-{  
-  RCLC_UNUSED(last_call_time);
-  if (timer != NULL) {
-    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
-    msg.data++;
-  }
-}
-
-void setup() {
-  
+void setup()
+{
   pinMode(LED_PIN, OUTPUT);
-  digitalWrite(LED_PIN, HIGH);  
-  
+  digitalWrite(LED_PIN, HIGH);
+
   delay(2000);
 
   allocator = rcl_get_default_allocator();
@@ -56,32 +62,16 @@ void setup() {
 
   // create publisher
   RCCHECK(rclc_publisher_init_default(
-    &publisher,
-    &node,
-    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
-    "micro_ros_arduino_node_publisher"));
-
-  // create timer,
-  timer = rcl_get_zero_initialized_timer();
-  const unsigned int timer_timeout = 1000;
-  RCCHECK(rclc_timer_init_default(
-    &timer,
-    &support,
-    RCL_MS_TO_NS(timer_timeout),
-    timer_callback));
-
-  // create executor
-  executor = rclc_executor_get_zero_initialized_executor();
-  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
-
-  unsigned int rcl_wait_timeout = 100;   // in ms
-  RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
-  RCCHECK(rclc_executor_add_timer(&executor, &timer));
-
+      &publisher,
+      &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
+      "micro_ros_arduino_node_publisher"));
   msg.data = 0;
 }
 
-void loop() {
+void loop()
+{
   delay(100);
-  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
+  RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
+  msg.data++;
 }

examples/micro-ros_publisher/transport.cpp

@@ -44,17 +44,7 @@ extern "C"
     // Place here your reading bytes platform code
     // Return number of bytes read (max bytes: len)
     (void)errcode;
-    uint32_t start_time = micros() * 1000;
-    size_t readed = 0;
-
-    while ((readed < len) && ((micros() * 1000) - start_time) < (uint32_t)timeout)
-    {
-      if (SerialUSB.available())
-      {
-        buf[readed++] = SerialUSB.read();
-      }
-    }
-
-    return readed;
+    SerialUSB.setTimeout(timeout);
+    return SerialUSB.readBytes((char *)buf, len);
   }
-}
+}