Cannot send Transform Stamped Sequence using micro-ROS Arduino library on ESP32

edit

I'm trying to send multiple Transform Stamped messages on a 'tf_static' topic using the micro-ROS Arduino library on the ESP32 Devkit board. But I encountered an exception. Here's my development environment.

Host PC: Ubuntu 22.04 LTS (192.168.1.25)
ROS2 package: humble
micro-ROS Arduino library: micro_ros_arduino-humble v2.0.5-humble
Microcontroller Board: DOIT ESP32 Devkit V1

When I send a single Transform Stamped using the sketch below. It works.

#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <geometry_msgs/msg/transform_stamped.h>

rcl_publisher_t publisher;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

#define LED_PIN 2
#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); }
}

static geometry_msgs__msg__TransformStamped base_link_msg;

void setup() {
  static const int frame_id_size = 100;
  Serial.begin(115200);

  char base_link_name[] = "base_link";
  base_link_msg.header.stamp.sec = 0;
  base_link_msg.header.stamp.nanosec = 0;
  base_link_msg.header.frame_id.data = (char*)malloc(sizeof(char)*frame_id_size);
  memcpy(base_link_msg.header.frame_id.data, base_link_name, strlen(base_link_name) + 1);
  base_link_msg.header.frame_id.size = strlen(base_link_msg.header.frame_id.data);
  base_link_msg.header.frame_id.capacity = frame_id_size;
  char base_link_child_name[] = "laser_link";
  base_link_msg.child_frame_id.data = (char*)malloc(sizeof(char)*frame_id_size);
  memcpy(base_link_msg.child_frame_id.data, base_link_child_name, strlen(base_link_child_name) + 1);
  base_link_msg.child_frame_id.size = strlen(base_link_msg.child_frame_id.data); 
  base_link_msg.child_frame_id.capacity = frame_id_size;

  set_microros_wifi_transports("ssid", "passwd", "192.168.1.25", 8888);
  Serial.println("wifi connected");
  pinMode(LED_PIN, OUTPUT);  digitalWrite(LED_PIN, HIGH);
  delay(2000);

  allocator = rcl_get_default_allocator();
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "my_node", "", &support));
  RCCHECK(rclc_publisher_init_default(&publisher, &node
  , ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TransformStamped), "/tf_static"));
}

void loop() {
  static unsigned long microsec = micros();
  unsigned long sec = microsec/1000000;
  unsigned long nsec = (microsec - sec*1000000)*1000;
  base_link_msg.header.stamp.sec = sec;
  base_link_msg.header.stamp.nanosec = nsec;

  base_link_msg.transform.translation.x = 0.0;
  base_link_msg.transform.translation.y = 0.0;
  base_link_msg.transform.translation.z = 0.02;
  base_link_msg.transform.rotation.x = 0.0;
  base_link_msg.transform.rotation.y = 0.0;
  base_link_msg.transform.rotation.z = 0.0;
  base_link_msg.transform.rotation.w = 1.0;

  Serial.println("publish " + String(sec));   
  RCSOFTCHECK(rcl_publish(&publisher, &base_link_msg, NULL));
  delay(100);
}

The result through the micro_ros_agent is like this below. It seems fine.

$ ros2 topic echo /tf_static
header:
  stamp:
   sec: 1435
   nanosec: 985460000
  frame_id: base_link
child_frame_id: laser_link
transform:
  translation:
   x: 0.0
   y: 0.0
   z: 0.02
  rotation:
   x: 0.0
   y: 0.0
   z: 0.0
   w: 1.0

But I encountered an exception when sending multiple transform-stamped messages using the sequence functions like below.

#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <geometry_msgs/msg/transform_stamped.h>

rcl_publisher_t publisher;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

#define LED_PIN 2
#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); }
}

geometry_msgs__msg__TransformStamped__Sequence* tf_static_msg;

void setup() {
  static const int frame_id_size = 100;
  Serial.begin(115200);
  tf_static_msg = geometry_msgs__msg__TransformStamped__Sequence__create(2);
  bool result = geometry_msgs__msg__TransformStamped__Sequence__init(tf_static_msg, 2);
  if (!result) {
    Serial.println("TransformStamped Sequence Initialization ERROR!");
    while(1);
  }

  char ...