4.2 micro‐ROS

• For information on how to install ROS on your computer, go here.
• For information on how to install microROS on a Teensy, go here.

micro-ROS (ROS 2) Skeleton for Teensy 4.0

This document provides a reference micro-ROS (ROS 2) skeleton for Teensy 4.0–based boards.
Because micro-ROS differs significantly from ROSserial in both syntax and execution model, this structure should be followed closely.

⚠️ Important
Please keep the functions and patterns shown here as intact as possible. They are critical for correct micro-ROS operation, agent discovery, and executor behavior.

This skeleton:

• Is based on the Power Board Teensy 4.0 implementation
• Has been adapted and validated against the latest Display Board code
• Should be used as best practice for any other micro-ROS subdivision using a Teensy 4.0

---

Key Design Notes

• Uses micro_ros_arduino with rclc
• Implements a ROS 2 agent connection state machine
• Demonstrates clean entity creation and teardown
• Uses an executor + timer model
• Publishes depth sensor data as std_msgs/msg/Float64

---

Reference Skeleton Code

#ifdef DISPLAY_H

#include "display_main.h"

#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"
#include "XPT2046_Touchscreen.h"
#include <Wire.h>
#include "MS5837.h"

// ---- micro-ROS includes (copied from Power Board) ----
#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>
#include <std_msgs/msg/float64.h>

// ---- Display pin definitions ----
#define TFT_DC     9
#define TFT_CS     10
#define TOUCH_CS   8
#define TOUCH_IRQ  2

// ---- Display and sensor objects ----
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
XPT2046_Touchscreen ts(TOUCH_CS);
MS5837 sensor;

// ---- ROS 2 publisher (depth sensor) ----
rcl_publisher_t sensors_depth_publisher;
std_msgs__msg__Float64 sensors_depth_msg;

// ---- micro-ROS core objects (copied from Power Board) ----
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

// ---- Helper macros ----
#define RCCHECK(fn) { rcl_ret_t rc = fn; if (rc != RCL_RET_OK) { error_loop(); } }
#define RCSOFTCHECK(fn) { rcl_ret_t rc = fn; if (rc != RCL_RET_OK) {} }

#define EXECUTE_EVERY_N_MS(MS, X) do {                 \
  static int64_t last = -1;                            \
  if (last == -1) last = uxr_millis();                 \
  if (uxr_millis() - last > MS) { X; last = uxr_millis(); } \
} while (0)

// ---- Agent connection state machine ----
enum states {
  WAITING_AGENT,
  AGENT_AVAILABLE,
  AGENT_CONNECTED,
  AGENT_DISCONNECTED
} state;

// ---- Error loop ----
void error_loop() {
  while (true) {
    delay(100);
  }
}

// ======================================================
// ROS 2 TIMER CALLBACK
// ======================================================
void timer_callback(rcl_timer_t *timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);

  if (timer != NULL) {
    sensor.read();
    sensors_depth_msg.data = sensor.depth();
    RCSOFTCHECK(
      rcl_publish(&sensors_depth_publisher, &sensors_depth_msg, NULL)
    );
  }
}

// ======================================================
// CREATE ROS ENTITIES
// ======================================================
bool create_entities() {
  allocator = rcl_get_default_allocator();

  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "display_node", "", &support));

  RCCHECK(
    rclc_publisher_init_default(
      &sensors_depth_publisher,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64),
      "/sensors/depth/z"
    )
  );

  const unsigned int timer_timeout = 1000; // 1 Hz
  RCCHECK(
    rclc_timer_init_default(
      &timer,
      &support,
      RCL_MS_TO_NS(timer_timeout),
      timer_callback
    )
  );

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

  return true;
}

// ======================================================
// DESTROY ROS ENTITIES
// ======================================================
void destroy_entities() {
  rmw_context_t *rmw_context =
    rcl_context_get_rmw_context(&support.context);
  rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);

  rcl_publisher_fini(&sensors_depth_publisher, &node);
  rcl_timer_fini(&timer);
  rclc_executor_fini(&executor);
  rcl_node_fini(&node);
  rclc_support_fini(&support);
}

// ======================================================
// DISPLAY FUNCTIONS
// ======================================================
void updateDisplay() {
  static unsigned long lastUpdate = 0;
  if (millis() - lastUpdate > 500) {
    lastUpdate = millis();

    tft.fillRect(50, 50, 200, 60, ILI9341_BLACK);
    tft.setCursor(50, 50);
    tft.setTextSize(2);
    tft.setTextColor(ILI9341_WHITE);
    tft.print("Depth: ");
    tft.print(sensor.depth());
    tft.print(" m");

    tft.setCursor(50, 80);
    tft.print("ROS2: ");
    switch (state) {
      case WAITING_AGENT:      tft.print("Waiting"); break;
      case AGENT_AVAILABLE:    tft.print("Available"); break;
      case AGENT_CONNECTED:    tft.print("Connected"); break;
      case AGENT_DISCONNECTED: tft.print("Disconnected"); break;
    }
  }
}

void handleTouch() {
  if (ts.touched()) {
    TS_Point p = ts.getPoint();
    tft.fillCircle(p.x, p.y, 5, ILI9341_RED);
    delay(100);
    tft.fillCircle(p.x, p.y, 5, ILI9341_BLACK);
  }
}

void initDisplay() {
  tft.begin();
  ts.begin();
  tft.setRotation(1);
  tft.fillScreen(ILI9341_BLACK);

  tft.setCursor(80, 10);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.print("Depth Sensor ROS2");
}

// ======================================================
// SETUP
// ======================================================
void display_setup() {
  Serial.begin(115200);
  delay(2000);

  initDisplay();

  Wire.begin();
  if (!sensor.init()) {
    return;
  }

  sensor.setModel(MS5837::MS5837_30BA);
  sensor.setFluidDensity(997);

  set_microros_transports();
  delay(2000);

  sensors_depth_msg.data = 0.0;
  state = WAITING_AGENT;
}

// ======================================================
// LOOP
// ======================================================
void display_loop() {
  handleTouch();
  updateDisplay();

  switch (state) {
    case WAITING_AGENT:
      EXECUTE_EVERY_N_MS(
        500,
        state = (rmw_uros_ping_agent(100, 1) == RMW_RET_OK)
                ? AGENT_AVAILABLE
                : WAITING_AGENT;
      );
      break;

    case AGENT_AVAILABLE:
      state = create_entities() ? AGENT_CONNECTED : WAITING_AGENT;
      if (state == WAITING_AGENT) {
        destroy_entities();
      }
      break;

    case AGENT_CONNECTED:
      EXECUTE_EVERY_N_MS(
        200,
        state = (rmw_uros_ping_agent(100, 1) == RMW_RET_OK)
                ? AGENT_CONNECTED
                : AGENT_DISCONNECTED;
      );
      if (state == AGENT_CONNECTED) {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
      }
      break;

    case AGENT_DISCONNECTED:
      destroy_entities();
      state = WAITING_AGENT;
      break;
  }

  delay(10);
}

#endif