No nodes or topics visible using ESP32 and WiFi transport

[n0sc3tipsum]

Issue template

• Hardware description: ESP32-Devkitc (WROOM 32D), Host computer running Ubuntu 22.04
• RTOS: No RTOS implementation
• Installation type: micro_ros_setup, rmw_microxrcedds, rosidl_typesupport_microxrcedds
• Version or commit hash: Humble

Steps to reproduce the issue

Install ROS2 humble as per the typical user guide. Install micro-ros-platformio Install micro_ros_setup as given on the repo

Install rmw_microxrcedds as well as the e-Prosima Micro-XRCE-DDS

Set the RMW_IMPLEMENTATION to rmw_microxrcedds Set DOMAIN_ID to 0 on host run the micro ros agent over udp with port 8888

Flash the following code onto ESP32 :

Main File

#include <Arduino.h>
#include "ROSComm.h"
#include "step.h"
#include <WiFi.h>
#include <IPAddress.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

ROSComm espRosAgent;
step left_motor = step(STEPPER_INTERVAL_US, STEPPER1_STEP_PIN, STEPPER1_DIR_PIN);
step right_motor = step(STEPPER_INTERVAL_US, STEPPER2_STEP_PIN, STEPPER2_DIR_PIN);
Adafruit_MPU6050 imu;

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    espRosAgent.PublishCallback(&left_motor, &right_motor, &imu);
}

void cmd_vel_sub_callback(const void *msgin)
{
    espRosAgent.CommandCallback(msgin);
}

void setup()
{
    Serial.begin(115200);
    delay(2000);
    WiFi.begin(espRosAgent._ssid, espRosAgent._pswd);

    while (WiFi.status() != WL_CONNECTED) 
    {
        delay(500);
        Serial.println("Connecting to WiFi..");
    }

    espRosAgent._agent_ip = IPAddress(192,168,1,100);
    espRosAgent._esp_ip = WiFi.localIP();
    Serial.print("Connected to WiFi with local IP : ");
    Serial.println(espRosAgent._esp_ip);

    espRosAgent.Init();
    const unsigned int timer_timeout = 1000;

    RCSOFTCHECK(rclc_timer_init_default(
        &espRosAgent.timer,
        &espRosAgent.support,
        RCL_MS_TO_NS(timer_timeout),
        timer_callback), 
        "Init Timer");

    RCSOFTCHECK(rclc_executor_add_subscription(
        &espRosAgent.executor, 
        &espRosAgent._cmd_vel_sub, &espRosAgent._cmd_vel_msg, 
        &cmd_vel_sub_callback, ON_NEW_DATA), 
        "Create cmd_vel Subscription");

    RCSOFTCHECK(rclc_executor_add_timer(&espRosAgent.executor, &espRosAgent.timer), 
                "Add Timer To Executor");
}

void loop()
{
    delay(50);
    rclc_executor_spin_some(&espRosAgent.executor, RCL_MS_TO_NS(100));
}

ROSComm Class

#include "ROSComm.h"

ROSComm::ROSComm()
{
    _agent_port = 8888;
    _ssid = "";
    _pswd = "";

}

void ROSComm::Init(IPAddress agent_ip, size_t agent_port)
{
    Serial.println("Setting micro-ROS wifi transports");

    set_microros_wifi_transports(_ssid, _pswd, _agent_ip, _agent_port);
    delay(2000);

    // Init with domain id = 0
    rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
    allocator = rcl_get_default_allocator();

    RCSOFTCHECK(rcl_init_options_init(
        &init_options, 
        allocator),
        "Init RCL Options");

    RCSOFTCHECK(rcl_init_options_set_domain_id(
        &init_options, 
        0), 
        "Init Domain ID 0");

    // Initialize rclc support object with custom options
    RCSOFTCHECK(rclc_support_init_with_options(
        &support, 
        0, 
        NULL, 
        &init_options, 
        &allocator), 
        "Init Support Object");

    // Init node with configured support object
    RCSOFTCHECK(rclc_node_init_default(
        &node, 
        "esp_node", "",
        &support), 
        "Init ESP Node");

    Serial.println("");
    CreatePublishers();
    Serial.println("");
    CreateSubscribers();
    Serial.println("");
    CreateMessages(&_imu_msg, _lwheel_state_msg, _rwheel_state_msg);

    Serial.println("Initialising Executor");
    unsigned int num_handles = 2;
    RCSOFTCHECK(rclc_executor_init(
        &executor, 
        &support.context, 
        num_handles, 
        &allocator), 
        "Init Executor");

    Serial.println("");
    Serial.println("---- Initialisation Complete ----");
    Serial.println("");

}

void ROSComm::CommandCallback(const void *cmd_vel_recv)
{
    _cmd_vel_msg = *(geometry_msgs__msg__Twist *) cmd_vel_recv;

    float linear_vel_setpoint = _cmd_vel_msg.linear.x;
    float angular_vel_setpoint = _cmd_vel_msg.angular.z;
    Serial.println("Got Cmd_Vel !");

    Serial.print("Linear Velocity Setpoint : ");
    Serial.println(linear_vel_setpoint);
    Serial.print("Angular Velocity Setpoint : ");
    Serial.println(angular_vel_setpoint);
    /*Control Integration*/
}

void ROSComm::CreatePublishers()
{
    Serial.println("Initialising Publishers...");

    _imu_pub = rcl_get_zero_initialized_publisher();
    _left_wheel_state_pub = rcl_get_zero_initialized_publisher();
    _right_wheel_state_pub = rcl_get_zero_initialized_publisher();

    RCSOFTCHECK(rclc_publisher_init_default(
        &_imu_pub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
        "esp/imu"), 
        "Init IMU Publisher");

    RCSOFTCHECK(rclc_publisher_init_default(
        &_batt_state_pub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, BatteryState),
        "sensor_msgs/BetteryState"),
        "Init Battery State Publisher");

    RCSOFTCHECK(rclc_publisher_init_default(
        &_left_wheel_state_pub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
        "esp/left_wheel_state"),
        "Init Left Wheel State Publisher");

    RCSOFTCHECK(rclc_publisher_init_default(
        &_right_wheel_state_pub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
        "esp/right_wheel_state"),
        "Init Right Wheel State Publisher");
}

void ROSComm::CreateSubscribers()
{
    Serial.println("Initializing Subscribers");

    RCSOFTCHECK(rclc_subscription_init_default(
        &_cmd_vel_sub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
        "/cmd_vel"),
        "Init cmd_vel Subsscriber");
}

void ROSComm::CreateMessages(sensor_msgs__msg__Imu &imu_data, sensor_msgs__msg__JointState &left_wheel_state, sensor_msgs__msg__JointState &right_wheel_state)
{
    Serial.println("Creating Messages");

    RCSOFTCHECK(!builtin_interfaces__msg__Time__init(&_time_stamp),
                "Init Time Stamp");

    RCSOFTCHECK(!geometry_msgs__msg__Twist__init(&_cmd_vel_msg),
                "Init cmd_vel Message");

    RCSOFTCHECK(!sensor_msgs__msg__Imu__init(&imu_data),
                "Init cmd_vel Messaage");
    RCSOFTCHECK(!(geometry_msgs__msg__Vector3__init(&imu_data.angular_velocity)),
                "Init Angular Vel Messaage");
    RCSOFTCHECK(!geometry_msgs__msg__Vector3__init(&imu_data.linear_acceleration),
                "Init Acceleration Messaage");

    //data for psd, rms are from data sheet
    float accel_psd = 400e-6;
    float accel_bw = 100;
    float accel_var = (std::pow(accel_psd, 2) * accel_bw) * std::pow(9.81, 2);
    float gyro_rms = 0.05 * (M_PI / 180.0);
    float gyro_var = std::pow(gyro_rms, 2);

    for (size_t i = 0; i < 9; ++i) 
    {
        imu_data.linear_acceleration_covariance[i] = (i % 4 == 0) ? accel_var : 0.0;
        imu_data.angular_velocity_covariance[i] = (i % 4 == 0) ? gyro_var : 0.0;
        imu_data.orientation_covariance[i] = -1.0; // -1 indicates no orientation estimate
    }

    const char *lwheel_name = "left_wheel_joint";

    //Setup the joint state msg
    RCSOFTCHECK(!sensor_msgs__msg__JointState__init(&left_wheel_state),
                "Init Left Wheel State Message");

    RCSOFTCHECK(!rosidl_runtime_c__double__Sequence__init(&left_wheel_state.position, 1),
                "Init Left Wheel Position Message");

    RCSOFTCHECK(!rosidl_runtime_c__double__Sequence__init(&left_wheel_state.velocity, 1),
                "Init Left Wheel Vel Message");

    left_wheel_state.position.size = 1;
    left_wheel_state.position.capacity = 1;

    left_wheel_state.velocity.size = 1;
    left_wheel_state.velocity.capacity = 1;

    RCSOFTCHECK(!rosidl_runtime_c__String__Sequence__init(&left_wheel_state.name, 1), "Init Left Wheel Name");
    if (!rosidl_runtime_c__String__assign(&left_wheel_state.name.data[0], lwheel_name)){Serial.println("ERROR: Joined assignment failed\n");}
    left_wheel_state.name.size=1;
    left_wheel_state.name.capacity=1;

// Same for right wheel

}
void ROSComm::PublishCallback(step *lmotor, step *rmotor, Adafruit_MPU6050 *imu)
{

    getData(_imu_msg, _lwheel_state_msg, _rwheel_state_msg, lmotor, rmotor, imu);

    RCSOFTCHECK(rcl_publish(&_left_wheel_state_pub, &_lwheel_state_msg, NULL), 
                "Publish Left Wheel State");

    RCSOFTCHECK(rcl_publish(&_right_wheel_state_pub, &_rwheel_state_msg, NULL), 
                "Publish Right Wheel State");

    RCSOFTCHECK(rcl_publish(&_imu_pub, &_imu_msg, NULL), 
                "Publish IMU Data");       
}

void ROSComm::getData(sensor_msgs__msg__Imu &imu_data, sensor_msgs__msg__JointState &left_wheel_state, sensor_msgs__msg__JointState &right_wheel_state, 
                      step *lmotor, step *rmotor, Adafruit_MPU6050 *imu)
{
    sensors_event_t a, g, temp;
    //imu->getEvent(&a, &g, &temp);

    int64_t now = esp_timer_get_time();
    _time_stamp.sec = now / 1000000;
    _time_stamp.nanosec = (now % 1000000) * 1000;

// init time stamp

    RCSOFTCHECK(!builtin_interfaces__msg__Time__copy(&_time_stamp, &imu_data.header.stamp),
    "Copy Time TO IMU Message");

// Load dummy data 

    imu_data.linear_acceleration.x = 0.02;//a.acceleration.x;
    imu_data.linear_acceleration.y = 0.01; //a.acceleration.y;
    imu_data.linear_acceleration.z = -9.81; //a.acceleration.z;

    imu_data.angular_velocity.x = 0.0; //g.gyro.x;
    imu_data.angular_velocity.y = 0.0; //g.gyro.y;
    imu_data.angular_velocity.z = 0.5; //g.gyro.z;

    float lmotor_pos = -1.0; //lmotor->getPositionRad();
    float lmotor_speed = -3.0; //lmotor->getSpeedRad();

    now = esp_timer_get_time();
    _time_stamp.sec = now / 1000000;
    _time_stamp.nanosec = (now % 1000000) * 1000;

    RCSOFTCHECK(!builtin_interfaces__msg__Time__copy(&_time_stamp, &left_wheel_state.header.stamp),
    "Copy Time TO Left Wheel State Message");

    left_wheel_state.position.data[0] = lmotor_pos;
    left_wheel_state.velocity.data[0] = lmotor_speed;

// Same code for right wheel
}

Expected behavior

Successful connection between micro-ROS agent and ESP32 ros2 topic list prints out topics relating to the right and left wheel

Actual behavior

Successful connection between micro-ROS agent and ESP32

Topics, Publishers and Data writers created

ESP Serial Confirms successful initialisation

Apparent Successful Data Exchange

At this point, when running ros2 topic list, the only visible topics are the typical rosout and parameter events.

I have been playing around with this for days, I have tried :

• Changing the domain id settings : removing domain id / setting domain id = 0
• Trying rmw_cyclonedds_cpp as the rmw implementation
• Tried rmw_microxrcedds as the rmw implementation
• Deleted and re-installed micro_ros_setup, rosidl_typsupport_microxrcedds, rmw_microxrcedds

By this point I have lost most hope and have no idea what the underlying issue is. I have searched online and most people were able to solve it by either using a common domain id or not using one at all.

I will love you with all my heart if you can help out <3

Additional information

I also noticed a weird thing happening when I restart the ROS2 daemon. Although this may be relevant to a different issue, maybe this could explain the missing nodes/topics :

1. Open terminal and run ros2 topic list ( with rmw_microxrcedds as the middleware)
   • Get the usual topics
2. Stop the daemon
3. Start the daemon
4. run ros2 topic list
   • Expected behaviour : same topics as before? (not sure of the actual expected behaviour)
   • Actual behaviour : Error message shown below

[hippo5329]

I run a lot of esp32 with wifi transport. Check my wiki. You may try out with a bare esp32 module.

[hippo5329]

esp32 with micro-ROS wifi transport

[n0sc3tipsum]

esp32 with micro-ROS wifi transport

I can get to the part where the esp32 connects to the Micro-ROS agent, it's just from that point on I can't seem to view any topics or nodes.

[hippo5329]

There is no need to do these. Just use the default will work.

X Install rmw_microxrcedds as well as the e-Prosima Micro-XRCE-DDS

X Set the RMW_IMPLEMENTATION to rmw_microxrcedds X Set DOMAIN_ID to 0 on host

X Changing the domain id settings : removing domain id / setting domain id = 0 X Trying rmw_cyclonedds_cpp as the rmw implementation X Tried rmw_microxrcedds as the rmw implementation X Deleted and re-installed micro_ros_setup, rosidl_typsupport_microxrcedds, rmw_microxrcedds

[hippo5329]

May I suggest try out my code to get a real sense of micro-ros connection test?

[n0sc3tipsum]

There is no need to do these. Just use the default will work.

X Install rmw_microxrcedds as well as the e-Prosima Micro-XRCE-DDS

X Set the RMW_IMPLEMENTATION to rmw_microxrcedds X Set DOMAIN_ID to 0 on host

X Changing the domain id settings : removing domain id / setting domain id = 0 X Trying rmw_cyclonedds_cpp as the rmw implementation X Tried rmw_microxrcedds as the rmw implementation X Deleted and re-installed micro_ros_setup, rosidl_typsupport_microxrcedds, rmw_microxrcedds

Oh ok, would I also need to build the firmware for the host?

[n0sc3tipsum]

May I suggest try out my code to get a real sense of micro-ros connection test?

Sure I will try this out

[hippo5329]

There are some useless codes in your program. The set_microros_wifi_transports will setup wifi. Those delay() are not needed. It seems you make it too complicated with class. It is better to keep the code clean. If you want to start from scratch. You may start with a publish int example with wifi transport.

[hippo5329]

cyclonedds does not work well with micro-ROS. Only basic publish/subscribe will work. But services and actions are not supported.

[hippo5329]

With some minor modification to platformio, this will be a good start.

https://github.com/micro-ROS/micro_ros_arduino/blob/jazzy/examples/micro-ros_publisher_wifi/micro-ros_publisher_wifi.ino

[n0sc3tipsum]

cyclonedds does not work well with micro-ROS. Only basic publish/subscribe will work. But services and actions are not supported.

Yeah I changed it to rmw_microxrcedds

[n0sc3tipsum]

There are some useless codes in your program. The set_microros_wifi_transports will setup wifi. Those delay() are not needed. It seems you make it too complicated with class. It is better to keep the code clean. If you want to start from scratch. You may start with a publish int example with wifi transport.

The reason I wanted to do it with a class is because I am trying to integrate this code with a control system which will eventually be running on the esp32, so I felt it would de-clutter the code. Im assuming have these functionalities within a class won't affect the actual performance.

[hippo5329]

You may skip the step of platfromio and linorobot2 installation, as you have installed micro_ros_setup.

git clone https://github.com/hippo5329/linorobot2_hardware.git

Modify the wifi keys and agent IP in config/custom/esp32_wifi_config.h

cd firmware
pio run -e esp32_wifi -t upload

Then it will try to connect to the agent.

[hippo5329]

Oh ok, would I also need to build the firmware for the host?

No.

[hippo5329]

Yeah I changed it to rmw_microxrcedds

No. We would just use the default, fastrstp.

[hippo5329]

The reason I wanted to do it with a class is because I am trying to integrate this code with a control system which will eventually be running on the esp32, so I felt it would de-clutter the code. Im assuming have these functionalities within a class won't affect the actual performance.

No really impact to performance.

It is the development process. For something new to you, it is better to start from basic. Build some, test some. Build some, test some. Step by step. Beside, sometime you will need to look into code of the upstream functions you called.

[n0sc3tipsum]

The reason I wanted to do it with a class is because I am trying to integrate this code with a control system which will eventually be running on the esp32, so I felt it would de-clutter the code. Im assuming have these functionalities within a class won't affect the actual performance.

No really impact to performance.

It is the development process. For something new to you, it is better to start from basic. Build some, test some. Build some, test some. Step by step. Beside, sometime you will need to look into code of the upstream functions you called.

Ah I see, I am kind of running tight on time for the project so I am trying to get it done ASAP, which is why I am trying to integrate it from the getgo

[hippo5329]

Just opposite, it proved to take more time when you wrote a lot and found it took more time to debug. :)

The better way is, find some working examples. Try out and modify to fit your purpose. Just my 2 cents.

[hippo5329]

With some minor modification to platformio, this will be a good start.

https://github.com/micro-ROS/micro_ros_arduino/blob/jazzy/examples/micro-ros_publisher_wifi/micro-ros_publisher_wifi.ino

This was where I started.

[n0sc3tipsum]

Just opposite, it proved to take more time when you wrote a lot and found it took more time to debug. :)

The better way is, find some working examples. Try out and modify to fit your purpose. Just my 2 cents.

Very true, I am currently setting up the project that you recommended.

[n0sc3tipsum]

esp32 with micro-ROS wifi transport

For the config of the esp32 with wifi, if I am not planning on using the LiDAR, I can just comment out its' config right?

[hippo5329]

I started from the publish int example. I built an esp32 wifi robot similar to yours. Later, I found the linorobto2 project, which has some interesting features. I ported my code to it. I added more devices and test programs.

I think it is easy to expand to fit your purpose. I have tested service and action server. It should work for bigger projects.

[hippo5329]

esp32 with micro-ROS wifi transport

For the config of the esp32 with wifi, if I am not planning on using the LiDAR, I can just comment out its' config right?

There is no need to change the other setting for the first connection test. Only change the wifi key and agent IP. You will change the other setting after the connection test success.

[hippo5329]

I wrote a workflow in my wiki. These are the steps I used to build new robots.

[hippo5329]

The very first step starts with a single bare esp32 module before wiring and building the robot. Try connect it to micro-ros. Make sure all the topics echoed and Hz rates are correct.

[hippo5329]

esp32 with micro-ROS wifi transport

For the config of the esp32 with wifi, if I am not planning on using the LiDAR, I can just comment out its' config right?

After the first connection test success, you may comment out the Lidar setting.

[n0sc3tipsum]

Hi, So I have been playing around and eventually got it working. I could see the topics and nodes as expected for my ESP program.

Yet for some reason ability to see topics and nodes isn't consistent.

I have been using the docker image with a ROS2 installation as provided on the tutorial page on the Micro-ROS website. When I run the micro ros agent with -v6 flag I can see data going from client to agent and all the topics, subscribers and so on are created fine, yet ros2 topic list only shows the default topics.

To make sure its not an issue with my code, I ran the ping pong demo.

• I had started the docker image with ROS2 and Micro-ROS installed
• Sourced /opt/ros/humble/setup.bash
• Sourced microros/install/local_setup.bash
• export RMW_IMPLEMENTATION=rmw_microxrcedds

Source workspace and run agent

Source Workspace and run rclc ping pong demo

Established connection?

Not topics or nodes :(

[hippo5329]

I do not use docker. There is no need for rmw_microxrcedds. It is used for testing on PC, not for esp32. My suggestion is my wiki. You may test it with a bare esp32 wroom. Just connect with a USB cable. No other wiring is needed.

[hippo5329]

Install the software https://github.com/hippo5329/linorobot2_hardware/wiki#install-the-software

Build, upload and test micro-ros wifi connection. https://github.com/hippo5329/linorobot2_hardware/wiki#esp32-with-micro-ros-wifi-transport

Check topics https://github.com/hippo5329/linorobot2_hardware/wiki#check-topics

[n0sc3tipsum]

Install the software https://github.com/hippo5329/linorobot2_hardware/wiki#install-the-software

Build, upload and test micro-ros wifi connection. https://github.com/hippo5329/linorobot2_hardware/wiki#esp32-with-micro-ros-wifi-transport

Check topics https://github.com/hippo5329/linorobot2_hardware/wiki#check-topics

Hi, I was able to get it working properly now. I think there is something wrong with my installation so I was using my friends machine, but that's beside the point.

I am using topic based ros2 control to send joint commands to the ESP32 over topics to control the wheels, and I publish wheel states back to the joint state broadcaster via topics too. I am having a slight issue though. When publishing joint commands, I can see the data being published as I set the command velocities using the teleop_twist_keyboard. Yet the latency in receiving those commands on the esp32 seems to be crazy high, with some commands not being received or being received minutes later.

This is the rqt_graph of the system for reference. The /joint_ctrl topic is the one the esp32 subscribes to for the wheel commands.

I have a feeling its' due to publishing quite a bit of info on the joint states, IMU data and battery state whilst also trying to read the joint_ctrl topic, I am not quite sure as I am not too familiar with how the executor works with the timer and subscribers. I have attached the code below for the main file and the actual publish/subscribe callbacks :

//Interrupt Service Routine for motor update
//Note: ESP32 doesn't support floating point calculations in an ISR
bool TimerHandler(void * timerNo)
{
    static bool toggle = false;

    //Update the stepper motors
    left_motor.runStepper();
    right_motor.runStepper();

    //Indicate that the ISR is running
    digitalWrite(TOGGLE_PIN,toggle);  
    toggle = !toggle;
    return true;
}

void setup()
{
    Serial.begin(115200);

    pinMode(TOGGLE_PIN,OUTPUT);
    left_motor_speed = 0.0;
    right_motor_speed = 0.0;

// IMU Setup and WiFi Setup Code ...

    //Attach motor update ISR to timer to run every STEPPER_INTERVAL_US μs
    if (!ITimer.attachInterruptInterval(STEPPER_INTERVAL_US, TimerHandler)) 
    {
        Serial.println("Failed to start stepper interrupt");
        while (1) delay(10);
    }
    Serial.println("Initialised Interrupt for Stepper");

    //Enable the stepper motor drivers
    pinMode(STEPPER_EN,OUTPUT);
    digitalWrite(STEPPER_EN, false);

   //Setting accelereration allows the stepper motors to move
    left_motor.setAccelerationRad(30);
    right_motor.setAccelerationRad(30);
   // use_kinematic_control tells the system to listen on the \joint_ctrl topic for wheel commands rather than \cmd_vel 
   bool use_kinematic_control = true;
    espRosAgent.Init(use_kinematic_control);

    const unsigned int timer_timeout = 1000;

    RCSOFTCHECK(rclc_timer_init_default(
                &espRosAgent.timer,
                &espRosAgent.support,
                RCL_MS_TO_NS(timer_timeout),
                timer_callback), 
                "Init Timer");

        RCSOFTCHECK(rclc_executor_add_subscription(
            &espRosAgent.executor, 
            &espRosAgent._kinematic_cmd_vel_sub, &espRosAgent._kinematic_cmd_msg, 
            &kinematic_cmd_vel_callback, ON_NEW_DATA), 
            "Create Kinematic Cmd Subscription");
 }

oid loop()
{
    //static unsigned long printTimer = 0;  //time of the next print
    static unsigned long loopTimer = 0;   //time of the next control update

    /*if (Serial.available() > 0) 
    {
        String command = Serial.readStringUntil('\n');  // Read the command until newline
        command.trim();  // Remove any whitespace or newline characters

        if (command == "q") 
        {           
            Serial.println("");
            Serial.println("------ Shutdown command received. Exiting... -------");
            Serial.println("");
            cleanup();  // Call the cleanup function
            while(true);  // Optionally, enter an infinite loop to stop further execution
        }
    }*/

    if (millis() > loopTimer) 
    {
        loopTimer += LOOP_INTERVAL;

        // Get data
        imu.getEvent(&imu_data.accel, &imu_data.gyro, &imu_data.temp);
        motor_data.left_pos = left_motor.getPositionRad();
        motor_data.left_speed = left_motor.getSpeedRad();
        motor_data.right_pos = right_motor.getPositionRad();
        motor_data.right_speed = right_motor.getSpeedRad();

        //PID Loop : Uses data on /cmd_vel topic (in this case disabled)
        if (!espRosAgent._kin_en)
        {
            accelTilt = imu_data.accel.acceleration.z/9.67 - 0.09;
            gyroRate  = imu_data.gyro.gyro.y;

            tilt = CompFilter(accelTilt, gyroRate, alpha, tilt);
            error = setpoint - tilt;
            imu_data.accel.acceleration.y = tilt;
            integral += error *dt;

            PIDout = error * Kp  - gyroRate*Kd + integral * Ki;
            left_motor.setAccelerationRad(PIDout);
            right_motor.setAccelerationRad(PIDout);

            if (PIDout < 0)
            {
                left_motor.setTargetSpeedRad(-20);
                right_motor.setTargetSpeedRad(-20);
            }

            else
            {
                left_motor.setTargetSpeedRad(20);
                right_motor.setTargetSpeedRad(20);
            }

            PreviousError = error;
        }

        else
        {   
           //left_motor_speed is just a global variable to hold target speed obtained from topic
            left_motor.setTargetSpeedRad(left_motor_speed);
            right_motor.setTargetSpeedRad(right_motor_speed);
        }

    }

    rclc_executor_spin_some(&espRosAgent.executor, RCL_MS_TO_NS(100));
}

These are some relevant functions in the ROSComm class

void ROSComm::Init(bool use_kinematic_control)
{
    Serial.println("Setting micro-ROS wifi transports");
    _kin_en = use_kinematic_control;
    set_microros_wifi_transports(_ssid, _pswd, _agent_ip, _agent_port);

    rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
    allocator = rcl_get_default_allocator();

    RCSOFTCHECK(rcl_init_options_init(
        &init_options, 
        allocator),
        "Init RCL Options");

    // Initialize rclc support object
    RCSOFTCHECK(rclc_support_init(&support, 0, NULL, &allocator), "Init Default SUpport");

    // Init node with configured support object
    RCSOFTCHECK(rclc_node_init_default(
        &node, 
        "esp_node", "",
        &support), 
        "Init ESP Node");

    CreatePublishers();

    CreateSubscribers();

    InitMessages();

    unsigned int num_handles = 2;

    RCSOFTCHECK(rclc_executor_init(
        &executor, 
        &support.context, 
        num_handles, 
        &allocator), 
        "Init Executor");

}
void ROSComm::KinematicCommandCallback(const void *kin_cmd_vel_recv, double *left_motor_vel, double *right_motor_vel)
{
    _kinematic_cmd_msg = *(sensor_msgs__msg__JointState *) kin_cmd_vel_recv;
    *left_motor_vel = _kinematic_cmd_msg.velocity.data[0];
    *right_motor_vel = _kinematic_cmd_msg.velocity.data[1];
}
void ROSComm::PublishCallback(motor_data_t *motor_data, imu_data_t *imu_data, int BattLevel, int BattPower)
{

    getData(motor_data, imu_data, BattLevel, BattPower);
    rcl_ret_t rc;
    rc += rcl_publish(&_joint_state_pub, &_joint_states_msg, NULL);
    rc += rcl_publish(&_imu_pub, &_imu_msg, NULL);
    rc += rcl_publish(&_batt_lvl_pub, &_battery_lvl_msg, NULL);
    rc += rcl_publish(&_batt_pwr_pub, &_battery_pwr_msg, NULL);
}

Could discrepancy between the data published onto the /joint_ctrl topic and the data read from it on the esp32 be related to the executor or timer timeout?

I know this issue was raised for a totally different reason, but I was hoping you could give some insight given your vast experience with ESP32s. This is for a project I am making so unfortunately I couldn't resort to your repo.

Thank you

[hippo5329]

I didn't mean that you should use the linorobot_hardware for your project. I suggested that you tried out the firmware to get the idea how it works. Then you can compare the codes and resolve your problem. I have my own projects and you will need to debug on your own.