Rework demos (#42)

* Rework examples

Acording to micro-ROS/rclc#58

* Rework add two ints service

* Rework callback

* fix callback

* Update service

* Remove rclc_executor_set_timeout

* Update apps/add_two_ints_service/app.c

Co-authored-by: Jose Antonio Moral <[email protected]>

Co-authored-by: Jose Antonio Moral <[email protected]>

Author: pablogs9

apps/add_two_ints_service/app.c

@@ -12,70 +12,44 @@
 #include <unistd.h>
 #include <pthread.h>
 
-#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);  vTaskDelete(NULL);;}}
-#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);}}
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printk("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);}}
+#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printk("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
 
-// TODO(jamoralp): update using RCLC convenience functions once services are implemented there.
-void appMain(void *argument)
-{
-  printf("Free heap pre uROS: %d bytes\n", xPortGetFreeHeapSize());
+void service_callback(const void * req, void * res){
+  example_interfaces__srv__AddTwoInts_Request * req_in = (example_interfaces__srv__AddTwoInts_Request *) req;
+  example_interfaces__srv__AddTwoInts_Response * res_in = (example_interfaces__srv__AddTwoInts_Response *) res;
 
-  rcl_init_options_t options = rcl_get_zero_initialized_init_options();
+  printf("Service request value: %d + %d.\n", (int) req_in->a, (int) req_in->b);
 
-  RCCHECK(rcl_init_options_init(&options, rcl_get_default_allocator()))
+  res_in->sum = req_in->a + req_in->b;
+}
 
-  // Optional RMW configuration
-  rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&options);
-  RCCHECK(rmw_uros_options_set_client_key(0xBA5EBA11, rmw_options))
+void main(void)
+{	
+    rcl_allocator_t allocator = rcl_get_default_allocator();
 
-  rcl_context_t context = rcl_get_zero_initialized_context();
-  RCCHECK(rcl_init(0, NULL, &options, &context))
+    // create init_options
+    rclc_support_t support;
+    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
 
-  rcl_node_options_t node_ops = rcl_node_get_default_options();
+    // create node
+    rcl_node_t node;
+    RCCHECK(rclc_node_init_default(&node, "add_twoints_server_rclc", "", &support));
 
-  rcl_node_t node = rcl_get_zero_initialized_node();
-  RCCHECK(rcl_node_init(&node, "olimex_node", "", &context, &node_ops))
+    // create service
+    rcl_service_t service;
+    RCCHECK(rclc_service_init_default(&service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts), "/addtwoints"));
 
-  const char * service_name = "addtwoints";
-  rcl_service_options_t service_op = rcl_service_get_default_options();
-  service_op.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
-  rcl_service_t serv = rcl_get_zero_initialized_service();
-  const rosidl_service_type_support_t * service_type_support =
-    ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);
+    // create executor
+    rclc_executor_t executor;
+    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
 
-  RCCHECK(rcl_service_init(&serv, &node, service_type_support, service_name, &service_op))
+    example_interfaces__srv__AddTwoInts_Response res;
+    example_interfaces__srv__AddTwoInts_Request req;
+    RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, service_callback));
 
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  RCCHECK(rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, &context, rcl_get_default_allocator()))
+    rclc_executor_spin(&executor);
 
-  printf("Free heap post uROS configuration: %d bytes\n", xPortGetFreeHeapSize());
-  printf("uROS Used Memory %d bytes\n", usedMemory);
-  printf("uROS Absolute Used Memory %d bytes\n", absoluteUsedMemory);
-
-  rcl_ret_t rc;
-  do {
-    RCSOFTCHECK(rcl_wait_set_clear(&wait_set))
-
-    size_t index_service;
-    RCSOFTCHECK(rcl_wait_set_add_service(&wait_set, &serv, &index_service))
-
-    RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(100)))
-
-    if (wait_set.services[index_service]) {
-      rmw_request_id_t req_id;
-      example_interfaces__srv__AddTwoInts_Request req;
-      example_interfaces__srv__AddTwoInts_Request__init(&req);
-      RCSOFTCHECK(rcl_take_request(&serv,&req_id,&req))
-
-      printf("Service request value: %d + %d. Seq %d\n", (int)req.a, (int)req.b, (int) req_id.sequence_number);
-
-      example_interfaces__srv__AddTwoInts_Response res;
-      example_interfaces__srv__AddTwoInts_Response__init(&res);
-
-      res.sum = req.a + req.b;
-
-      RCSOFTCHECK(rcl_send_response(&serv,&req_id,&res))
-    }
-    usleep(10000);
-  } while (true);
-}
+    RCCHECK(rcl_service_fini(&service, &node));
+    RCCHECK(rcl_node_fini(&node));
+}

apps/crazyflie_position_publisher/app.c

@@ -65,7 +65,7 @@ void appMain(){
 	RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
 
 	// create node
-	rcl_node_t node = rcl_get_zero_initialized_node();
+	rcl_node_t node;
 	RCCHECK(rclc_node_init_default(&node, "crazyflie_node", "", &support));
 
 	// create publishers

apps/int32_publisher/app.c

@@ -37,7 +37,7 @@ void appMain(void * arg)
 	RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
 
 	// create node
-	rcl_node_t node = rcl_get_zero_initialized_node();
+	rcl_node_t node;
 	RCCHECK(rclc_node_init_default(&node, "freertos_int32_publisher", "", &support));
 
 	// create publisher
@@ -48,7 +48,7 @@ void appMain(void * arg)
 		"freertos_int32_publisher"));
 
 	// create timer,
-	rcl_timer_t timer = rcl_get_zero_initialized_timer();
+	rcl_timer_t timer;
 	const unsigned int timer_timeout = 1000;
 	RCCHECK(rclc_timer_init_default(
 		&timer,
@@ -57,11 +57,8 @@ void appMain(void * arg)
 		timer_callback));
 
 	// create executor
-	rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
+	rclc_executor_t executor;
 	RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
-
-	unsigned int rcl_wait_timeout = 1000;   // in ms
-	RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
 	RCCHECK(rclc_executor_add_timer(&executor, &timer));
 
 	msg.data = 0;

apps/int32_subscriber/app.c

@@ -28,7 +28,7 @@ void appMain(void * arg)
 	RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
 
 	// create node
-	rcl_node_t node = rcl_get_zero_initialized_node();
+	rcl_node_t node;
 	RCCHECK(rclc_node_init_default(&node, "int32_subscriber_rclc", "", &support));
 
 	// create subscriber
@@ -39,20 +39,18 @@ void appMain(void * arg)
 		"/microROS/int32_subscriber"));
 
 	// create executor
-	rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
+	rclc_executor_t executor;
 	RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
-
-	unsigned int rcl_wait_timeout = 1000;   // in ms
-	RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
 	RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
 
-        while(1){
-                rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
-                usleep(100000);
-        }
-
+	while(1){
+			rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
+			usleep(100000);
+	}
 
+	// free resources
 	RCCHECK(rcl_subscription_fini(&subscriber, &node));
 	RCCHECK(rcl_node_fini(&node));
+	
 	vTaskDelete(NULL);
 }

apps/ping_pong/app.c

@@ -87,7 +87,7 @@ void appMain(void *argument)
 	RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
 
 	// create node
-	rcl_node_t node = rcl_get_zero_initialized_node();
+	rcl_node_t node;
 	RCCHECK(rclc_node_init_default(&node, "pingpong_node", "", &support));
 
 	// Create a reliable ping publisher
@@ -108,16 +108,13 @@ void appMain(void *argument)
 
 
 	// Create a 3 seconds ping timer timer,
-	rcl_timer_t timer = rcl_get_zero_initialized_timer();
+	rcl_timer_t timer;
 	RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(2000), ping_timer_callback));
 
 
 	// Create executor
-	rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
+	rclc_executor_t executor;
 	RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator));
-
-	unsigned int rcl_wait_timeout = 10;   // in ms
-	RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
 	RCCHECK(rclc_executor_add_timer(&executor, &timer));
 	RCCHECK(rclc_executor_add_subscription(&executor, &ping_subscriber, &incoming_ping,
 		&ping_subscription_callback, ON_NEW_DATA));