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
frameid: baselink
childframeid: 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
clkdrv: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