Release micro-ROS Foxy

README.md

@@ -1 +1,11 @@
-freertos_apps
+# Introduction
+Set of example applications for micro-ROS running over FreeRTOS.
+
+# Supported platforms
+Supported hardware platforms are:
+* Olimex STM32-E407 development board.
+* Crazyflie 2.1 drone.
+
+# Usage
+These applications are integrated within the [micro-ROS build system](https://github.com/micro-ROS/micro-ros-build).
+Please refer to this repository's documentation for further information about how to configure, build and flash them.

apps/add_two_ints/app-colcon.meta → apps/add_two_ints_service/app-colcon.meta

@@ -2,14 +2,12 @@
     "names": {
         "rmw_microxrcedds": {
             "cmake-args": [
-                "-DRMW_UXRCE_TRANSPORT=custom_serial",
                 "-DRMW_UXRCE_MAX_NODES=1",
                 "-DRMW_UXRCE_MAX_PUBLISHERS=0",
                 "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=0",
                 "-DRMW_UXRCE_MAX_SERVICES=1",
                 "-DRMW_UXRCE_MAX_CLIENTS=0",
                 "-DRMW_UXRCE_MAX_HISTORY=4",
-                "-DRMW_UXRCE_DEFAULT_SERIAL_DEVICE=3"
             ]
         }
     }

apps/add_two_ints/app.c → apps/add_two_ints_service/app.c

@@ -15,16 +15,16 @@
 #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);}}
 
+// 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());
 
   rcl_init_options_t options = rcl_get_zero_initialized_init_options();
 
   RCCHECK(rcl_init_options_init(&options, rcl_get_default_allocator()))
 
-  // Optional RMW configuration 
+  // 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))
 
@@ -40,28 +40,28 @@ void appMain(void *argument)
   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);
+  const rosidl_service_type_support_t * service_type_support =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);
 
   RCCHECK(rcl_service_init(&serv, &node, service_type_support, service_name, &service_op))
 
   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()))
-  
+
   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)))
 
+    RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(100)))
 
-    if (wait_set.services[index_service]) {   
+    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);
@@ -71,7 +71,7 @@ void appMain(void *argument)
 
       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))

apps/crazyflie_position_publisher/app.c

@@ -1,14 +1,14 @@
-/*FreeRtos includes*/
 #include "FreeRTOS.h"
 #include "task.h"
 
 #include <rcl/rcl.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
 #include <geometry_msgs/msg/point32.h>
-#include "example_interfaces/srv/add_two_ints.h"
-#include <geometry_msgs/msg/twist.h>
 
 #include <rcutils/allocator.h>
 
+
 #include "config.h"
 #include "log.h"
 #include "crc.h"
@@ -19,17 +19,20 @@
 
 #include "microrosapp.h"
 
-#define RCCHECK(msg) if((rc != RCL_RET_OK)){DEBUG_PRINT("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)rc); vTaskSuspend( NULL );}
-#define RCSOFTCHECK(msg) if((rc != RCL_RET_OK)){DEBUG_PRINT("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)rc);}
+#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);}}
+
+rcl_publisher_t publisher_odometry;
+rcl_publisher_t publisher_attitude;
 
 static int pitchid, rollid, yawid;
 static int Xid, Yid, Zid;
 
-float sign(float x){   
+float sign(float x){
     return (x >= 0) ? 1.0 : -1.0;
 }
 
-void appMain(){ 
+void appMain(){
     absoluteUsedMemory = 0;
     usedMemory = 0;
 
@@ -41,73 +44,38 @@ void appMain(){
     DEBUG_PRINT("Radio connected\n");
 
     // ####################### MICROROS INIT #######################
-
     DEBUG_PRINT("Free heap pre uROS: %d bytes\n", xPortGetFreeHeapSize());
     vTaskDelay(50);
 
-    rcl_context_t      context;
-    rcl_init_options_t init_options;
-    rcl_ret_t          rc;
-    init_options = rcl_get_zero_initialized_init_options();
-
     rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();
     freeRTOS_allocator.allocate = __crazyflie_allocate;
     freeRTOS_allocator.deallocate = __crazyflie_deallocate;
     freeRTOS_allocator.reallocate = __crazyflie_reallocate;
     freeRTOS_allocator.zero_allocate = __crazyflie_zero_allocate;
 
     if (!rcutils_set_default_allocator(&freeRTOS_allocator)) {
-        DEBUG_PRINT("Error on default allocators (line %d)\n",__LINE__); 
+        DEBUG_PRINT("Error on default allocators (line %d)\n",__LINE__);
         vTaskSuspend( NULL );
     }
 
-    rc = rcl_init_options_init(&init_options, rcutils_get_default_allocator());
-    RCCHECK()
-
-    context = rcl_get_zero_initialized_context();
-
-    rc = rcl_init(0, NULL, &init_options, &context);
-    RCCHECK()
-
-    rc = rcl_init_options_fini(&init_options);
-
-    rcl_node_t node = rcl_get_zero_initialized_node();
-    rcl_node_options_t node_ops = rcl_node_get_default_options();
-
-    rc = rcl_node_init(&node, "crazyflie_node", "", &context, &node_ops);
-    RCCHECK()
+    rcl_allocator_t allocator = rcl_get_default_allocator();
+	rclc_support_t support;
 
-    // Create publisher 1
-    const char* drone_odom = "/drone/odometry";
+	// create init_options
+	RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
 
-    rcl_publisher_t pub_odom        = rcl_get_zero_initialized_publisher();
-    const rosidl_message_type_support_t * pub_type_support_odom = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Point32);
-    rcl_publisher_options_t pub_opt_odom = rcl_publisher_get_default_options();
+	// create node
+	rcl_node_t node = rcl_get_zero_initialized_node();
+	RCCHECK(rclc_node_init_default(&node, "crazyflie_node", "", &support));
 
-    rc = rcl_publisher_init(
-        &pub_odom,
-        &node,
-        pub_type_support_odom,
-        drone_odom,
-        &pub_opt_odom);
-    RCCHECK()
+	// create publishers
+    // TODO (pablogs9): these publishers must be best effort
+	RCCHECK(rclc_publisher_init_default(&publisher_odometry, &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Point32), "/drone/odometry"));
+	RCCHECK(rclc_publisher_init_default(&publisher_attitude, &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Point32), "/drone/attitude"));
 
-    // Create publisher 2
-    const char* drone_attitude = "/drone/attitude";
-
-    rcl_publisher_t pub_attitude        = rcl_get_zero_initialized_publisher();
-    const rosidl_message_type_support_t * pub_type_support_att = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Point32);
-    rcl_publisher_options_t pub_opt_att = rcl_publisher_get_default_options();
-
-    rc = rcl_publisher_init(
-        &pub_attitude,
-        &node,
-        pub_type_support_att,
-        drone_attitude,
-        &pub_opt_att);
-    RCCHECK()
-
-    // Init messages 
+    // // Init messages
     geometry_msgs__msg__Point32 pose;
     geometry_msgs__msg__Point32__init(&pose);
     geometry_msgs__msg__Point32 odom;
@@ -127,25 +95,24 @@ void appMain(){
     DEBUG_PRINT("uROS Used Memory %d bytes\n", usedMemory);
     DEBUG_PRINT("uROS Absolute Used Memory %d bytes\n", absoluteUsedMemory);
 
-    // ####################### MAIN LOOP #######################
-
-    while(1){
-
+	while(1){
         pose.x     = logGetFloat(pitchid);
         pose.y     = logGetFloat(rollid);
         pose.z     = logGetFloat(yawid);
         odom.x     = logGetFloat(Xid);
         odom.y     = logGetFloat(Yid);
         odom.z     = logGetFloat(Zid);
 
-        rc = rcl_publish( &pub_attitude, (const void *) &pose, NULL);
-        RCSOFTCHECK()
+        RCSOFTCHECK(rcl_publish( &publisher_attitude, (const void *) &pose, NULL));
+
+        RCSOFTCHECK(rcl_publish( &publisher_odometry, (const void *) &odom, NULL));
 
-        rc = rcl_publish( &pub_odom, (const void *) &odom, NULL);
-        RCSOFTCHECK()
-
         vTaskDelay(10/portTICK_RATE_MS);
-    }
-
+	}
+
+	RCCHECK(rcl_publisher_fini(&publisher_attitude, &node))
+	RCCHECK(rcl_publisher_fini(&publisher_odometry, &node))
+	RCCHECK(rcl_node_fini(&node))
+
     vTaskSuspend( NULL );
 }