OrientedTactileSensorUnit.cpp

Go to the documentation of this file.

#include "OrientedTactileSensorUnit.h"
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <math.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <RobotAPI/libraries/core/Pose.h>
 
using namespace armarx;
 
OrientedTactileSensorUnit::OrientedTactileSensorUnit()
{
 
    sampleIndexRotation = 0;
    sampleIndexPressure = 0;
    sampleIndexAcceleration = 0;
    sampleIndexPressureRate = 0;
}
 
void OrientedTactileSensorUnit::onInitComponent()
{
    //logger part
    if (getProperty<bool>("logData").getValue())
    {
        IceUtil::Time now = IceUtil::Time::now();
        time_t timer = now.toSeconds();
        struct tm* ts;
        char buffer[80];
        ts = localtime(&timer);
        strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", ts);
        std::string packageName = "RobotAPI";
        armarx::CMakePackageFinder finder(packageName);
        std::string dataDir = finder.getDataDir() + "/" + packageName + "/logs/";
        std::string filename = dataDir +  buffer + std::string("_data")  + ".json";
        //ARMARX_IMPORTANT << filename;
 
        logger.reset(new SimpleJsonLogger(filename, true));
        prefix = std::string(buffer);
    }
    maxSamplesRotation = stoi(getProperty<std::string>("SamplesRotation").getValue());
    maxSamplesPressure = stoi(getProperty<std::string>("SamplesPressure").getValue());
    maxSamplesAcceleration = stoi(getProperty<std::string>("SamplesAcceleration").getValue());
 
    std::string topicName = getProperty<std::string>("TopicName").getValue();
    offeringTopic(topicName);
 
    //open serial port
    std::string portname = getProperty<std::string>("SerialInterfaceDevice").getValue();
    arduinoIn.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
    arduinoOut.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
 
    fd = open(portname.c_str(), O_RDWR | O_NOCTTY);
    struct termios toptions;
 
    /* Get currently set options for the tty */
    tcgetattr(fd, &toptions);
 
    /* Set custom options */
    cfsetispeed(&toptions, B115200);
    cfsetospeed(&toptions, B115200);
 
    /* 8 bits, no parity, no stop bits */
    toptions.c_lflag = 0;
    toptions.c_iflag = 0;
    toptions.c_oflag = 0;
 
    /* commit the options */
    tcsetattr(fd, TCSANOW, &toptions);
 
    /* Wait for the Arduino to reset */
    usleep(1000 * 1000);
 
    /* Flush anything already in the serial buffer */
    tcflush(fd, TCIFLUSH);
 
    ARMARX_INFO << "opening device " << getProperty<std::string>("SerialInterfaceDevice").getValue();
 
    if (!arduinoIn.is_open())
    {
 
        throw LocalException("Cannot open Arduino on ") << getProperty<std::string>("SerialInterfaceDevice").getValue();
    }
 
    ARMARX_INFO << "Arduino restarts, please wait ...";
 
    //wait for the Arduino to reboot
    usleep(4000000);
    std::string arduinoLine;
 
    //wait for the IMU to be calibrated or load calibration
    ARMARX_INFO << "waiting for IMU calibration - this can take some time";
    if (getProperty<bool>("calibrateSensor").getValue())
    {
        //calibrate
 
        while (arduinoLine.find("mode") == std::string::npos)
        {
            getline(arduinoIn, arduinoLine, '\n');
        }
 
        arduinoOut << "calibrate";
        arduinoOut.flush();
 
        while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
        {
            getline(arduinoIn, arduinoLine, '\n');
            ARMARX_INFO << arduinoLine;
        }
        getline(arduinoIn, arduinoLine, '\n');
        if (getCalibrationValues(arduinoLine))
        {
            ARMARX_IMPORTANT << "calibrated sensor";
        }
    }
    else
    {
        //load calibration
        ARMARX_INFO << "load calibration data " << getProperty<std::string>("CalibrationData").getValue();
        if (loadCalibration())
        {
            ARMARX_IMPORTANT << "loaded calibration";
            first = true;
            /*std::string line;
            getline(arduinoIn, line, '\n');
            ARMARX_IMPORTANT<<line;
            SensorData dataInit = getValues(line.c_str());
            ARMARX_IMPORTANT<<dataInit.qw<<" "dataInit.qx<<" "<<dataInit.qy<<" "<<dataInit.qz;
            Eigen::Quaternionf initialOrientation =  Eigen::Quaternionf(dataInit.qw, dataInit.qx, dataInit.qy, dataInit.qz);
            inverseOrientation = initialOrientation.inverse();*/
        }
    }
    readTask = new RunningTask<OrientedTactileSensorUnit>(this, &OrientedTactileSensorUnit::run);
    readTask->start();
 
}
 
void OrientedTactileSensorUnit::onConnectComponent()
{
    debugDrawerTopic = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
    std::string topicName = getProperty<std::string>("TopicName").getValue();
    topicPrx = getTopic<OrientedTactileSensorUnitListenerPrx>(topicName);
}
 
PropertyDefinitionsPtr OrientedTactileSensorUnit::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new OrientedTactileSensorUnitPropertyDefinitions(
                                      getConfigIdentifier()));
}
 
void OrientedTactileSensorUnit::run()
{
    while (readTask->isRunning())
    {
        std::string line;
        getline(arduinoIn, line, '\n');
        SensorData data = getValues(line.c_str());
        IceUtil::Time now = IceUtil::Time::now();
        TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
 
        //compute rotationRate
        float rotationRate = 0;
 
        //condition for inverse quaternion
        if ((pow(data.qw, 2) + pow(data.qx, 2) + pow(data.qy, 2) + pow(data.qz, 2)) != 0)
        {
            RotationRate sampleRotation;
            sampleRotation.timestamp = now;
            sampleRotation.orientation =  Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
            //sampleRotation.orientation = sampleRotation.orientation * inverseOrientation;
            if (0 < maxSamplesRotation  && samplesRotation.size() < static_cast<std::size_t>(maxSamplesRotation))
            {
                samplesRotation.push_back(sampleRotation);
            }
            else
            {
                samplesRotation[sampleIndexRotation].timestamp = sampleRotation.timestamp;
                samplesRotation[sampleIndexRotation].orientation = sampleRotation.orientation;
                sampleIndexRotation = (sampleIndexRotation + 1) % maxSamplesRotation;
                RotationRate oldsampleRotation;
                oldsampleRotation.timestamp = samplesRotation.at(sampleIndexRotation).timestamp;
                oldsampleRotation.orientation = samplesRotation.at(sampleIndexRotation).orientation;
                Eigen::AngleAxisf aa(sampleRotation.orientation * oldsampleRotation.orientation.inverse());
                //ARMARX_IMPORTANT << "aa: " << aa.axis() << " " << aa.angle();
                rotationRate = aa.angle() / (sampleRotation.timestamp - oldsampleRotation.timestamp).toSecondsDouble();
            }
        }
        //compute pressureRate
        float pressureRate = 0;
        PressureRate samplePressure;
        samplePressure.timestamp = now;
        samplePressure.pressure = data.pressure;
        if (0 < maxSamplesPressure && samplesPressure.size() < static_cast<std::size_t>(maxSamplesPressure))
        {
            samplesPressure.push_back(samplePressure);
        }
        else
        {
            samplesPressure[sampleIndexPressure] = samplePressure;
            sampleIndexPressure = (sampleIndexPressure + 1) % maxSamplesPressure;
            PressureRate oldsamplePressure;
            oldsamplePressure.timestamp = samplesPressure.at(sampleIndexPressure).timestamp;
            oldsamplePressure.pressure = samplesPressure.at(sampleIndexPressure).pressure;
            pressureRate = (samplePressure.pressure - oldsamplePressure.pressure) / (samplePressure.timestamp - oldsamplePressure.timestamp).toSecondsDouble();
        }
 
        //compute angular accceleration Rate
        float accelerationRate = 0;
        AccelerationRate sampleAcceleration;
        sampleAcceleration.timestamp = now;
        sampleAcceleration.rotationRate = rotationRate;
        if (0 < maxSamplesAcceleration && samplesAcceleration.size() < static_cast<std::size_t>(maxSamplesAcceleration))
        {
            samplesAcceleration.push_back(sampleAcceleration);
        }
        else
        {
            samplesAcceleration[sampleIndexAcceleration] = sampleAcceleration;
            sampleIndexAcceleration = (sampleIndexAcceleration + 1) % maxSamplesAcceleration;
            AccelerationRate oldsampleAcceleration;
            oldsampleAcceleration.timestamp = samplesAcceleration.at(sampleIndexAcceleration).timestamp;
            oldsampleAcceleration.rotationRate = samplesAcceleration.at(sampleIndexAcceleration).rotationRate;
            accelerationRate = (sampleAcceleration.rotationRate - oldsampleAcceleration.rotationRate) / (sampleAcceleration.timestamp - oldsampleAcceleration.timestamp).toSecondsDouble();
        }
        if (0 < maxSamplesPressure && pressureRates.size() < static_cast<std::size_t>(maxSamplesPressure))
        {
            pressureRates.push_back(pressureRate);
        }
        else
        {
            pressureRates[sampleIndexPressureRate] = pressureRate;
            sampleIndexPressureRate = (sampleIndexPressureRate + 1) % maxSamplesPressure;
        }
        if (pressureRate > 50)
        {
            ARMARX_IMPORTANT << "contact";
        }
 
        Eigen::Quaternionf orientationQuaternion = Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
        if (getProperty<bool>("logData").getValue())
        {
 
            if (i < 50)
            {
                inverseOrientation = orientationQuaternion.inverse();
                i++;
            }
            Eigen::Matrix3f quatMatrix = orientationQuaternion.toRotationMatrix();
            Eigen::Matrix4f quat4Matrix = Eigen::Matrix4f::Identity();
 
            quat4Matrix.block(0, 0, 3, 3) = quatMatrix;
 
            Eigen::Vector3f linearAcceleration(data.accelx, data.accely, data.accelz);
            SimpleJsonLoggerEntry e;
            e.AddTimestamp();
            e.Add("Pressure", data.pressure);
            e.Add("PressureRate", pressureRate);
            e.Add("RotationRate", rotationRate);
            e.AddAsArr("Orientation", quat4Matrix);
            e.AddAsArr("Linear Acceleration", linearAcceleration);
            logger->log(e);
        }
        /*Eigen::Matrix3f rotZ;
        rotZ(0, 0) = 0;
        rotZ(0, 1) = 1;
        rotZ(0, 2) = 0;
        rotZ(1, 0) = -1;
        rotZ(1, 1) = 0;
        rotZ(1, 2) = 0;
        rotZ(2, 0) = 0;
        rotZ(2, 1) = 0;
        rotZ(2, 2) = 1;
        Eigen::Matrix3f rotX;
        rotX(0, 0) = 1;
        rotX(0, 1) = 0;
        rotX(0, 2) = 0;
        rotX(1, 0) = 0;
        rotX(1, 1) = -1;
        rotX(1, 2) = 0;
        rotX(2, 0) = 0;
        rotX(2, 1) = 0;
        rotX(2, 2) = -1;*/
        Eigen::Matrix3f rotY;
        rotY(0, 0) = 0;
        rotY(0, 1) = 0;
        rotY(0, 2) = 1;
        rotY(1, 0) = 0;
        rotY(1, 1) = 1;
        rotY(1, 2) = 0;
        rotY(2, 0) = -1;
        rotY(2, 1) = 0;
        rotY(2, 2) = 0;
        Eigen::Matrix3f rawOrientation = orientationQuaternion.toRotationMatrix();
 
        PosePtr pose = new Pose(rawOrientation, Eigen::Vector3f(100.0, 200.0, 0.0));
        if (debugDrawerTopic)
        {
            debugDrawerTopic->setPoseVisu("debugdrawerlayer", "pose", pose);
        }
        Eigen::Quaternionf quaternion(rawOrientation);
        data.qw = quaternion.w();
        data.qx = quaternion.x();
        data.qy = quaternion.y();
        data.qz = quaternion.z();
        ARMARX_IMPORTANT << "or " << orientationQuaternion.w() << " " << orientationQuaternion.x() << " " << orientationQuaternion.y() << " " << orientationQuaternion.z();
        if (topicPrx)
        {
            topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, accelerationRate, data.accelx, data.accely, data.accelz, nowTimestamp);
        }
    }
 
}
 
// get imu values from incoming string
OrientedTactileSensorUnit::SensorData OrientedTactileSensorUnit::getValues(std::string line)
{
    SensorData data;
    std::vector<std::string> splitValues = Split(line, " ");
    data.id = stoi(splitValues.at(0));
    data.pressure = std::stof(splitValues.at(1));
    data.qw = std::stof(splitValues.at(2));
    data.qx = std::stof(splitValues.at(3));
    data.qy = std::stof(splitValues.at(4));
    data.qz = std::stof(splitValues.at(5));
    data.accelx = std::stof(splitValues.at(6));
    data.accely = std::stof(splitValues.at(7));
    data.accelz = std::stof(splitValues.at(8));
    return data;
}
 
std::string space = " ";
bool OrientedTactileSensorUnit::loadCalibration()
{
    std::string calibrationStream = getProperty<std::string>("CalibrationData").getValue();
    std::string arduinoLine;
    while (arduinoLine.find("mode") == std::string::npos)
    {
        getline(arduinoIn, arduinoLine, '\n');
    }
    arduinoOut << "load";
    arduinoOut.flush();
    while (arduinoLine.find("calibration data") == std::string::npos)
    {
        getline(arduinoIn, arduinoLine, '\n');
    }
 
    arduinoOut << calibrationStream;
    arduinoOut.flush();
 
    while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
    {
        getline(arduinoIn, arduinoLine, '\n');
    }
    return true;
}
 
bool OrientedTactileSensorUnit::getCalibrationValues(std::string line)
{
    std::vector<std::string> splitValues = Split(line, " ");
    calibration.accel_offset_x = stoi(splitValues.at(0));
    calibration.accel_offset_y = stoi(splitValues.at(1));
    calibration.accel_offset_z = stoi(splitValues.at(2));
    calibration.gyro_offset_x = stoi(splitValues.at(3));
    calibration.gyro_offset_y = stoi(splitValues.at(4));
    calibration.gyro_offset_z = stoi(splitValues.at(5));
    calibration.mag_offset_x = stoi(splitValues.at(6));
    calibration.mag_offset_y = stoi(splitValues.at(7));
    calibration.mag_offset_z = stoi(splitValues.at(8));
    calibration.accel_radius = stoi(splitValues.at(9));
    calibration.mag_radius = stoi(splitValues.at(10));
    std::string space = " ";
    std::string calibrationStream = "";
    calibrationStream = calibrationStream + to_string(calibration.accel_offset_x) + space + to_string(calibration.accel_offset_y) + space + to_string(calibration.accel_offset_z) + space + to_string(calibration.gyro_offset_x) + space + to_string(calibration.gyro_offset_y) + space +
                        to_string(calibration.gyro_offset_z) + space + to_string(calibration.mag_offset_x) + space + to_string(calibration.mag_offset_y) + space + to_string(calibration.mag_offset_z) + space + to_string(calibration.accel_radius) + space + to_string(calibration.mag_radius) + space;
    ARMARX_IMPORTANT << "calibration data: " << calibrationStream ;
    return true;
}