Subscribing to the Speed Control Topic

1. Experiment Objective

In this experiment, you will learn how to subscribe to a speed control topic (cmd_vel) of type geometry_msgs/msg/Twist through micro-ROS and drive the motors of the MicroROS-V2 control board to implement motion control of the robot car. Through this example, you will master the receiving and parsing of the ROS2 standard motion control message, the integration of motor drivers, and the design method of a timeout automatic stop safety mechanism. This example subscribes to the /cmd_vel topic and controls the car's forward, backward, turning and in-place rotation based on linear velocity and angular velocity.

2. Experiment Principle

1. Keyword Explanation

2. Technical Architecture

This example adopts the ESP32 → micro-ROS → Agent → ROS2 host communication chain. The control board connects via Wi-Fi to the ROS2 host running the micro-ROS Agent and subscribes to the /cmd_vel topic to obtain speed control commands.

In terms of software architecture, the program adopts a multi-task separation + timeout protection design:

• micro_ros_task is responsible for ROS2 communication (subscribing to cmd_vel)

• motion_worker_task is responsible for motor control and timeout detection (20 ms period)

• The two are decoupled through a FreeRTOS queue and do not block each other

3. Core Topic Quick Reference

Firmware Downlink (Control Commands)

Timeout safety: If no new cmd_vel is received within 1500 ms, the car stops automatically.

Usage Example:

# The car moves forward at 0.3 m/s
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.3}, angular: {z: 0.0}}"
# The car stops
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0}, angular: {z: 0.0}}"

4. Middleware Configuration

micro-ROS uses a static memory model (RCUTILS_AVOID_DYNAMIC_ALLOCATION=ON), all entity memory is pre-allocated at compile time, and runtime dynamic creation is not supported.

Configuration file: ~/esp/extra_components/micro_ros_espidf_component/colcon.meta

| RMW_UXRCE_TRANSPORT | udp | UDP transport (Wi-Fi communication) | | RMW_UXRCE_MAX_PUBLISHERS | 6 | Maximum number of publishers | | RMW_UXRCE_MAX_SUBSCRIPTIONS | 6 | Maximum number of subscribers | | RMW_UXRCE_MAX_NODES | 1 | Maximum number of nodes | | RMW_UXRCE_MAX_SERVICES | 0 | Services not supported | | RMW_UXRCE_MAX_CLIENTS | 0 | Clients not supported | | RMW_UXRCE_MAX_HISTORY | 1 | History depth = 1, no message queuing | | UCLIENT_UDP_TRANSPORT_MTU | 4096 | Maximum UDP packet size 4 KB | | UCLIENT_MIN_HEARTBEAT_TIME_INTERVAL | 1 | Agent heartbeat interval 1 ms |

Key Constraints:

• MAX_HISTORY=1 means there is no message buffer queue, so uplink topics (firmware → host) uniformly use best_effort QoS

• MAX_SERVICES=0, MAX_CLIENTS=0 means only topic communication is supported, not Service/Client

• After modifying colcon.meta, the micro-ROS library must be recompiled for the changes to take effect: run idf.py clean-microros in the project directory to clean and rebuild the micro-ROS library, then idf.py build to compile the firmware

3. Experiment Steps

1. Hardware Preparation

2. Prerequisites

1. The ESP32-IDF development environment has been set up

2. The MicroROS-V2 control board is connected to the computer via USB, and the motors are correctly connected

3. Configure the Wi-Fi SSID, password, Agent IP and port via idf.py menuconfig (the micro_ros menu item)

   xxxxxxxxxx
   cd ~/esp/microROS_samples/twist_subscriber
   source ~/esp/esp-idf/export.sh
   idf.py menuconfig
   

   

   4. Select example-app setting to set the domain ID to be consistent with the virtual machine

   

   After configuration, press "s" to save and press "q" to exit the configuration interface.

3. Start micro_ros_agent (Terminal 1)

Start the micro-ROS Agent on the ROS2 host:

xxxxxxxxxx
cd ~/uros_ws
source install/local_setup.bash
ros2 run micro_ros_agent micro_ros_agent udp4 --port 8090 -v4

4. Build and Flash (Terminal 2)

​x

cd ~/esp/microROS_samples/twist_subscriber
source ~/esp/esp-idf/export.sh
idf.py build flash monitor

5. How to Stop

1. First send a stop command to ensure the car is stationary: ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0}, angular: {z: 0.0}}" -1

2. Press Ctrl+] in Terminal 2 to exit the serial monitor (stop firmware execution)

3. Press Ctrl+C in Terminal 1 to stop micro_ros_agent

Safety tip: Timeout automatic stop (1500 ms) is a backup mechanism; proactively sending a stop command is the safest way.

4. Experiment Result

After the build and flash succeed, publish speed control commands on the ROS2 host:

xxxxxxxxxx
# The car moves forward at 0.3 m/s
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.3}, angular: {z: 0.0}}"

# The car rotates in place to the left at 0.5 rad/s
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0}, angular: {z: 0.5}}"

# The car stops
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0}, angular: {z: 0.0}}"

Motion mode comparison table:

The control board serial port will output:

xxxxxxxxxx
I (xxx) MAIN: Agent reachable: 192.168.x.x:8888
I (xxx) MAIN: micro-ROS cmd_vel subscriber init done
I (xxx) MAIN: cmd_vel: vx=0.300, wz=0.000

When no command is sent for more than 1.5 seconds:

xxxxxxxxxx
W (xxx) MAIN: cmd_vel timeout: 1500 ms
W (xxx) MAIN: cmd_vel timeout , stop motion

5. Program Analysis

Source code paths:

• Firmware source code: ~/esp/microROS_samples/twist_subscriber/main/main.c

• Motion control: ~/esp/microROS_samples/twist_subscriber/components/car_motion/car_motion.h, car_motion.c

• Motor driver: ~/esp/microROS_samples/twist_subscriber/components/motor/motor.h, motor.c

1. Main Program Logic

main/main.c adopts a multi-task separation + timeout protection design:

1. In app_main():

   • Calls Motor_Init() to initialize the motor driver

   • Creates the FreeRTOS queue s_motion_queue (length 1, always keeps the latest command)

   • Creates the motion_worker_task task, which is responsible for motor control and timeout detection

   • Initializes the Wi-Fi network

   • Creates the micro_ros_task task

2. In micro_ros_task:

   • Creates the node twist_subscriber

   • Subscribes to the topic cmd_vel in Reliable mode, with message type geometry_msgs/msg/Twist

   • The subscription callback twist_callback forwards the speed command to the execution task through the queue

3. In motion_worker_task:

   • Checks for a new command every 20 ms

   • Calls Motion_Ctrl() to execute motion control

   • Continuously monitors command freshness: if no new command is received within 1500 ms, it stops the car automatically

2. Key Components

components/car_motion/car_motion.h — Motion control interface:

Parameter definition file: ~/esp/microROS_samples/twist_subscriber/components/car_motion/car_motion.h

components/car_motion/car_motion.c — Core motion control functions:

Differential kinematics model Motion_Ctrl:

Source file: ~/esp/microROS_samples/twist_subscriber/components/car_motion/car_motion.c

xxxxxxxxxx
void Motion_Ctrl(float V_x, float V_y, float V_z)
{
    line_v = limit_linear_speed(V_x);
    angular_v = limit_angular_speed(V_z);

    // Differential decomposition: left wheel speed = Vx - Vz × half width
    speed_L_setup = line_v - angular_v * ROBOT_HALF_WIDTH;
    // Right wheel speed = Vx + Vz × half width
    speed_R_setup = line_v + angular_v * ROBOT_HALF_WIDTH;

    Motor_Set_Speed(speed_L_setup, speed_R_setup);
}

The timeout automatic stop mechanism of motion_worker_task:

Source file: ~/esp/microROS_samples/twist_subscriber/main/main.c

xxxxxxxxxx
#define CMD_VEL_TIMEOUT_MS         1500

static void motion_worker_task(void *arg)
{
    (void)arg;

    motion_cmd_t cmd = {0};
    int64_t last_cmd_us = 0;
    bool stopped_by_timeout = false;

    while (1) {
        /* Check every 20 ms whether there is a new command */
        if (xQueueReceive(s_motion_queue, &cmd, pdMS_TO_TICKS(20)) == pdTRUE) {
            last_cmd_us = cmd.stamp_us;
            stopped_by_timeout = false;

            Motion_Ctrl(cmd.linear_x, 0.0f, cmd.angular_z);

            ESP_LOGI(TAG, "cmd_vel: vx=%.3f, wz=%.3f",
                     (double)cmd.linear_x, (double)cmd.angular_z);
        }

        /* Timeout automatic stop to prevent the car from continuing to move after the host disconnects */
        if (!stopped_by_timeout && last_cmd_us > 0) {
            int64_t now_us = esp_timer_get_time();
            if ((now_us - last_cmd_us) > ((int64_t)CMD_VEL_TIMEOUT_MS * 1000)) {
                int64_t timeout_ms = (now_us - last_cmd_us) / 1000;
                ESP_LOGW(TAG, "cmd_vel timeout: %lld ms", timeout_ms);
                Motion_Ctrl(0.0f, 0.0f, 0.0f);
                stopped_by_timeout = true;
                ESP_LOGW(TAG, "cmd_vel timeout , stop motion");
            }
        }
    }
}

twist_callback subscription callback:

Source file: ~/esp/microROS_samples/twist_subscriber/main/main.c

xxxxxxxxxx
static void twist_callback(const void *msgin)
{
    if (msgin == NULL || s_motion_queue == NULL) {
        return;
    }

    const geometry_msgs__msg__Twist *msg =
        (const geometry_msgs__msg__Twist *)msgin;

    motion_cmd_t cmd = {
        .linear_x = (float)msg->linear.x,
        .angular_z = (float)msg->angular.z,
        .stamp_us = esp_timer_get_time(),
    };

    /* Queue length is 1, always overwrites with the latest command */
    (void)xQueueOverwrite(s_motion_queue, &cmd);
}

components/motor/motor.h — Key parameters of the motor driver interface:

Parameter definition file: ~/esp/microROS_samples/twist_subscriber/components/motor/motor.h

6. Common Problems

Safety tip: The timeout automatic stop mechanism (1500 ms) ensures that the car can stop automatically when the host disconnects or the program malfunctions. This is a necessary safety design for mobile robot control. Please stay alert during testing and be ready to cut off power manually at any time.