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 imageahendrix ( 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 imageCerin ( 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 imageahendrix ( 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 imageCerin ( 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 imageahendrix ( 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 imageCerin ( 2017-05-22 23:33:25 -0500 )edit