ROS 2 and micro-ROS with ESP32
What is micro-ROS?
- Micro-ROS brings ROS 2 to microcontrollers.
- It enables small devices (ESP32, STM32) to communicate with ROS 2 systems.
- Ideal for:
- Small robotics
- IoT devices
- Sensor networks
Micro-ROS vs ROS 2
- ROS 2: Runs on computers with full Linux or Windows systems.
- micro-ROS: Designed for microcontrollers with limited resources.
- Allows communication between small, resource-constrained devices and full ROS 2 systems.
ESP32 with micro-ROS
Pre-requisites:
- ROS 2 installed.
- ESP-IDF for ESP32.
- micro-ROS-Agent set up to bridge ROS 2 and micro-ROS nodes.
ESP32 Example: Control Built-in LED
Firmware Overview
- Topic:
led_control
- Message type:
std_msgs/msg/Bool
true
: LED onfalse
: LED off
- Subscribes to ROS 2 messages to control the ESP32 built-in LED.
Overview
- ESP32 micro-ROS code to receive “on” or “off” from a ROS 2 topic
- Controls the built-in LED
- Flash using PlatformIO
Setting up the micro-ROS Agent
Install the micro-ROS Agent on your ROS 2 system:
sudo apt update sudo apt install ros-humble-micro-ros-agent ros2 run micro_ros_agent micro_ros_agent serial —dev /dev/ttyUSB0 -v6
PlatformIO Setup
- Install PlatformIO in Visual Studio Code
- Create a new PlatformIO project for ESP32
- Update the
platformio.ini
file:
[env:esp32dev]
platform = espressif32
board = esp32dev
framework = arduino
monitor_speed = 115200
lib_deps =
https://github.com/micro-ROS/micro_ros_platformio
build_flags =
-D UROS_SERIAL_TRANSPORT
upload_port = /dev/ttyUSB0
monitor_port = /dev/ttyUSB0
- Add the following code to
src/main.cpp
:
include <Arduino.h> #include <micro_ros_platformio.h>
include <rcl/rcl.h> #include <rclc/rclc.h> #include <rclc/executor.h>
include <std_msgs/msg/int32.h>
if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL) #error This example is only available for Arduino framework with serial transport. #endif
define LED_BUILTIN 33
---
```cpp
rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
// Error handle loop
void error_loop() {
while(1) {
delay(100);
}
}
// Callback for subscription
void subscription_callback(const void * msgin) {
const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
Serial.print("Received: ");
Serial.println(msg->data);
// Control the built-in LED
if (msg->data == 1) {
digitalWrite(LED_BUILTIN, HIGH); // Turn LED on
} else if (msg->data == 0) {
digitalWrite(LED_BUILTIN, LOW); // Turn LED off
}
}
void setup() {
// Configure serial transport
Serial.begin(115200);
set_microros_serial_transports(Serial);
delay(2000);
// Initialize built-in LED
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, LOW); // Ensure LED is off initially
allocator = rcl_get_default_allocator();
// Create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// Create node
RCCHECK(rclc_node_init_default(&node, "led_state_node", "", &support));
// Create subscriber
RCCHECK(rclc_subscription_init_default(
&subscriber,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"led_state"));
// Create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
msg.data = 0;
}
void loop() {
delay(100);
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}
Flashing the Code
- Connect the ESP32 to your PC
- In PlatformIO, click the “Upload” button, or run:
platformio run --target upload
Publish Messages from ROS 2
- build microros agent into your workspace https://github.com/micro-ROS/micro-ROS-Agent
cd src
git clone --branch humble https://github.com/micro-ROS/micro-ROS-Agent
git clone --branch humble https://github.com/micro-ROS/micro_ros_msgs
colcon build
source install/setup.bash
- Run the micro-ROS Agent
- Use ROS 2 to send messages:
orros2 topic pub /led_state std_msgs/msg/Int32 "{data: 0}"
ros2 topic pub /led_state std_msgs/msg/Int32 "{data: 1}"
Running micro-ROS-Agent
- Run the agent on your ROS 2 system to enable communication between ESP32 and ROS 2:
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0
Testing: Turn LED on/off
- Turn LED on:
ros2 topic pub /led_state std_msgs/msg/Int32 "{data: 1}"
- Turn LED off:
ros2 topic pub /led_state std_msgs/msg/Int32 "{data: 0}"