Robotics StackExchange | Archived questions

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

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: microrosarduino-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 microrosagent is like this below. It seems fine.

$ ros2 topic echo /tfstatic
header:
  stamp:
   sec: 1435
   nanosec: 985460000
  frame
id: baselink
child
frameid: laserlink
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 base_link_name[] = "base_link";
  tf_static_msg->data[0].header.stamp.sec = 0;
  tf_static_msg->data[0].header.stamp.nanosec = 0;
  tf_static_msg->data[0].header.frame_id.data = (char*)malloc(sizeof(char)*frame_id_size);
  memcpy(tf_static_msg->data[0].header.frame_id.data, base_link_name, strlen(base_link_name) + 1);
  tf_static_msg->data[0].header.frame_id.size = strlen(tf_static_msg->data[0].header.frame_id.data);
  tf_static_msg->data[0].header.frame_id.capacity = frame_id_size;
  char laser_link_name[] = "laser_link";
  tf_static_msg->data[0].child_frame_id.data = (char*)malloc(sizeof(char)*frame_id_size);
  memcpy(tf_static_msg->data[0].child_frame_id.data, laser_link_name, strlen(laser_link_name) + 1);
  tf_static_msg->data[0].child_frame_id.size = strlen(tf_static_msg->data[0].child_frame_id.data); 
  tf_static_msg->data[0].child_frame_id.capacity = frame_id_size;

  tf_static_msg->data[1].header.stamp.sec = 0;
  tf_static_msg->data[1].header.stamp.nanosec = 0;
  tf_static_msg->data[1].header.frame_id.data = (char*)malloc(sizeof(char)*frame_id_size);
  memcpy(tf_static_msg->data[1].header.frame_id.data, base_link_name, strlen(base_link_name) + 1);
  tf_static_msg->data[1].header.frame_id.size = strlen(tf_static_msg->data[1].header.frame_id.data);
  tf_static_msg->data[1].header.frame_id.capacity = frame_id_size;
  char wheel_link_name[] = "wheel_link";
  tf_static_msg->data[1].child_frame_id.data = (char*)malloc(sizeof(char)*frame_id_size);
  memcpy(tf_static_msg->data[1].child_frame_id.data, wheel_link_name, strlen(wheel_link_name) + 1);
  tf_static_msg->data[1].child_frame_id.size = strlen(tf_static_msg->data[1].child_frame_id.data); 
  tf_static_msg->data[1].child_frame_id.capacity = frame_id_size;

  set_microros_wifi_transports("HG8045-F69A-bg", "cesan56y", "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 uint32_t microsec = 0;
  uint32_t sec = microsec/1000000;
  uint32_t nsec = (microsec - sec*1000000)*1000;
  tf_static_msg->data[0].header.stamp.sec = sec;
  tf_static_msg->data[0].header.stamp.nanosec = nsec;
  tf_static_msg->data[0].transform.translation.x = 0.0;
  tf_static_msg->data[0].transform.translation.y = 0.0;
  tf_static_msg->data[0].transform.translation.z = 0.02;
  tf_static_msg->data[0].transform.rotation.x = 0.0;
  tf_static_msg->data[0].transform.rotation.y = 0.0;
  tf_static_msg->data[0].transform.rotation.z = 0.0;
  tf_static_msg->data[0].transform.rotation.w = 1.0;

  tf_static_msg->data[1].header.stamp.sec = sec;
  tf_static_msg->data[1].header.stamp.nanosec = nsec;
  tf_static_msg->data[1].transform.translation.x = 0.0;
  tf_static_msg->data[1].transform.translation.y = 0.0;
  tf_static_msg->data[1].transform.translation.z = 0.04;
  tf_static_msg->data[1].transform.rotation.x = 0.0;
  tf_static_msg->data[1].transform.rotation.y = 0.0;
  tf_static_msg->data[1].transform.rotation.z = 0.0;
  tf_static_msg->data[1].transform.rotation.w = 1.0;

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

The exception message is like below. I found that the exception occurred when calling the rcl_publish function.

wifi connected
publish 0
Guru Meditation Error: Core 1 panic'ed (LoadProhibited). Exception was unhandled.

Core 1 register dump:
PC : 0x400899b4 PS : 0x00060b30 A0 : 0x8013b06d A1 : 0x3ffb2170

A2 : 0x00000002 A3 : 0x00000000 A4 : 0x000000ff A5 : 0x0000ff00
A6 : 0x00ff0000 A7 : 0xff000000 A8 : 0x8013b158 A9 : 0x3ffb2130
A10 : 0x00000001 A11 : 0x00000004 A12 : 0x00000000 A13 : 0x00000000
A14 : 0x00000000 A15 : 0x00000000 SAR : 0x00000008 EXCCAUSE : 0x0000001c
EXCVADDR : 0x00000000 LBEG : 0x40089278 LEND : 0x40089282 LCOUNT : 0x00000000

Backtrace: 0x400899b1:0x3ffb2170 0x4013b06a:0x3ffb2180 0x40139f6a:0x3ffb21a0 0x4013a5ed:0x3ffb21c0 0x4013a359:0x3ffb2220 0x400d2c3d:0x3ffb2240 0x400d55b1:0x3ffb2290

ELF file SHA256: 16928a8f2a0a3486

Rebooting... ets Jun 8 2016 00:22:57

rst:0xc (SWCPURESET),boot:0x13 (SPIFASTFLASHBOOT)
configsip: 0, SPIWP:0xee
clk
drv:0x00,qdrv:0x00,ddrv:0x00,cs0drv:0x00,hddrv:0x00,wp_drv:0x00
mode:DIO, clock div:1
load:0x3fff0030,len:1184
load:0x40078000,len:13220 ho 0 tail 12
room 4 load:0x40080400,len:3028 entry
0x400805e4,len:13220
ho 0 tail 12 room 4
load:0x40080400,len:3028
entry 0x400805e4

Does anybody see what is wrong with my sketch?

Asked by TaroYoshino on 2023-06-05 10:41:38 UTC

Comments

Answers