How do you publish diagnostics messages from an Arduino? This is a follow up to my more general question about publishing diagnostics messages from C++.

This is my current minimal code:

#include <ros.h>
#include <math.h>
#include <stdint.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <diagnostic_msgs/KeyValue.h>

#define DIAGNOSTIC_STATUS_LENGTH 1

// http://wiki.ros.org/roscpp/Overview/Logging
// http://wiki.ros.org/rosserial/Overview/Logging
ros::NodeHandle nh;

diagnostic_msgs::DiagnosticArray dia_array;

diagnostic_msgs::DiagnosticStatus robot_status;
diagnostic_msgs::KeyValue status_keyvalue;

void toggle_led() {
    digitalWrite(STATUS_LED_PIN, HIGH-digitalRead(STATUS_LED_PIN));   // blink the led
}

void on_toggle_led(const std_msgs::Empty& msg) {
    nh.loginfo("LED toggled.");
    toggle_led();
}
ros::Subscriber<std_msgs::Empty> toggle_led_sub("toggle_led", &on_toggle_led);

void setup() {

    // Initialize diagnostic status array.
    dia_array.status = (diagnostic_msgs::DiagnosticStatus*)malloc(DIAGNOSTIC_STATUS_LENGTH * sizeof(diagnostic_msgs::DiagnosticStatus));
    dia_array.status_length = DIAGNOSTIC_STATUS_LENGTH;
    robot_status.values = (diagnostic_msgs::KeyValue*)malloc(DIAGNOSTIC_STATUS_LENGTH * sizeof(diagnostic_msgs::KeyValue));
    robot_status.values_length = DIAGNOSTIC_STATUS_LENGTH;

    // Turn on power status light.
    pinMode(STATUS_LED_PIN, OUTPUT);
    digitalWrite(STATUS_LED_PIN, true);

    nh.getHardware()->setBaud(115200);
    nh.initNode();

    nh.subscribe(toggle_led_sub);
    nh.advertise(diagnostics_publisher);

}

void loop() {

    robot_status.name = "Robot";
    robot_status.level = diagnostic_msgs::DiagnosticStatus::OK;
    robot_status.message = "Everything seem to be ok.";
    status_keyvalue.key = "Emgergencystop hit";
    status_keyvalue.value = "false";
    robot_status.values[0] = status_keyvalue;
    dia_array.status[0] = robot_status;
    diagnostics_publisher.publish(&dia_array);

    nh.spinOnce();

}

It compiles and seems to run fine. The rosserial output indicates it's publishing to the /diagnostics topic, but when I run rostopic echo /diagnostics I never see any messages. I know the Arduino is properly running and not crashing because it still responds promptly to toggle_led messages, which toggles the standard D13 led.

What am I doing wrong?

My target Arduino is an Arduino*Pro, which has roughly the RAM and flash of an Arduino Mega.

Edit: Explicitly setting the *_length value for both the dia_array.status and robot_status.values arrays caused some messages to start to be sent on /diagnostics, but the messages are all errors that look like:

header: 
  seq: 10
  stamp: 
    secs: 1495512912
    nsecs: 6547927
  frame_id: ''
status: 
  - 
    level: 2
    name: rosserial_python
    message: Packet Failed : Failed to read msg data
    hardware_id: ''
    values: 
      - 
        key: last sync
        value: Tue May 23 00:15:06 2017
      - 
        key: last sync lost
        value: Wed Dec 31 19:00:00 1969

Presumably, something's now being published, but I'm still structuring my data incorrectly and it's causing rosserial to corrupt the packet. What am I doing wrong?

#include "ros.h"
#include "diagnostic_msgs/DiagnosticArray.h"

#define VALUES 3
#define STATUS_NAME "status"
#define STATUS_MSG "message"
#define HARDWARE_ID "hardware"

diagnostic_msgs::DiagnosticArray diag_array;
diagnostic_msgs::DiagnosticStatus diag_status;
diagnostic_msgs::KeyValue values[VALUES];
ros::Publisher diag_pub("diagnostics", &diag_array);

void setup_diags() {
    if (! nh.advertise(diag_pub)) {
        nh.logwarn("Failed to advertise diag_pub");
    }

    diag_array.status_length = 1;

    diag_status.name = STATUS_NAME;
    diag_status.message = STATUS_MSG;
    diag_status.hardware_id = HARDWARE_ID;
    diag_status.level = diagnostic_msgs::DiagnosticStatus::STALE;
    diag_status.values_length = VALUES;

    values[SENT].key = "sent";
    values[FAILED].key = "failed";
    values[PENDING].key = "pending;

    return;
}

void publish_diags(int sent, int failed, bool pending) {
    sprintf(sent_str,
            "%d",
            sent);
    values[SENT].value = sent_str;
    sprintf(failed_str,
            "%d",
            failed);
    values[FAILED].value = failed_str;
    values[PENDING].value = (pending ? "true" : "false");

    diag_status.values = values;
    diag_status.level = diagnostic_msgs::DiagnosticStatus::OK;

    diag_array.status = &diag_status;
    diag_array.header.stamp = nh.now();
    diag_pub.publish(&diag_array);

    return;
}