Ask Your Question
0

Publishing diagnostics from an Arduino

asked 2017-05-22 10:05:52 -0500

Cerin gravatar image

updated 2017-05-22 23:19:16 -0500

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?

edit retag flag offensive close merge delete

Comments

rosserial doesn't have std::vector so you have to explicitly set size for array members in messages, in addition to setting the array contents.

ahendrix gravatar image ahendrix  ( 2017-05-22 12:12:49 -0500 )edit

@ahendrix, Is this not what I'm doing in the code I posted? How would I fix it?

Cerin gravatar image Cerin  ( 2017-05-22 13:05:01 -0500 )edit

Per http://wiki.ros.org/rosserial/Overvie... is looks like you also need to set robot_status.values_length

ahendrix gravatar image ahendrix  ( 2017-05-22 13:16:20 -0500 )edit

@ahendrix, That seems to have partially fixed it, but now I'm getting an error message on the host. Please see my edit.

Cerin gravatar image Cerin  ( 2017-05-22 23:20:00 -0500 )edit

It looks like you're seeing the diagnostics from the rosserial_python node itself, and not your arduino. I don't know much else about rosserial so I'm not sure why your diagnostics aren't publishing.

ahendrix gravatar image ahendrix  ( 2017-05-22 23:29:29 -0500 )edit

@ahendrix, It's because the Arduino is crashing due to the malformed diagnostics array. This is causing the rosserial node to lose connection when it tries to read the diagnostics packet, and throws that diagnostics message. The solution is to fix the diagnostics array, but I don't know how.

Cerin gravatar image Cerin  ( 2017-05-22 23:33:25 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-01-15 11:48:44 -0500

billmania gravatar image

updated 2022-01-15 11:49:39 -0500

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;
}
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-05-22 10:05:52 -0500

Seen: 715 times

Last updated: Jan 15