Serial number (#26)

Reading the CPU ID from the `/get_cpu_id` service

.github/workflows/firmware_build.yml

@@ -5,13 +5,20 @@ on:
 jobs:
   firmware-build:
     name: Firmware build
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-22.04
     steps:
     - name: Checkout
       uses: actions/checkout@v2
+      with:
+        submodules: true
+    # - name: Installing platformio
+    #   run: pip3 install -U platformio
 
-    - name: Installing platformio
-      run: pip3 install -U platformio
+    # - name: Building a firmware for STM32
+    #   run: git submodule update --init --recursive && pio run
 
     - name: Building a firmware for STM32
-      run: git submodule update --init --recursive && pio run
+      run: |
+        set -e
+        docker compose build --no-cache
+        docker compose up --exit-code-from pio

.github/workflows/firmware_release.yml

@@ -49,16 +49,24 @@ jobs:
   firmware-release:
     name: Firmware release
     needs: bump-version
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-22.04
     steps:
     - name: Checkout
-      uses: actions/checkout@v2
+      uses: actions/checkout@v4
+      with:
+        submodules: true
+
+    # - name: Installing platformio
+    #   run: pip3 install -U platformio
 
-    - name: Installing platformio
-      run: pip3 install -U platformio
+    # - name: Building a firmware for STM32
+    #   run: git submodule update --init --recursive && pio run
 
     - name: Building a firmware for STM32
-      run: git submodule update --init --recursive && pio run
+      run: |
+        set -e
+        docker compose build --no-cache 
+        docker compose up --exit-code-from pio
 
     - name: Release a firmware
       uses: softprops/action-gh-release@v1

.mbedignore

@@ -1,9 +1,9 @@
-cellular/*
-cryptocell/*
-deprecated_warnings/*
-lorawan/*
-lwipstack/*
-nanostack/*
-netsocket/*
-nfc/*
-unsupported/*
+**/cellular/*
+**/cryptocell/*
+**/deprecated_warnings/*
+**/lorawan/*
+**/lwipstack/*
+**/nanostack/*
+**/netsocket/*
+**/nfc/*
+**/unsupported/*

Dockerfile

@@ -0,0 +1,30 @@
+FROM ubuntu:22.04
+
+RUN apt update && apt install -y \
+        python3 \
+        python3-pip \
+        python3.10-venv \
+        git \
+        cmake
+
+RUN pip3 install --upgrade pip setuptools
+
+RUN pip3 install -U platformio
+
+WORKDIR /app
+
+COPY . .
+
+ENV USER=root
+
+RUN git config --global --add safe.directory /app && \
+    git config --global --add safe.directory /app/lib/encoder-mbed && \
+    git config --global --add safe.directory /app/lib/imu-driver && \
+    git config --global --add safe.directory /app/lib/motor-driver-mbed && \
+    git config --global --add safe.directory /app/lib/vl53l0x-mbed && \
+    git submodule update --init --recursive && \
+    pio lib install
+
+RUN python3 -m venv /root/.platformio/penv/bin/activate
+
+CMD pio run

colcon.meta

@@ -4,14 +4,14 @@
             "cmake-args": [
                 "-DRMW_UXRCE_MAX_NODES=1",
                 "-DRMW_UXRCE_MAX_PUBLISHERS=11",
-                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=4",
-                "-DRMW_UXRCE_MAX_SERVICES=5",
-                "-DRMW_UXRCE_MAX_CLIENTS=0",
+                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=6",
+                "-DRMW_UXRCE_MAX_SERVICES=6",
+                "-DRMW_UXRCE_MAX_CLIENTS=1",
                 "-DRMW_UXRCE_MAX_HISTORY=4",
                 "-DRMW_UXRCE_TRANSPORT=custom",
                 "-DRMW_UXRCE_ENTITY_CREATION_TIMEOUT=100",
                 "-DRMW_UXRCE_ENTITY_DESTROY_TIMEOUT=0"
             ]
         }
     }
-}
+}

compose.yaml

@@ -0,0 +1,6 @@
+services:
+  pio:
+    build: .
+    volumes:
+      - .:/app
+    # command: tail -f /dev/null

include/microros.hpp

@@ -27,6 +27,8 @@ constexpr const char *WHEELS_COMMAND_TOPIC_NAME = "_motors_cmd";
 constexpr const char *BATTERY_TOPIC_NAME = "battery";
 constexpr const char *SERVOS_COMMAND_TOPIC_NAME = "cmd_ser";
 
+constexpr const char *GET_CPU_ID_SERVICE_NAME = "/get_cpu_id";
+
 enum AgentStates {
     WAITING_AGENT,
     AGENT_AVAILABLE,
@@ -67,9 +69,12 @@ bool init_button_publishers();
 bool init_led_subscribers();
 bool init_param_server();
 bool init_parameters();
+bool init_services();
 
 bool publish_imu_msg(sensor_msgs__msg__Imu *imu_msg);
 bool publish_wheels_state_msg(sensor_msgs__msg__JointState *msg);
 bool publish_battery_msg(sensor_msgs__msg__BatteryState *msg);
 bool publish_range_msg(sensor_msgs__msg__Range *msg, uint8_t id);
 bool publish_button_msg(std_msgs__msg__Bool *msg, uint8_t id);
+
+void get_cpu_id_service_callback(const void *request, void *response);

platformio.ini

@@ -15,13 +15,13 @@ extra_scripts =
     post:gen_hex.py
     pre:ignore_packages.py
 
-board_microros_distro = humble
-board_microros_transport = custom
-board_microros_user_meta = colcon.meta
-
 lib_deps =
     https://github.com/husarion/micro_ros_platformio
 lib_compat_mode = off
 
+board_microros_distro = humble
+board_microros_transport = custom
+board_microros_user_meta = colcon.meta
+
 upload_protocol = stlink
 debug_tool = stlink

src/main.cpp

@@ -197,6 +197,7 @@ int main()
   ThisThread::sleep_for(100);
   sens_power = 1; // sensors power on
   ThisThread::sleep_for(100);
+
   init_battery();
   init_wheels();
   init_button_and_attach_to_callbacks(&button1, button1_rise_callback, button1_fall_callback);

src/microros.cpp

@@ -1,4 +1,5 @@
 #include <microros.hpp>
+#include <std_srvs/srv/trigger.h>
 
 rclc_executor_t executor;
 rclc_support_t support;
@@ -18,10 +19,15 @@ rcl_subscription_t wheels_command_sub;
 rcl_subscription_t servos_command_sub;
 rcl_subscription_t led_subs[LED_COUNT];
 
+rcl_service_t get_cpu_id_service;
+
 rclc_parameter_server_t param_server;
 
 std_msgs__msg__Bool led_msg;
 
+std_srvs__srv__Trigger_Request get_cpu_id_service_request;
+std_srvs__srv__Trigger_Response get_cpu_id_service_response;
+
 const char *range_pub_names[] = {"range/fr", "range/fl", "range/rr", "range/rl"};
 const char *buttons_pub_names[] = {"button/left", "button/right"};
 const char *led_subs_names[] = {"led/left", "led/right"};
@@ -42,15 +48,16 @@ bool microros_init() {
     RCCHECK(rclc_node_init_default(&node, NODE_NAME, "", &support));
 
     if (not init_wheels_command_subscriber() or
-        not init_servos_command_subscriber() or
         not init_wheels_state_publisher() or
+        not init_servos_command_subscriber() or
         not init_imu_publisher() or
         not init_battery_publisher() or
         not init_range_publishers() or
         not init_button_publishers() or
         not init_led_subscribers() or
         not init_param_server() or
-        not init_parameters()) {
+        not init_parameters() or
+        not init_services()) {
         return false;
     }
 
@@ -59,7 +66,7 @@ bool microros_init() {
     RCCHECK(rclc_timer_init_default(&range_timer, &support, RCL_MS_TO_NS( 200  ),
                                     publish_range_sensors));
 
-    RCCHECK(rclc_executor_init(&executor, &support.context, 11, &rcl_allocator));
+    RCCHECK(rclc_executor_init(&executor, &support.context, 12, &rcl_allocator));
     RCCHECK(rclc_executor_add_timer(&executor, &timer));
     RCCHECK(rclc_executor_add_timer(&executor, &range_timer));
 
@@ -72,6 +79,7 @@ bool microros_init() {
     RCCHECK(rclc_executor_add_subscription(&executor, &led_subs[1], &led_msg,
                                            &led2_callback, ON_NEW_DATA));
     RCCHECK(rclc_executor_add_parameter_server(&executor, &param_server, on_parameter_changed));
+    RCCHECK(rclc_executor_add_service(&executor, &get_cpu_id_service, &get_cpu_id_service_request, &get_cpu_id_service_response, get_cpu_id_service_callback)); 
 
     RCCHECK(rclc_executor_prepare(&executor));
     RCCHECK(rmw_uros_sync_session(1000));
@@ -97,6 +105,7 @@ bool microros_deinit() {
         RCCHECK(rcl_publisher_fini(&buttons_pubs[i], &node));
     }
     RCCHECK(rclc_parameter_server_fini(&param_server, &node));
+
     RCCHECK(rclc_executor_fini(&executor));
     RCCHECK(rcl_node_fini(&node));
     return true;
@@ -208,6 +217,20 @@ bool init_parameters(){
     return true;
 }
 
+bool init_services() {
+    std_srvs__srv__Trigger_Request__init(&get_cpu_id_service_request);
+    std_srvs__srv__Trigger_Response__init(&get_cpu_id_service_response);
+
+    RCCHECK(rclc_service_init_default(
+        &get_cpu_id_service,
+        &node,
+        ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger),
+        GET_CPU_ID_SERVICE_NAME
+    ));
+
+    return true;
+}
+
 bool publish_wheels_state_msg(sensor_msgs__msg__JointState *msg) {
     RCCHECK(rcl_publish(&wheels_state_pub, msg, NULL));
     return true;
@@ -231,4 +254,31 @@ bool publish_range_msg(sensor_msgs__msg__Range *msg, uint8_t id) {
 bool publish_button_msg(std_msgs__msg__Bool *msg, uint8_t id) {
     RCCHECK(rcl_publish(&buttons_pubs[id], msg, NULL));
     return true;
-}
+}
+
+void get_cpu_id_service_callback(const void *req, void *res) {
+    (void)req; // Unused parameter
+
+    const uint32_t ADDRESS = 0x1FFF7A10;
+    const uint8_t NUM_BYTES = 12;
+    uint8_t buffer[NUM_BYTES];
+    memcpy(buffer, (void *)ADDRESS, NUM_BYTES);
+
+    // Prepare the CPU ID in hexadecimal format
+    char cpu_id_buffer[NUM_BYTES * 2 + 1] = {0};
+    char *hex_ptr = cpu_id_buffer;
+    for (uint8_t i = 0; i < NUM_BYTES; ++i) {
+        snprintf(hex_ptr, 3, "%02X", buffer[i]);
+        hex_ptr += 2;
+    }
+
+    // Prepare the final output buffer with "CPU ID: " prefix
+    static char out_buffer[100]; // Ensure this is large enough
+    snprintf(out_buffer, sizeof(out_buffer), "{\"cpu_id\": \"%s\"}", cpu_id_buffer);
+
+    // Set the response
+    std_srvs__srv__Trigger_Response *response = (std_srvs__srv__Trigger_Response *)res;
+    response->success = true;
+    response->message.data = out_buffer;
+    response->message.size = strlen(out_buffer);
+}