toolbox.cpp

Go to the documentation of this file.

#include <stdio.h>      /* for printf() and fprintf() */
#include <sys/socket.h> /* for socket(), bind(), and connect() */
#include <arpa/inet.h>  /* for sockaddr_in and inet_ntoa() */
#include <stdlib.h>     /* for atoi() and exit() */
#include <string.h>     /* for memset() */
#include "sick_scan/tcp/toolbox.hpp"
#include <iostream>
#include <iomanip>              // for std::setprecision
#include <sstream>              // for std::stringstream
#include "sick_scan/tcp/errorhandler.hpp"

#ifdef _MSC_VER
#pragma warning(disable: 4267)
#pragma warning(disable: 4996)
#endif
//
// Write a binary trace output, e.g. of buffer contents.
// Can be used for debugging.
//
void traceBuffer(std::string headerText, BYTE* buffer, UINT32 len)
{
        // Table header
        printInfoMessage(headerText, true);

        // Length
        std::string line;
        line = "Length= " + toString(len) + " bytes.";
        printInfoMessage(line, true);
        
        // Contents
        UINT32 pos = 0;
        while (pos < len)
        {
                line = toHexString(pos) + ": ";
                for (UINT16 i=0; i< 16; i++)
                {
                        line += toHexString(buffer[pos]) +  " ";
                        pos++;
                        if (pos >= len)
                        {
                                break;
                        }
                }
                printInfoMessage(line, true);
        }
}

//
// String conversion: Values to a hex string.
// 0..16 --> 0..F
//
std::string toHexStringNibble(UINT8 val)
{
        std::string s = "0123456789ABCDEF";
        std::string c;
        if (val < 16)
        {
                c = s.substr(val, 1);
        }
        else
        {
                c = "x";
        }
        return c;
}

//
// UINT32-value to Hex-String
// Result: "xxxxxxxx"
//
std::string toHexString(UINT32 val)
{
        std::string s = toHexString((UINT16)(val >> 16));
        s += toHexString((UINT16)(val & 0xFFFF));
        return s;
}

// Ergebnis: "xxxx"
std::string toHexString(UINT16 val)
{
        std::string s = toHexStringNibble((UINT8)(val >> 12));
        s += toHexStringNibble((UINT8)((val >> 8) & 0xF));
        s += toHexStringNibble((UINT8)((val >> 4) & 0xF));
        s += toHexStringNibble((UINT8)(val & 0xF));
        return s;
}

// Ergebnis: "xx"
std::string toHexString(UINT8 val)
{
        std::string s1 = toHexStringNibble((UINT8)(val >> 4));
        std::string s2 = toHexStringNibble((UINT8)(val & 0x0F));
        std::string s = s1 + s2;
        return s;
}


//
// Konvertiert einen String in Kleinbuchstaben
//
std::string toLower(const std::string& text)
{
        std::string low;
        UINT32 i;
        unsigned char c;
        for (i=0; i < text.length(); i++)
        {
                c = text.at(i);
                if ((c >= 'A') || (c <= 'Z'))
                {
                        // Grossbuchstabe umwandeln
                        c += ('a' - 'A');
                }
                low += c;
        }
        
        return low;
}


//
// Konvertiere eine Angabe in [m] in Fuesse und Inches, als Text.
// Ausgabeformat ist <<ff' ii">>
//
std::string convertMeterToFeetAndInch(double m)
{
        std::ostringstream os;
        std::string text;

        // Vorzeichen verarbeiten
        if (m < 0.0)
        {
                os << "-";
                m = -m;
        }

        INT32 feet = (INT32)(m / 0.3048);
        INT32 inch = (INT32)((m - ((double)feet * 0.3048)) / 0.0254);
        if (feet > 0)
        {
                os << feet << "'";
        }
        if ((inch > 0) || (feet == 0))
        {
                os << inch << "\"";
        }
        text = os.str();

        // Ausgabe
        return text;
}



//
// String --> UINT16
//
UINT16 fromString(const std::string& text)
{
        int value;
        int conversions = sscanf(text.c_str(), "%d", &value);
        if (conversions == 1)
        {
                return (UINT16)value;
        }
        
        return 0;
}

int hexCharToValue(char c)
{
        int value = 0;
        
        if ((c >= '0') && (c <= '9'))
        {
                value = c - '0';
        }
        else if ((c >= 'A') && (c <= 'F'))
        {
                value = c - 'A' + 10;
        }
        else if ((c >= 'a') && (c <= 'f'))
        {
                value = c - 'a' + 10;
        }
        
        return value;
}


char convertNibbleToHexChar(int value, bool useLowerCaseLetters)
{
        char c;
        
        if (value < 10)
        {
                c = '0' + value;
        }
        else
        {
                if (useLowerCaseLetters == false)
                {
                        // Grossbuchstaben
                        c = 'A' + value - 10;
                }
                else
                {
                        // Kleinbuchstaben
                        c = 'a' + value - 10;
                }
        }
        
        return c;
}

void convertUINT8toHexString(UINT8 byte, char* buffer)
{
        UINT8 value = (byte >> 4);
        buffer[0] = convertNibbleToHexChar(value);
        value = byte & 0x0F;
        buffer[1] = convertNibbleToHexChar(value);
}

void convertRGBtoHexString(UINT8 r, UINT8 g, UINT8 b, char* buffer)
{
        convertUINT8toHexString(r, buffer);
        convertUINT8toHexString(g, &buffer[2]);
        convertUINT8toHexString(b, &buffer[4]);
}



double makeAngleValid(double angle)
{
        const double twoPi = (2.0 * PI);
        
        while (angle >= PI)
        {
                angle -= twoPi;
        }
        while (angle < -PI)
        {
                angle += twoPi;
        }
        
        return angle;
}

std::string toString(INT32 value)
{
        char c[16];
        sprintf(c, "%i", value);
        return (std::string(c));
}

std::string toString(UINT32 value)
{
        char c[16];
        sprintf(c, "%i", value);
        return (std::string(c));
}

#if INTPTR_MAX != INT32_MAX
std::string toString(size_t value)
{
        char c[16];
        sprintf(c, "%zu", value);
        return (std::string(c));
}
#endif

/*
 * Konvertiere Zahl in formatierten String.
 * digits_before_decimal_point = 0..20
 * digits_after_decimal_point = 0..20
 *
 * Der Ergebnisstring ist formatiert gem. den Vorgaben, d.h. er hat
 * exakt die Laenge digits_before_decimal_point + digits_after_decimal_point + 1,
 * ausser, die Vorkommazahl passte nicht in die Laengenvorgabe
 * "digits_before_decimal_point", dann ist er entsprechend laenger.
 */
std::string doubleToString(double val,
                                                   std::string::size_type digits_before_decimal_point,
                                                   std::string::size_type digits_after_decimal_point)
{
        // Konvertierung in String
        std::string text = doubleToString(val, digits_after_decimal_point);

        // Laengen festlegen: Zuerst vor dem Dezimalpunkt
        const std::string::size_type dotPosition = text.find_first_of('.', 0);
        if (dotPosition != std::string::npos)
        {
                // Punkt gefunden
                if (dotPosition < digits_before_decimal_point)
                {
                        // Zu kurz, also vorne auffuellen
                        std::string::size_type numExtraSpaces = digits_before_decimal_point - dotPosition;
                        text = std::string(numExtraSpaces, ' ') + text;
                }
        }

        // Gesamtlaenge pruefen und ggf. verlaengern. NOTE: This will
        // never happen because the first doubleToString() will always fill
        // up with zero at the end, doesn't it?
        if (text.length() < (digits_before_decimal_point + digits_after_decimal_point + 1))
        {
                // Spaces hinten anfuegen
                std::string::size_type numExtraSpaces =
                        (digits_before_decimal_point + digits_after_decimal_point + 1) -  text.length();
                text += std::string(numExtraSpaces, ' ');
        }

        return text;
}

std::string doubleToString(double val,
                                                   int digits_after_decimal_point)
{
        // Konvertierung in String
        std::stringstream sstr;
        sstr << std::fixed << std::setprecision(digits_after_decimal_point) << val;

        return sstr.str();
}

std::string toString(double val, int digits_after_decimal_point)
{
        return doubleToString(val, digits_after_decimal_point);
}


//
// Konvertiere einen String in seine Adresse und seinen Port
//
// Beispiel: "192.168.0.1:1234" -> 0x0100A8C0
//
void stringToIpTarget(std::string ipAdrStr, UINT32& ipAddress, UINT16& port)
{
        std::string addrStr;
        std::string portStr;
        
        if (ipAdrStr.length() < 3)
        {
                // Ungueltig
                return;
        }
        
        UINT32 adrVal = INADDR_NONE;
        UINT16 portVal = 0;
        
        // Port extrahieren
        size_t pos = ipAdrStr.find_first_of(':');
        if ((pos > 0) && (pos < (ipAdrStr.length() - 1)))
        {
                addrStr = ipAdrStr.substr(0, pos);
                portStr = ipAdrStr.substr(pos+1);
        }
        else
        {
                addrStr = ipAdrStr;
        }

        // Adresse
        adrVal = (UINT32)inet_addr(addrStr.c_str());    //      inet_addr("127.0.0.1");
        ipAddress = adrVal;
        
        // Port
        if (portStr.length() > 0)
        {
                portVal = fromString(portStr);
                port = portVal;
        }
}


//
// Konvertiere die IP-Adresse (IPv4) in einen String der Form a.b.c.d:port
//
std::string ipTargetToString(UINT32 ipAddress, UINT16 port)
{
        std::string addr;
        addr = ipAdrToString(ipAddress);
        
        // Port
        addr += ":";
        addr += toString((UINT16)port);
        
        return addr;
}


//
// Konvertiere die IP-Adresse (IPv4) in einen String der Form a.b.c.d.
// OHNE PORT!
//
std::string ipAdrToString(UINT32 ipAddress)
{
        std::string addr;
        addr =  toString((UINT16)((ipAddress >> 0 ) & 0xFF)) + "." +
                        toString((UINT16)((ipAddress >> 8 ) & 0xFF)) + "." +
                        toString((UINT16)((ipAddress >> 16) & 0xFF)) + "." +
                        toString((UINT16)((ipAddress >> 24) & 0xFF));

        return addr;
}


//
// Read an UINT32 from a buffer. The buffer has the value in Big Endian format.
//
UINT32 memread_UINT32(BYTE*& buffer)
{
        UINT32 value = (((UINT32)buffer[0]) << 24) +
                                        (((UINT32)buffer[1]) << 16) +
                                        (((UINT32)buffer[2]) << 8 ) +
                                        (((UINT32)buffer[3])      );
        buffer += 4;
        return value;
}


//
// Read an UINT16 from a buffer. The buffer has the value in Big Endian format.
//
UINT16 memread_UINT16(BYTE*& buffer)
{
        UINT16 value = (((UINT16)buffer[0]) << 8) +
                                        ((UINT16)buffer[1]);
        buffer += 2;
        return value;
}

//
// Read an UINT8 from a buffer.
//
UINT8 memread_UINT8(BYTE*& buffer)
{
        UINT8 value = buffer[0];
        buffer++;
        return value;
}

//
// Read an INT16 from a buffer. The buffer has the value in Big Endian format.
//
INT16 memread_INT16(BYTE*& buffer)
{
        UINT16 value = (((UINT16)buffer[0]) << 8) +
                                        ((UINT16)buffer[1]);
        buffer += 2;
        return (INT16)value;
}

//
// Read an INT32 from a buffer. The buffer has the value in Big Endian format.
//
INT32 memread_INT32(BYTE*& buffer)
{
        UINT32 value = (((UINT32)buffer[0]) << 24) +
                                        (((UINT32)buffer[1]) << 16) +
                                        (((UINT32)buffer[2]) << 8 ) +
                                        (((UINT32)buffer[3])      );
        buffer += 4;
        return (INT32)value;
}

//
// Read a string (with fixed length) from a buffer.
//
std::string memread_string(BYTE*& buffer, UINT16 length)
{
        std::string text;
        
        for (UINT16 i = 0; i<length; i++)
        {
                text += buffer[i];
        }
        buffer += length;
        
        return text;
}


union FloatInt
{
        float value_float;
        UINT32 value_int;
};

//
// Read a float value from a buffer. The buffer has the value in Big Endian format, so it
// cannot be read directly on x86 machines.
//
float memread_float(BYTE*& buffer)
{
        FloatInt floatint;
        floatint.value_int = memread_UINT32(buffer);
        return floatint.value_float;
}

//
// Write a float value to a buffer, and advance the buffer pointer.
// After writing, the buffer has the value in Big Endian format.
//
void memwrite_float(BYTE*& buffer, float value)
{
        FloatInt floatint;
        floatint.value_float = value;
        memwrite_UINT32(buffer, floatint.value_int);
}

//
// Write an INT32 to a buffer, and advance the buffer pointer.
// The buffer has the value in Big Endian format.
//
void memwrite_INT32(BYTE*& buffer, INT32 value)
{
        memwrite_UINT32(buffer, (UINT32)value);
}

//
// Write an UINT32 to a buffer, and advance the buffer pointer.
// The buffer has the value in Big Endian format.
//
void memwrite_UINT32(BYTE*& buffer, UINT32 value)
{
        buffer[0] = ((value >> 24) & 0xFF);
        buffer[1] = ((value >> 16) & 0xFF);
        buffer[2] = ((value >> 8 ) & 0xFF);
        buffer[3] = ((value      ) & 0xFF);
        buffer += 4;
}

//
// Write an INT16 to a buffer, and advance the buffer pointer.
// The buffer has the value in Big Endian format.
//
void memwrite_INT16(BYTE*& buffer, INT16 value)
{
        memwrite_UINT16(buffer, (UINT16)value);
}

//
// Write an UINT16 to a buffer, and advance the buffer pointer.
// The buffer has the value in Big Endian format.
//
void memwrite_UINT16(BYTE*& buffer, UINT16 value)
{
        buffer[0] = ((value >> 8 ) & 0xFF);
        buffer[1] = ((value      ) & 0xFF);
        buffer += 2;
}

//
// Write an UINT8 to a buffer, and advance the buffer pointer.
//
void memwrite_UINT8(BYTE*& buffer, UINT8 value)
{
        buffer[0] = value;
        buffer++;
}

//
// Write an INT8 to a buffer, and advance the buffer pointer.
//
void memwrite_INT8(BYTE*& buffer, INT8 value)
{
        buffer[0] = value;
        buffer++;
}

//
// Write a string to a buffer, and advance the buffer pointer.
//
void memwrite_string(BYTE*& buffer, std::string text)
{
        strncpy((char*)buffer, text.c_str(), text.length());
        buffer += text.length();
}