IMU Data Publishing Rate too Small

[mich1342]

Issue template

• Hardware description: ESP32 Dev Module via Arduino IDE
• RTOS:
• Installation type: micro_ros_setup
• Version or commit hash: humble

Steps to reproduce the issue

I attach GY-85 IMU module to ESP32 and try to publish the IMU data using I2D Dev Library.

#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 <rmw_microros/rmw_microros.h>

#include <std_msgs/msg/int32.h>
#include <sensor_msgs/msg/imu.h>

#include <Wire.h>
#include "I2Cdev.h"
#include "ADXL345.h"

ADXL345 accel;

const float g_to_accel_ = 9.81;
const float mgauss_to_utesla_ = 0.1;
const float utesla_to_tesla_ = 0.000001;

const float accel_cov_ = 0.00001;
const float gyro_cov_ = 0.00001;
const float accel_scale_ = 1 / 256.0;
const float gyro_scale_ = 1 / 14.375;
#define LED_PIN 13
#define RCCHECK(fn) \
  { \
    rcl_ret_t temp_rc = fn; \
    if ((temp_rc != RCL_RET_OK)) { return false; } \
  }
#define EXECUTE_EVERY_N_MS(MS, X) \
  do { \
    static volatile int64_t init = -1; \
    if (init == -1) { init = uxr_millis(); } \
    if (uxr_millis() - init > MS) { \
      X; \
      init = uxr_millis(); \
    } \
  } while (0)

rclc_support_t support;
rcl_node_t node;
rcl_timer_t timer;
rcl_timer_t timer_scanning;
rclc_executor_t executor;
rcl_allocator_t allocator;
rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;

rcl_publisher_t imuPublisher;
sensor_msgs__msg__Imu imuMsg;

bool micro_ros_init_successful;
TaskHandle_t Task1;

enum states {
  WAITING_AGENT,
  AGENT_AVAILABLE,
  AGENT_CONNECTED,
  AGENT_DISCONNECTED
} state;

void timer_callback(rcl_timer_t *timer, int64_t last_call_time) {
  (void)last_call_time;
  if (timer != NULL) {
    rcl_publish(&publisher, &msg, NULL);
    msg.data++;
    imu__update();
    rcl_publish(&imuPublisher, &imuMsg, NULL);
  }
}

// Functions create_entities and destroy_entities can take several seconds.
// In order to reduce this rebuild the library with
// - RMW_UXRCE_ENTITY_CREATION_DESTROY_TIMEOUT=0
// - UCLIENT_MAX_SESSION_CONNECTION_ATTEMPTS=3

bool create_entities() {
  allocator = rcl_get_default_allocator();

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

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

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "std_msgs_msg_Int32"));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &imuPublisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
    "imu_msg"));

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

  // RCCHECK(rclc_timer_init_default(
  //   &timer_scanning,
  //   &support,
  //   RCL_MS_TO_NS(timer_timeout),
  //   timer_scanning_callback));

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

  return true;
}

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

  rcl_publisher_fini(&publisher, &node);
  rcl_publisher_fini(&imuPublisher, &node);
  rcl_timer_fini(&timer);
  rcl_timer_fini(&timer_scanning);
  rclc_executor_fini(&executor);
  rcl_node_fini(&node);
  rclc_support_fini(&support);
}

void imu__update() {
  int16_t ax, ay, az;
  accel.getAcceleration(&ax, &ay, &az);

  double accelX = ax * (double)accel_scale_ * g_to_accel_;
  double accelY = ay * (double)accel_scale_ * g_to_accel_;
  double accelZ = az * (double)accel_scale_ * g_to_accel_;

  double gyro_cov = 0.00001;
  double accel_cov = 0.00001;
  double heading_cov = 0.00001;

  imuMsg.linear_acceleration.x = accelX;
  imuMsg.linear_acceleration.y = accelY;
  imuMsg.linear_acceleration.z = accelZ;
  imuMsg.linear_acceleration_covariance[0] = accel_cov;
  imuMsg.linear_acceleration_covariance[4] = accel_cov;
  imuMsg.linear_acceleration_covariance[8] = accel_cov;
}
void setup() {
  set_microros_transports();
  pinMode(LED_PIN, OUTPUT);

  state = WAITING_AGENT;
  Wire.begin();
  Wire.setClock(400000);
  accel.initialize();
  if (!accel.testConnection()) {  // change to your own address
    while (1) {
      ESP.restart();
    }
  }
  imuMsg.header.frame_id.data = (char *)"imu_link";
  msg.data = 0;
}

void loop() {
  switch (state) {
    case WAITING_AGENT:
      EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;);
      break;
    case AGENT_AVAILABLE:
      state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT;
      if (state == WAITING_AGENT) {
        destroy_entities();
      };
      break;
    case AGENT_CONNECTED:
      EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? 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;
    default:
      break;
  }

  if (state == AGENT_CONNECTED) {
    digitalWrite(LED_PIN, 1);
  } else {
    digitalWrite(LED_PIN, 0);
  }
}

Expected behavior

I expect to get 50Hz data rate (Checked using RQT Topic Monitor)

Actual behavior

I get only 25 Hz of data rate

Additional information

As addition, I have tried to publish empty IMU data (commenting all IMU code so that it is not reading anything) and just inputting constant hardcoded value. I still got 25 Hz of data rate only.

[pablogs9]

Does it increase if after reaching AGENT_CONNECTED you just run rclc_executor_spin_some without checking EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;); ?

[mich1342]

Hi @pablogs9

Thank you for your reply, will try on that. One thing that bothers me is if I publish only Int32 message it could reach the 50Hz publishing frequency. Will let you know soon after did a trial.

Best Regards

Michael

[mich1342]

Hi @pablogs9 ,

Just tried that and the publish frequency increase by a little ~0.7-1 Hz

[pablogs9]

What if you do not make a imu__update inside the timer?

[mich1342]

Hi @pablogs9 ,

I tried to call the imu__update() function at the end of my main loop and still got ~25 Hz frequency.

[pablogs9]

Is the timer callback being called at 50 Hz if you keep it empty of operations?

[mich1342]

Yes, If I remove all of the IMU / Odom related code and leave it as the auto reconnect examples publishing incremental integers, it runs at 50Hz.

[pablogs9]

Does it improves if you publish the IMU in best effort?

[mich1342]

It increased to ~28 Hz

[pablogs9]

How big is your payload?

[mich1342]

It's only a standard IMU message (sensor_msgs/msg/Imu). Not sure how to check the payload. The data looks like this:

header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: imu_link
orientation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
linear_acceleration:
  x: -0.03832031413912773
  y: 0.4981640838086605
  z: -9.848320733755827
linear_acceleration_covariance:
- 1.0e-05
- 0.0
- 0.0
- 0.0
- 1.0e-05
- 0.0
- 0.0
- 0.0
- 1.0e-05
---

[pablogs9]

So probably it is related to message size, since when you publish smaller messages you increase the rate.