micro_ros_arduino
micro_ros_arduino copied to clipboard
rcl_publish() seems to fail intermittently (with error code 1) and *eventually* causes a "CORRUPT HEAP" crash on ESP32
Issue template
- Hardware description: ESP32
- RTOS: arduino
- Installation type: github + docker build + platformio
- Version or commit hash: humble
Steps to reproduce the issue
I'm running firmware that periodicaly publishes a ROS message:
MyMessage:msg:
string<=150 something
string<=64[<=20] items
float64[<=20] levels
Expected behavior
Should work smoothly once the microros components are initialized.
Actual behavior
After publishing a large number of messages, the rcl_publish()
API returns error code # 1. This happens intermittently.
After a few such intermittent error codes occur, the following crash occurs (immediately after the last error):
CORRUPT HEAP: Bad head at 0x3ffd40a4. Expected 0xabba1234 got 0x3ffcf264
assert failed: multi_heap_free multi_heap_poisoning.c:253 (head != NULL)
Backtrace:0x40083181:0x3ffdc1d00x4008cc15:0x3ffdc1f0 0x40091f6d:0x3ffdc210 0x40091bb3:0x3ffdc340 0x400837dd:0x3ffdc360 0x40091f9d:0x3ffdc380 0x401092c7:0x3ffdc3a0 0x40109323:0x3ffdc3c0 0x40109357:0x3ffdc3e0 0x4011a6b5:0x3ffdc400 0x401065a7:0x3ffdc420 0x401076e2:0x3ffdc460 0x4016d29d:0x3ffdc4c0 0x400e2062:0x3ffdc510 0x400f6213:0x3ffdc530 0x400f039c:0x3ffdc580 0x400f0530:0x3ffdc620 0x400f5b7e:0x3ffdc640 0x400f3faf:0x3ffdc690 0x400ea679:0x3ffdc700 0x400ea6c3:0x3ffdc740 0x400d3583:0x3ffdc760 0x400d3531:0x3ffdc790
#0 0x40083181:0x3ffdc1d00 in panic_abort at /home/runner/work/esp32-arduino-lib-builder/esp32-arduino-lib-builder/esp-idf/components/esp_system/panic.c:402
Additional information
My publisher class wrapper is shown below:
template <class MessageType>
class Publisher {
public:
Publisher(const std::string& topic, rcl_node_t* node, const rosidl_message_type_support_t * type_support)
: topic(topic), node(node), type_support(type_support) {
// See: https://micro.ros.org/docs/tutorials/programming_rcl_rclc/pub_sub/
Serial.printf("Initializing '%s' publisher...", this->topic.c_str());
RCCHECK(
rclc_publisher_init_default(
&this->publisher_data,
this->node,
type_support,
topic.c_str()
)
);
Serial.println("DONE!");
}
void publish(const MessageType* const message) {
RCSOFTCHECK(
rcl_publish( <====== Error code '1' intermittently returned from here
&this->publisher_data,
message,
NULL
)
);
}
virtual ~Publisher() {
RCSOFTCHECK(
rcl_publisher_fini(
&this->publisher_data,
this->node
)
);
}
private:
std::string topic;
rcl_node_t* node;
const rosidl_message_type_support_t* type_support;
rcl_publisher_t publisher_data;
};
Where are you initializing const MessageType* const message
?
I initialize it on the heap only once (using the default memory conf), and then cache it for reuse on all subsequent publish invocations using a shared pointer. There's no new memory allocation done for this message after the first invocation.
On Thu, Aug 11, 2022, 11:03 PM Pablo Garrido @.***> wrote:
Where are you initializing const MessageType* const message?
— Reply to this email directly, view it on GitHub https://github.com/micro-ROS/micro_ros_arduino/issues/1135#issuecomment-1212758006, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAJDKA4QL4CV5EZD6L7A6JLVYXSJZANCNFSM56KJNHWQ . You are receiving this because you authored the thread.Message ID: @.***>
But could you please share the code where you are initializing the message memory that is going to be published?
Sure. Here's my message wrapper class:
const static int BUFFER_LENGTH = 5000;
template <class MessageType>
class Message {
public:
Message(const rosidl_message_type_support_t * type_support)
: message(), type_support(type_support) {
assert(type_support);
// See: https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/
assert(BUFFER_LENGTH >= micro_ros_utilities_get_static_size(this->type_support, micro_ros_utilities_memory_conf_default));
assert(micro_ros_utilities_create_static_message_memory(
this->type_support,
&this->message, micro_ros_utilities_memory_conf_default, this->message_buffer, BUFFER_LENGTH));
Serial.println("DONE!");
}
virtual ~Message() = default;
const MessageType* get_data_pointer() const {
return &this->message;
}
const rosidl_message_type_support_t* get_type_support() const {
return this->type_support;
}
private:
MessageType message;
const rosidl_message_type_support_t* type_support;
uint8_t message_buffer[BUFFER_LENGTH];
};
I initialize an instance of a Message class as below:
using MyMessageType = my_message_package::msg::MyMessage;
auto allocated_message = std::shared_ptr<Message<MyMessageType>>(
new Message<MyMessageType>(
ROSIDL_GET_MSG_TYPE_SUPPORT (my_message_package, msg, MyMessage)
)
);
... and then I use the get_data_pointer()
API to retrieve the raw ROS message that rcl_publish()
requires.
In order to set data for any Sequence
types, I have assertions that ensure the capacity
is never crossed, and manage the size
and data
variables accordingly. For example, to set a string in the message, I use this code:
assert(message.somestring.capacity >= (value.size() + 1)); // Since we need an extra character for the null character
strcpy(message.somestring.data, value.c_str());
message.somestring.size = value.size();
Same goes for other Sequence types.