Clock only increments at 1Hz

[amsjntz]

I'm running this simple example using an rcl timer on an ESP32S3, expecting the timer to fire at 500Hz:

#include <Arduino.h>
#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

// ---- WiFi / agent config ----
const char* wifi_ssid     = "SSID";
const char* wifi_password = "PASSWORD";
const char* agent_ip      = "HOST";
const size_t agent_port   = 8888;

rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rclc_executor_t executor;
rcl_timer_t test_timer;

// ---- rate measurement ----
#define RATE_WINDOW 10
unsigned long call_times[RATE_WINDOW];
size_t call_idx = 0;
size_t call_count = 0;

static void timer_callback(rcl_timer_t* timer, int64_t last_call_time) {
    (void) last_call_time;
    if (timer == NULL) return;

    unsigned long now = micros();
    call_times[call_idx] = now;
    call_idx = (call_idx + 1) % RATE_WINDOW;
    call_count++;

    // once we've filled the window at least once, report average rate
    if (call_count % RATE_WINDOW == 0) {
        // oldest sample in the (now full) circular buffer is at call_idx
        unsigned long oldest = call_times[call_idx];
        unsigned long newest = call_times[(call_idx + RATE_WINDOW - 1) % RATE_WINDOW];
        float elapsed_s = (newest - oldest) / 1000000.0f;
        float rate_hz = (RATE_WINDOW - 1) / elapsed_s;
        Serial.printf("timer fired %u times total, rate: %.2f Hz\n", (unsigned) call_count, rate_hz);
    }
}

void setup() {
    Serial.begin(115200);

    set_microros_wifi_transports((char*) wifi_ssid, (char*) wifi_password, (char*) agent_ip, agent_port);

    rmw_ret_t ping_result = rmw_uros_ping_agent(500, 5);
    if (ping_result != RMW_RET_OK) {
        Serial.println("Agent not reachable :(");
    }

    allocator = rcl_get_default_allocator();
    rclc_support_init(&support, 0, NULL, &allocator);

    node = rcl_get_zero_initialized_node();
    rclc_node_init_default(&node, "rate_test_node", "", &support);

    // 500Hz timer = 2ms period
    test_timer = rcl_get_zero_initialized_timer();
    rclc_timer_init_default2(&test_timer, &support, RCL_MS_TO_NS(2), timer_callback, true);

    executor = rclc_executor_get_zero_initialized_executor();
    rclc_executor_init(&executor, &support.context, 1, &allocator);
    rclc_executor_add_timer(&executor, &test_timer);

    Serial.println("setup complete, timer running at 500Hz");
}

void loop() {
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1));
    // rcl_time_point_value_t now;
    // rcl_clock_get_now(&support.clock, &now);
    // Serial.printf("clock now: %lld\n", now);
}

This is what my code outputs though:

timer fired 10 times total, rate: 1.00 Hz
timer fired 20 times total, rate: 1.00 Hz
...

confirming that the timer is only running at 1Hz. Further, when printing the result of rcl_clock_get_now() in my loop, I do get very frequent serial output, but the nanosecond value only increases in 1 second steps, making me believe the root of the issue must lie in the way micro-ROS gets its clock values.

I have tried recompiling the library with -DRCUTILS_NO_64_ATOMIC=OFF with no luck.

Is there an issue with my code, or does micro-ROS actually have some kind of issue with its internal clock on the ESP32?

[amsjntz]

This clock resolution issue definetly also appears when using rcl_clock_get_now():

rcl_clock_t test_clock;
rcl_clock_init(RCL_STEADY_TIME, &test_clock, &allocator);
rcl_time_point_value_t t1, t2;
rcl_clock_get_now(&test_clock, &t1);
delay(500);
rcl_clock_get_now(&test_clock, &t2);
Serial.printf("t1: %lld, t2: %lld, diff: %lld ns (expected ~500000000)\n", t1, t2, t2 - t1);

This outputs either diff: 1000000000 ns or diff: 0 ns