Revision history [back]

This works with ROS Melodic, rosserial_arduino, and ros-teensy on a Teensy 4.1

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

This works with ROS Melodic, rosserial_arduino, and ros-teensy on a Teensy 4.1

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