Feature/add lognsequence sample (#23)

* Added long sequence pub/sub

* Update examples

* Update memset

* Update strcmp

* Minor fix

* Renaming

Author: pablogs9

rcl/CMakeLists.txt

@@ -41,6 +41,8 @@ export_executable(int32_multinode)
 export_executable(guard_condition)
 export_executable(int32_publisher_subscriber)
 export_executable(ping_pong)
+export_executable(fragmented_publication)
+export_executable(fragmented_subscription)
 
 if(EXISTS ${CMAKE_BINARY_DIR}/temp_install/)
     install(

rcl/addtwoints_server/main.c

@@ -36,7 +36,7 @@ int main(int argc, const char * const * argv)
     size_t index;
     RCSOFTCHECK(rcl_wait_set_add_service(&wait_set, &serv, &index))
 
-    RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(1)))
+    rcl_wait(&wait_set, RCL_MS_TO_NS(1));
     for (size_t i = 0; i < wait_set.size_of_services; i++) {
       if (wait_set.services[i]) {   
             rmw_request_id_t req_id;

rcl/fibonacci_action_server/main.c

@@ -84,7 +84,7 @@ int main(int argc, const char * const * argv)
     size_t index;
     RCSOFTCHECK(rcl_action_wait_set_add_action_server(&wait_set, &action_server, &index))
 
-    RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(200)))
+    rcl_wait(&wait_set, RCL_MS_TO_NS(200));
 
     bool is_goal_request_ready = false;
     bool is_cancel_request_ready = false;

rcl/fragmented_publication/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(fragmented_publication LANGUAGES C)
+
+find_package(ament_cmake REQUIRED)
+find_package(rcl REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rmw_microxrcedds REQUIRED)
+
+add_executable(${PROJECT_NAME} main.c)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rcl
+  std_msgs
+  rmw_microxrcedds
+  )
+
+install(TARGETS ${PROJECT_NAME}
+  DESTINATION ${PROJECT_NAME}
+  )

rcl/fragmented_publication/main.c

@@ -0,0 +1,59 @@
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <std_msgs/msg/string.h>
+
+#include <rmw_uros/options.h>
+
+#include <stdio.h>
+#include <unistd.h>
+
+#define ARRAY_LEN 4096
+
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
+#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
+
+int main(int argc, const char * const * argv)
+{
+  rcl_init_options_t options = rcl_get_zero_initialized_init_options();
+  RCCHECK(rcl_init_options_init(&options, rcl_get_default_allocator()))
+
+  rcl_context_t context = rcl_get_zero_initialized_context();
+  RCCHECK(rcl_init(argc, argv, &options, &context))
+
+  rcl_node_options_t node_ops = rcl_node_get_default_options();
+
+  rcl_node_t node = rcl_get_zero_initialized_node();
+  RCCHECK(rcl_node_init(&node, "char_long_sequence_publisher_rcl", "", &context, &node_ops))
+
+  rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options();
+  rcl_publisher_t publisher = rcl_get_zero_initialized_publisher();
+  RCCHECK(rcl_publisher_init(&publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), "/char_long_sequence", &publisher_ops))
+  
+  std_msgs__msg__String msg;
+  msg.data.data = (char * ) malloc(ARRAY_LEN * sizeof(char));
+  msg.data.size = 0;
+  msg.data.capacity = ARRAY_LEN;
+
+  // Fill the array with a known sequence
+  memset(msg.data.data,'z',3500);
+  msg.data.data[3500] = '\0';
+  msg.data.size = 3501;
+
+  sleep(2); // Sleep a while to ensure DDS matching before sending request
+
+  rcl_ret_t rc;
+  do {
+    rc = rcl_publish(&publisher, (const void*)&msg, NULL);
+    if (RCL_RET_OK == rc ) {
+        printf("Sent %ld array\n", msg.data.size);
+    }else{
+        printf("Publishing error %d\n", rc);
+    }
+    sleep(1);
+  } while (true);
+  
+  RCCHECK(rcl_publisher_fini(&publisher, &node))
+  RCCHECK(rcl_node_fini(&node))
+
+  return 0;
+}

rcl/fragmented_subscription/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(fragmented_subscription LANGUAGES C)
+
+find_package(ament_cmake REQUIRED)
+find_package(rcl REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rmw_microxrcedds REQUIRED)
+
+add_executable(${PROJECT_NAME} main.c)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rcl
+  std_msgs
+  rmw_microxrcedds
+  )
+
+install(TARGETS ${PROJECT_NAME}
+  DESTINATION ${PROJECT_NAME}
+  )

rcl/fragmented_subscription/main.c

@@ -0,0 +1,68 @@
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <std_msgs/msg/string.h>
+
+#include <rmw_uros/options.h>
+
+#include <stdio.h>
+
+#define ARRAY_LEN 4096
+
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
+#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
+
+int main(int argc, const char * const * argv)
+{
+  rcl_init_options_t options = rcl_get_zero_initialized_init_options();
+  RCCHECK(rcl_init_options_init(&options, rcl_get_default_allocator()))
+
+  rcl_context_t context = rcl_get_zero_initialized_context();
+  RCCHECK(rcl_init(argc, argv, &options, &context))
+
+  rcl_node_options_t node_ops = rcl_node_get_default_options();
+  rcl_node_t node = rcl_get_zero_initialized_node();
+  RCCHECK(rcl_node_init(&node, "char_long_sequence_subscriber_rcl", "", &context, &node_ops))
+
+  rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options();
+  rcl_subscription_t subscription = rcl_get_zero_initialized_subscription();
+  RCCHECK(rcl_subscription_init(&subscription, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), "/char_long_sequence", &subscription_ops))
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  RCCHECK(rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator()))
+
+  std_msgs__msg__String msg;
+
+  msg.data.data = (char * ) malloc(ARRAY_LEN * sizeof(char));
+  msg.data.size = 0;
+  msg.data.capacity = ARRAY_LEN;
+  
+  char test_array[ARRAY_LEN];
+  memset(test_array,'z',ARRAY_LEN);
+
+  do {
+    RCSOFTCHECK(rcl_wait_set_clear(&wait_set))
+    
+    size_t index;
+    RCSOFTCHECK(rcl_wait_set_add_subscription(&wait_set, &subscription, &index))
+    
+    RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(1)))
+
+    if (wait_set.subscriptions[index]) {
+
+
+      rcl_ret_t rc = rcl_take(wait_set.subscriptions[index], &msg, NULL, NULL);
+      if (RCL_RET_OK == rc) {
+        
+        // Check if sequence items matches the know pattern
+        bool pass_test = strncmp(test_array, msg.data.data, msg.data.size) == 0;
+
+        printf("I received an %ld array. Test: [%s]\n", msg.data.size, (pass_test) ? "OK" : "FAIL");
+      }
+    }
+  } while ( true );
+
+  RCCHECK(rcl_subscription_fini(&subscription, &node))
+  RCCHECK(rcl_node_fini(&node))
+
+  return 0;
+}