Publishing diagnostics from an Arduino

edit

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?