Micro-ROS timer not working on my esp32 board/toolchain about micro_ros_platformio

[pablogs9]

Hello, @migsdigs is there a possibility of using directly micro-ROS module for ESP-IDF? I guess that you will have a better development experience with micro-ROS.

If not possible, could you share your code so we can take a look?

[migsdigs]

Hello @pablogs9, I hope you are well. I have been doing the majority of my project using platformio and I am unfamiliar with the ESP-IDF, but If this will be the only option to get micro-ROS working nicely on my board, for sure I will use it. I also see that the ESP-IDF does not have support for the older espressif platforms, which I require.

The code is at this link: https://github.com/migsdigs/Hiwonder_xArm_ESP32/blob/main/Hiwonder_xArm_ROS2/src/main.cpp. The uncommented code is just the example from the publisher example. You can see in https://github.com/migsdigs/Hiwonder_xArm_ESP32/blob/main/Hiwonder_xArm_ROS2/platformio.ini how much environment is configured.

My feeling is that the issue is something to do with the way ros interfaces with the timers on the board itself, and maybe there is some slight difference in the timer or clock configuration between the two different boards.

[pablogs9]

Ok, so let's focus on this code running in your current toolchain/platform: https://github.com/migsdigs/Hiwonder_xArm_ESP32/blob/main/Hiwonder_xArm_ROS2/src/main.cpp

• Does it work if you publish directly in the loop() function?
• Does it freeze or if you have a printf inside the loop it just runs and skips the timer trigger?
• Does it enter in the timer callback?
• Could you provide the micro-ROS Agent output with the flag -v6?

[migsdigs]

• Yes, it does publish directly in the loop() function.
• It does not enter the timer callback, I have done some debugging with this and also used some flag to see if it does enter it. It does not.
• The micro-ROS agent output is in the attached txt file.

microros-timer-agent.txt

[pablogs9]

Does it enters the timer if you remove the delay(100) in the loop()?

If not I guess that you shall check that this function is working correctly:

micro_ros_platformio/platform_code/arduino/clock_gettime.cpp

Line 8 in 841dc13

extern "C" int clock_gettime(clockid_t unused, struct timespec *tp)

Maybe printing its return value to check that you are having a monotonic clock.

[migsdigs]

I removed the delay(100) from the loop.

It now enters the callback, but it does it somewhat randomly (definitely not 1 second periodically), and I see it publish when I echo the topic, but yeah somewhat randomly (around 2-4 seconds).

Also tried making the delay in the loop smaller, for example delay(10), then it does not enter the callback, as before.

You still think I should look into that function?

Thanks for the help thus far :)

[pablogs9]

Sure, take a look at that function we have found some platforms where micros() or millis() are not working properly.

If not, it might be a transport issue.

Btw if you test a smaller delay, let say 1/10 of you spin rate. How does it behaves?

[migsdigs]

Alright, thanks. I will take a look.

On your last point, i tried a smaller delay (10 ms), and then it didn't enter the callback either.

[migsdigs]

Does it enters the timer if you remove the delay(100) in the loop()?

If not I guess that you shall check that this function is working correctly:

micro_ros_platformio/platform_code/arduino/clock_gettime.cpp

Line 8 in 841dc13

extern "C" int clock_gettime(clockid_t unused, struct timespec *tp)

Maybe printing its return value to check that you are having a monotonic clock.

On a bit of a separate note. Is there a way of printing to the ROS agent terminal? Whenever I printf() or Serial.println() in platformio I have random outputs in the terminal even though my baud and monitor rates are consistent. This also only happens when I am running something else over the serial line (like communicating between my pc and my esp32).

Thanks again

[pablogs9]

You are using the Serial device in your platform as the micro-ROS communication device.

You cannot use it to print into the terminal or you will generate interferences with the micro-ROS wire protocol

You can:

• Try to create a new Serial device on another port.
• Don't call any micro-ROS code, just print normally the output of this method in an independent application with no micro-ROS

[migsdigs]

To try make my life simpler I tried using a 'native' esp32 timer using functions from esp32-hal-timer.c to publish my messages. While the timer does indeed work, which I can confirm by my periodically lighting LED on IO pin 2. I am however having some issues when I add the publish line to my timer callback, being that 1. it does not publish every second, rather every +-10 seconds (while the LED continues to blink every second) and 2. the ESP proceeds to reset and run its setup() function between every publish.

Since the script for this is rather long, the basic code is as follows:

// includes
...

rosidl_runtime_c__String__Sequence name__string_sequence;

// Array to populate JointState message for Servo positions
double pos[6] = {0, 0, 0, 0, 0, 0};				// servo positions
double effort_array[] = {0, 0, 0, 0, 0, 0};		// effort array (just needed to populate message component)
double vel[] = {0, 0, 0, 0, 0, 0};				// velocity array "  "

// Timer settings
static const uint16_t timer_divider = 80;
static const uint64_t timer_max_count = 1000000;

static hw_timer_t *timer = NULL;

rcl_node_t node_hiwonder;	// Node object

// Publisher and Subscriber objects
rcl_publisher_t 	publisher;
rclc_support_t 	support;
rcl_allocator_t allocator;

rclc_executor_t executor_servo_pos_publish;

// ROS Error Handling
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
...

// ROS messages
sensor_msgs__msg__JointState servo_position_array_msg;

// timer callback / Interupt
void IRAM_ATTR onTimer() {
	// Toggle LED
	int pin_state = digitalRead(LED_BUILTIN);
	digitalWrite(LED_BUILTIN, !pin_state);

	pos[0] = servo1.pos_read();
	pos[1] = servo2.pos_read();
	pos[2] = servo3.pos_read();
	pos[3] = servo4.pos_read();
	pos[4] = servo5.pos_read();
	pos[5] = servo6.pos_read();

	// Update servo_position_array_msg
	servo_position_array_msg.position.data = pos;
	servo_position_array_msg.effort.data = effort_array;
	servo_position_array_msg.velocity.data = vel;
	servo_position_array_msg.header.stamp.sec = (uint16_t)(rmw_uros_epoch_millis()/1000); 	// timestamp
	servo_position_array_msg.header.stamp.nanosec = (uint32_t)rmw_uros_epoch_nanos();		// timestamp

  	// Publishes
  	RCSOFTCHECK(rcl_publish(&publisher_servo_pos, &servo_position_array_msg, NULL));
}


void setup() {
	Serial.begin(115200);
	set_microros_serial_transports(Serial);
	delay(2000);

       // Timer settings
       // Create and start timer (num, divider, countUp)
	timer = timerBegin(0, timer_divider, true);

	// Provide ISR to timer (timer, function, edge)
	timerAttachInterrupt(timer, &onTimer, true);

	// At what count should ISR trigger (timer, count, autoreload)
	timerAlarmWrite(timer, timer_max_count, true);

	// Allow ISR to trigger
	timerAlarmEnable(timer);

      
       // Servo settings etc...
       Serial.println("Beginning Servo Example");
	servoBus.beginOnePinMode(&Serial2,33);
	servoBus.debug(true);
	servoBus.retry=1;

        ...

	// ros setup
	allocator = rcl_get_default_allocator();									// Initialize micro-ROS allocator
	RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));					// Initialize support options
	
	// create node
  	RCCHECK(rclc_node_init_default(&node_hiwonder, "Hiwonder_xArm_node", "", &support));

	// Servo position publisher (publishes at a rate of 100 Hz)
	RCCHECK(rclc_publisher_init_default(
		&publisher_servo_pos, 
		&node_hiwonder, 
		ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState), 
		"servo_pos_publisher"));

        // message initialisation
	bool success = rosidl_runtime_c__String__Sequence__init(&name__string_sequence, 6);
	servo_position_array_msg.name = name__string_sequence;
	success = rosidl_runtime_c__String__assignn(&servo_position_array_msg.name.data[0], "Servo1", 6);
	success = rosidl_runtime_c__String__assignn(&servo_position_array_msg.name.data[1], "Servo2", 6);
	success = rosidl_runtime_c__String__assignn(&servo_position_array_msg.name.data[2], "Servo3", 6);
	success = rosidl_runtime_c__String__assignn(&servo_position_array_msg.name.data[3], "Servo4", 6);
	success = rosidl_runtime_c__String__assignn(&servo_position_array_msg.name.data[4], "Servo5", 6);
	success = rosidl_runtime_c__String__assignn(&servo_position_array_msg.name.data[5], "Servo6", 6);

       ...


	// init executors
	RCCHECK(rclc_executor_init(&executor_servo_pos_publish, &support.context, 1, &allocator));			// executor for publishing servo pos.

}

void loop() {	
	delay(20);
}

Have you perhaps experienced something similar before? And is the issue of the ESP resetting perhaps something memory related (memory leak or exceeding flash)?

I would greatly appreciate any feedback.

[pablogs9]

I'm not sure that running micro-ROS code in an ISR is a good idea. Is this timer an ISR?

[migsdigs]

I'm not sure that running micro-ROS code in an ISR is a good idea. Is this timer an ISR?

It is. And judging by my troubles, I am inclined to agree with you.

[pablogs9]

Not sure if running the whole rcl_publish procedure inside an ISR. It takes some time, is very depth in stack and it is probable that makes other system calls.

[migsdigs]

Not sure if running the whole rcl_publish procedure inside an ISR. It takes some time, is very depth in stack and it is probable that makes other system calls.

Alright, thanks for the info!