[Firmata-devel] Connecting Firmata to ROS

[Abe S. <or...@gm...>]

Hi all,

I am working on a ROS (Robot Operating System) node to connect an Arduino
running Firmata to ROS on a PC. The idea is to use the Arduino as at least
part of the sensors and motor controllers for a low-cost robot using COTS
(Commercial/Cheap Off-The-Shelf) parts.

I'm implementing the node as a C++ class that will use ROS's "Cereal Port"
node to communicate with Firmata. As a first test, I am attempting to query
the Firmware version of the copy of Firmata on my Arduino (An Arduino NG
with an Atmega168).

The code that I'm using to try to do this is below. The result I get back
starts out promising and then falls apart. I get the START_SYSEX byte
(0xF0), 0x02 0x03 (good, as I'm using Firmata 2.3), and then nothing. Since
my code currently prints bytes until it hits the END_SYSEX byte (0xF7),
this prints out a bunch of random stuff from RAM and then sometimes crashes.

Am I missing something about the protocol (like when to expect the
END_SYSEX), or do I have a deeper problem?


I call my method like so:

char reply[1024];
fp.queryFirmware(reply);

And the body of the method follows. Calls to device.write() and
device.read() are how data gets passed to and from the cereal port node:

#define START_SYSEX 0xF0
#define END_SYSEX 0xF7

void FirmataProtocol::queryFirmware(char* reply) {
    //Set up and send the query
    char* msg = new char[3];
    msg[0] = 0xF0;
    msg[1] = 0x79;
    msg[2] = 0xF7;
    //TODO memory leak around new
    device.write(msg, 3);

    //Try for a reply, parameters are buffer, length, and timeout
    try {
        device.read(reply, 1024, 1000);
    } catch (cereal::TimeoutException& e) {
        ROS_ERROR("Timeout!");
    }

    //Parse the response
    int ii = 0;
    if (reply[ii] == char(START_SYSEX)) {
        while (reply[ii] != char(END_SYSEX)) {
            printf("%02X", ((unsigned char *) reply)[ii] );
            ii++;
        }
    }
    //
    // //Return the protocol string
    // return "Parse not implemented yet";
}