Refs #3845. Solves bug of publisher susbscriber examples.

Author: BorjaOuterelo

examples/uros/publisher/publisher_main.c

@@ -11,21 +11,37 @@ int publisher_main(int argc, char* argv[])
 {
     (void)argc;
     (void)argv;
+    int result = 0;
     rclc_init(1, "");
     const rclc_message_type_support_t type_support = RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);
-    rclc_node_t* node     = rclc_create_node("publisher_node", "");
-    rclc_publisher_t* publisher = rclc_create_publisher(node, type_support, "publisher_example", 1);
-
-    std_msgs__msg__Int32 msg;
-    std_msgs__msg__Int32__init(&msg);
+    rclc_node_t* node = NULL;
+    if (node = rclc_create_node("publisher_node", ""))
+    {
+        rclc_publisher_t* publisher = NULL;
+        if(publisher = rclc_create_publisher(node, type_support, "std_msgs_msg_Int32", 1))
+        {
 
-    while (rclc_ok())
+            std_msgs__msg__Int32 msg;
+            msg.data = 0;
+            while (rclc_ok() && msg.data <= 1000)
+            {
+                printf("Sending: '%i'\n", msg.data++);
+                rclc_publish(publisher, (const void*)&msg);
+                rclc_spin_node_once(node, 500);
+            }
+            rclc_destroy_publisher(publisher);
+        }
+        else
+        {
+            printf("Issues creating publisher\n");
+            result = -1;
+        }
+        rclc_destroy_node(node);
+    }
+    else
     {
-        printf("Sending: '%i'\n", msg.data++);       
-        rclc_publish(publisher, (const void*)&msg);
-        rclc_spin_node_once(node, 500);
+        printf("Issues creating node\n");
+        result = -1;
     }
-    rclc_destroy_publisher(publisher);
-    rclc_destroy_node(node);
-    return 0;
+    return result;
 }

examples/uros/subscriber/subscriber_main.c

@@ -3,10 +3,13 @@
 #include <std_msgs/msg/int32.h>
 #include <stdio.h>
 
+static int count = 0;
+
 void on_message(const void* msgin)
 {
     const std_msgs__msg__Int32* msg = (const std_msgs__msg__Int32*)msgin;
     printf("I heard: [%i]\n", msg->data);
+    count++;
 }
 
 #if defined(BUILD_MODULE)
@@ -17,15 +20,32 @@ int subscriber_main(int argc, char* argv[])
 {
     (void)argc;
     (void)argv;
-    rclc_init(0, NULL);
-    rclc_node_t* node        = rclc_create_node("int32_subscriber_c", "");
-
-    rclc_subscription_t* sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
-                                                        "std_msgs_msg_Int32", on_message, 1, false);
-
-    rclc_spin_node(node);
-
-    rclc_destroy_subscription(sub);
-    rclc_destroy_node(node);
-    return 0;
+    int result = 0;
+    rclc_init(1, "");
+    rclc_node_t* node = NULL;
+    if (node = rclc_create_node("int32_subscriber_c", ""))
+    {
+        rclc_subscription_t* sub = NULL;
+        if(sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
+                                                        "std_msgs_msg_Int32", on_message, 1, false))
+        {
+            while (rclc_ok() && count <= 50)
+            {
+                rclc_spin_node_once(node, 500);
+            }
+            rclc_destroy_subscription(sub);
+        }
+        else
+        {
+            printf("Issues creating subscriber\n");
+            result = -1;
+        }
+        rclc_destroy_node(node);
+    }
+    else
+    {
+        printf("Issues creating node\n");
+        result = -1;
+    }
+    return result;
 }