OrientedTactileSensorUnit.h

Go to the documentation of this file.

#pragma once
 
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/units/UnitInterface.h>
#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <netinet/in.h>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <Eigen/Dense>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
namespace armarx
{
    class OrientedTactileSensorUnitPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        OrientedTactileSensorUnitPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "SerialInterfaceDevice",
                "/dev/ttyACM0",
                "The serial device the arduino is connected to.");
 
            defineOptionalProperty<std::string>(
                "TopicName",
                "OrientedTactileSensorValues",
                "Name of the topic on which the sensor values are provided");
 
            defineOptionalProperty<std::string>(
                "CalibrationData",
                "65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
                "Sensor Register Data to calibrate the sensor");
 
            defineOptionalProperty<std::string>(
                "SamplesRotation",
                "20",
                "number of orientation values to differentiate");
 
            defineOptionalProperty<std::string>(
                "SamplesPressure",
                "10",
                "number of pressure values to differentiate");
 
            defineOptionalProperty<std::string>(
                "SamplesAcceleration",
                "20",
                "number of pressure values to differentiate");
 
            defineOptionalProperty<bool>(
                "logData",
                "false",
                "log data from sensor");
            defineOptionalProperty<bool>(
                "calibrateSensor",
                "false"
                "Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
            defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "Name of the debug drawer topic that should be used");
        }
    };
 
    /**
     * @class OrientedTactileSensorUnit
     * @brief ArmarX wrapper for an arduino due with one BNO055 IMU and one BMP280 pressure sensor
     *
     */
    class OrientedTactileSensorUnit:
        virtual public armarx::Component
    //TODO: needs interface to send calibration data
    {
    public:
        OrientedTactileSensorUnit();
 
        std::string getDefaultName() const override
        {
            return "OrientedTactileSensorUnit";
        }
 
        struct SensorData
        {
            int id;
            float pressure;
            float qw, qx, qy, qz;
            float accelx, accely, accelz;
        };
 
        struct CalibrationData
        {
            int accel_offset_x, accel_offset_y, accel_offset_z, gyro_offset_x, gyro_offset_y, gyro_offset_z, mag_offset_x, mag_offset_y, mag_offset_z, accel_radius, mag_radius;
        };
 
        struct PressureRate
        {
            IceUtil::Time timestamp;
            float pressure;
        };
 
        struct RotationRate
        {
            IceUtil::Time timestamp;
            Eigen::Quaternionf orientation;
        };
 
        struct AccelerationRate
        {
            IceUtil::Time timestamp;
            float rotationRate;
        };
        struct LinAccRate
        {
            IceUtil::Time timestamp;
            float accx, accy, accz;
        };
 
    protected:
        void onInitComponent() override;
        void onConnectComponent() override;
 
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    private:
        std::ifstream arduinoIn;
        std::ofstream arduinoOut;
        RunningTask<OrientedTactileSensorUnit>::pointer_type readTask;
        OrientedTactileSensorUnitListenerPrx topicPrx;
        OrientedTactileSensorUnitInterfacePrx interfacePrx;
 
        void run();
        SensorData getValues(std::string line);
        bool getCalibrationValues(std::string line);
        bool loadCalibration();
        int fd;
        CalibrationData calibration;
 
        Eigen::Quaternionf inverseOrientation;
        std::vector<RotationRate> samplesRotation;
        std::vector<PressureRate> samplesPressure;
        std::vector<AccelerationRate> samplesAcceleration;
        std::vector<float> pressureRates;
        //Eigen::AngleAxisf aa;
        int maxSamplesRotation;
        int sampleIndexRotation;
        int maxSamplesPressure;
        int sampleIndexPressure;
        int maxSamplesAcceleration;
        int sampleIndexAcceleration;
        int sampleIndexPressureRate;
        float sumPressureRates;
        Eigen::Matrix3f sumOrientation;
        bool first;
        int i = 0;
        DebugDrawerInterfacePrx debugDrawerTopic;
        SimpleJsonLoggerPtr logger;
        std::string prefix;
    };
}