micro-ros + esp32 so slow

[markilius508]

I set up a micro-ROS system on an ESP32 to publish three messages simultaneously in a single timer callback with a 20 ms interval.

tf2_msgsmsgTFMessage sensor_msgsmsgJointState nav_msgsmsgOdometry

However, I noticed my motor controller wasn't responding as smoothly as it used to. When I measured the time taken by the timer callback, I was shocked to find it was using up 100 ms just to publish those three messages! For comparison, I tested the same task on my OpenCR board, and it only took 6 ms to complete. Can you help me figure out what's causing this issue?

• Coded with Arduino IDE
• No RTOS yet
• Reference example from micro_ros_arduino micro-ros_types_handling

#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <micro_ros_utilities/type_utilities.h>
#include <micro_ros_utilities/string_utilities.h>

#include <control_msgs/msg/joint_jog.h>
#include <builtin_interfaces/msg/duration.h>
#include <nav_msgs/msg/odometry.h>
#include <geometry_msgs/msg/transform_stamped.h>
#include <tf2_msgs/msg/tf_message.h>
#include <sensor_msgs/msg/joint_state.h>

#define WHEEL_NUM 2
#define LEFT 0
#define RIGHT 1

rcl_publisher_t publisher;
control_msgs__msg__JointJog msg;
control_msgs__msg__JointJog msg_static;

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

// rcl_subscription_t cmd_vel_sub;
rcl_publisher_t odom_pub;
rcl_publisher_t tf_pub;
rcl_publisher_t joint_state_pub;
rcl_publisher_t dbg_pub;

rcl_timer_t odom_timer;
// geometry_msgs__msg__Twist cmd_vel_msg;
nav_msgs__msg__Odometry odom_msg;
tf2_msgs__msg__TFMessage odom_tf;
geometry_msgs__msg__TransformStamped odom_geo;
sensor_msgs__msg__JointState joint_state_msg;
// std_msgs__msg__Float64 dbg_msg;
// std_msgs__msg__UInt64 dbg_msg;
builtin_interfaces__msg__Duration dbg_msg;

uint8_t my_buffer[1000];

#define LED_PIN 13
extern "C" int clock_gettime(clockid_t unused, struct timespec *tp);

#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)){}}

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
        struct timespec time_stamp;
        clock_gettime(CLOCK_REALTIME, &time_stamp);

        odom_msg.header.stamp.sec = time_stamp.tv_sec;
        odom_msg.header.stamp.nanosec = time_stamp.tv_nsec;
        RCSOFTCHECK(rcl_publish(&odom_pub, &odom_msg, NULL));

        odom_geo.header.stamp.sec = odom_msg.header.stamp.sec;
        odom_geo.header.stamp.nanosec = odom_msg.header.stamp.nanosec;
        // copy_odomPose_to_tf(&odom_msg, &odom_geo);
        odom_tf.transforms.data = &odom_geo;
        RCSOFTCHECK(rcl_publish(&tf_pub, &odom_tf, NULL));

        joint_state_msg.header.stamp.sec = odom_msg.header.stamp.sec;
        joint_state_msg.header.stamp.nanosec = odom_msg.header.stamp.nanosec;
        for (uint8_t whl_idx = 0; whl_idx < WHEEL_NUM; whl_idx++) {
            joint_state_msg.position.data[whl_idx] = 5.0;
            joint_state_msg.velocity.data[whl_idx] = 5.0;
        }
        RCSOFTCHECK(rcl_publish(&joint_state_pub, &joint_state_msg, NULL));

        struct timespec time_stop;
        clock_gettime(CLOCK_REALTIME, &time_stop);
        dbg_msg.sec = time_stop.tv_sec - time_stamp.tv_sec;
        dbg_msg.nanosec = time_stop.tv_nsec - time_stamp.tv_nsec;
        RCSOFTCHECK(rcl_publish(&dbg_pub, &dbg_msg, NULL));
    }
}

void setup() {
  set_microros_transports();

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_platformio_node", "", &support));

  // create publisher
  rclc_publisher_init_default(
        &dbg_pub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(builtin_interfaces, msg, Duration),
        "dbg");

    rclc_publisher_init_default(
        &odom_pub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
        "odom_raw");

    rclc_publisher_init_default(
        &tf_pub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage),
        "/tf");

    rclc_publisher_init_default(
        &joint_state_pub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
        "/joint_states");

  // create timer,
  const unsigned int timer_timeout = 100;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

    // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  static micro_ros_utilities_memory_conf_t conf = {0};
    // conf.max_string_capacity = 20;
    // conf.max_ros2_type_sequence_capacity = 5;
    // conf.max_basic_type_sequence_capacity = 5;

    micro_ros_utilities_create_message_memory(
        ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
        &odom_msg,
        conf);

    micro_ros_utilities_memory_rule_t tf_msgs_rules[] = {
        {"transform", 1}};
    conf.rules = tf_msgs_rules;

    micro_ros_utilities_create_message_memory(
        ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage),
        &odom_tf,
        conf);

    micro_ros_utilities_memory_rule_t joint_state_rules[] = {
        {"name", WHEEL_NUM},
        {"position", WHEEL_NUM},
        {"velocity", WHEEL_NUM},
        {"effort", 0}};

    conf.rules = joint_state_rules;

    micro_ros_utilities_create_message_memory(
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
        &joint_state_msg,
        conf);

    micro_ros_string_utilities_set(odom_msg.header.frame_id, "odom_frame");
    micro_ros_string_utilities_set(odom_msg.child_frame_id, "base_footprint");

    odom_msg.pose.covariance[0] = 0.001;
    odom_msg.pose.covariance[7] = 0.001;
    odom_msg.pose.covariance[35] = 0.001;

    odom_msg.twist.covariance[0] = 0.0001;
    odom_msg.twist.covariance[7] = 0.0001;
    odom_msg.twist.covariance[35] = 0.0001;

    odom_geo.header.frame_id = odom_msg.header.frame_id;
    odom_geo.child_frame_id = odom_msg.child_frame_id;
    // copy_odomPose_to_tf(&odom_msg, &odom_geo);

    odom_tf.transforms.size = 1;
    odom_tf.transforms.data = &odom_geo;

    micro_ros_string_utilities_set(joint_state_msg.header.frame_id, "base_link");

    joint_state_msg.name.size = WHEEL_NUM;
    joint_state_msg.name.data[LEFT] = micro_ros_string_utilities_set(joint_state_msg.name.data[LEFT], "wheel_left_joint");
    joint_state_msg.name.data[RIGHT] = micro_ros_string_utilities_set(joint_state_msg.name.data[RIGHT], "wheel_right_joint");

    joint_state_msg.position.size = WHEEL_NUM;
    joint_state_msg.velocity.size = WHEEL_NUM;
    joint_state_msg.effort.size = 0;
}

void loop() {
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

[hippo5329]

You should set serial baudrate to 921600. Or , 2000000 if your esp32 serial bridge supports, eg ch340.

[markilius508]

You should set serial baudrate to 921600. Or , 2000000 if your esp32 serial bridge supports, eg ch340.

Thank you for your reply. I tried to change baudrate to 2000000 but somehow the maximum achievable rate appeared to be limited to 921600 (second last line in the Terminal Output).

Despite this adjustment, I observed no significant improvement in micro-ROS performance; the time needed for publishing those three messages remained unchanged.

[hippo5329]

Your timer is 100ms, eq, 10Hz // create timer, const unsigned int timer_timeout = 100;

You can check the rate with "ros2 topic hz /your_topic" .

[hippo5329]

The maximum topic rate of int32 publisher is around 250Hz on esp32. I use 50Hz for control loop. You should also use low latency kernel on host.

https://github.com/hippo5329/micro_ros_arduino_examples_platformio/wiki

[markilius508]

@hippo5329 You are absolutely right, 10Hz is too slow. Increasing the publishing frequency and using a low latency kernel on the host are both on my to-do list. Thank you for your recommendation and sharing your repo - I'll definitely check it out.

[TheHassanShahzad]

Im also running through a similar issue. Im publishing 2 int64 messages of a custom data type and im trying to send them at 20Hz with a timer callback period of 20ms

When i do ros2 topic echo /topic_name, the data is being published at around 1-1.5Hz

Could it be that Int64 generally takes that long to publish?

I also tried uploading an example micro ros publisher and changed the timer callback to 20ms and removed the 100ms delay in the main loop. still no change. the data is being displayed like this:

https://asciinema.org/a/D1Z1pUeNKVQrnGEfAYVl7WP9p