[BUG] SDO message error report in Drive

[vcrocher]

Description

Error reporting in Drive SDO message return not working as expected. The error code parsing does not work.

Possible Fix

Tested possible fix:

int Drive::sendSDOMessages(std::vector<std::string> messages) {
    int successfulMessages = 0;
    for (auto strCommand : messages) {
        spdlog::trace(strCommand);

#ifndef NOROBOT
        // explicitly cast c++ string to from const char* to char* for use by cancomm function
        char *SDO_Message = (char *)(strCommand.c_str());
        char returnMessage[STRING_BUFFER_SIZE];
        cancomm_socketFree(SDO_Message, returnMessage);
        std::string retMsg = returnMessage;

        // Because returnMessage includes sequence it is possible value is "[1] OK".
        // Therefore it is checked if return message includes the string "OK".
        // Another option would be erasing the sequence value before returning in cancomm_socketFree
        if (retMsg.find("OK") != std::string::npos) {
            successfulMessages++;
        }
        else {
            std::string errormsg = "sendSDOMessage: ERROR: " + strCommand;
            if(retMsg.find("0x")!=std::string::npos) {
                size_t err_code_l = 10;
                std::string error_code = retMsg.substr(retMsg.find("0x"), err_code_l);
                errormsg += " => " +  SDO_Standard_Error[error_code] + " (" + error_code + ")";
            }
            else {
                errormsg += " => " + retMsg;
            }
            spdlog::error(errormsg);
        }
        spdlog::trace(retMsg);
#else
        spdlog::trace("VCAN OK no reply.");
        successfulMessages++;
#endif
    }

    return successfulMessages-messages.size();
}