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_msgs__msg__TFMessage sensor_msgs__msg__JointState nav_msgs__msg__Odometry

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)));
}