micro_ros_arduino icon indicating copy to clipboard operation
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

Open alikureishy opened this issue 2 years ago • 5 comments

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;
        };

alikureishy avatar Aug 12 '22 03:08 alikureishy

Where are you initializing const MessageType* const message?

pablogs9 avatar Aug 12 '22 06:08 pablogs9

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: @.***>

alikureishy avatar Aug 12 '22 06:08 alikureishy

But could you please share the code where you are initializing the message memory that is going to be published?

pablogs9 avatar Aug 12 '22 06:08 pablogs9

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.

alikureishy avatar Aug 14 '22 00:08 alikureishy

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.

alikureishy avatar Aug 14 '22 00:08 alikureishy