SafeVisionaryData.h

Go to the documentation of this file.

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
 
#pragma once
 
#include <string>
#include <vector>
 
#include "VisionaryData.h"
 
#define DEPTHMAP_SEGMENT 1
#define DEVICESTATUS_SEGMENT 2
#define ROI_SEGMENT 3
#define LOCALIOS_SEGMENT 4
#define FIELDINFORMATION_SEGMENT 5
#define LOGICSIGNALS_SEGMENT 6
#define IMU_SEGMENT 7
 
namespace visionary {
enum class DEVICE_STATUS : std::uint8_t
{
  DEVICE_STATUS_CONFIGURATION       = 0U, 
  DEVICE_STATUS_WAIT_FOR_INPUTS     = 1U, 
  DEVICE_STATUS_APPLICATION_STOPPED = 2U, 
  DEVICE_STATUS_NORMAL_OPERATION =
    3U,                        
  DEVICE_STATUS_INVALID = 255U 
};
 
/*
 * Segment 1  DepthMap
 */
#pragma pack(push, 1)
 
struct DEPTHMAP_FLAGS
{
  uint16_t filteredDepthMap : 1;
  uint16_t intrusionData : 1;
  uint16_t dataStream : 1;
  uint16_t reserved : 13;
};
 
struct DEPTHMAP_ELEMENT
{
  uint32_t frameNumber;
  DEVICE_STATUS deviceStatus;
  DEPTHMAP_FLAGS flags;
  uint16_t distance;
  uint16_t intensity;
  uint16_t confidence;
};
 
#pragma pack(pop)
 
/*
 * Segment 2  Device Status
 */
#pragma pack(push, 1)
 
struct DEVICESTATUS_DATA_GENERALSTATUS
{
  uint8_t runModeActive : 1;
  uint8_t deviceError : 1;
  uint8_t applicationError : 1;
  uint8_t sleepMode : 1;
  uint8_t waitForInput : 1;
  uint8_t waitForCluster : 1;
  uint8_t contaminationWarning : 1;
  uint8_t contaminationError : 1;
  uint8_t deadZoneDetection : 1;
  uint8_t temperatureWarning : 1;
  uint8_t reserved : 6;
};
 
struct DEVICESTATUS_DATA_ACTIVE_MONITORING_CASE
{
  uint8_t currentCaseNumberMonitoringCase1 : 8;
  uint8_t currentCaseNumberMonitoringCase2 : 8;
  uint8_t currentCaseNumberMonitoringCase3 : 8;
  uint8_t currentCaseNumberMonitoringCase4 : 8;
};
 
struct DEVICE_STATUS_ELEMENT
{
  DEVICESTATUS_DATA_GENERALSTATUS generalStatus;
  uint32_t COPSaftyRelated;    // Cut-Off-Path (safety related)
  uint32_t COPNonSaftyRelated; // Cut-Off-Path non safety related
  uint32_t COPResetRequired;   // Cut-Off-Path Reset Required
  DEVICESTATUS_DATA_ACTIVE_MONITORING_CASE activeMonitoringCase;
  uint8_t contaminationLevel;
};
 
#pragma pack(pop)
 
/*
 * Segment 3  ROI
 */
#pragma pack(push, 1)
 
struct ROI_DATA_RESULT
{
  uint8_t taskResult : 1;
  uint8_t resultSafe : 1;
  uint8_t resultValid : 1;
  uint8_t distanceValid : 1;
  uint8_t distanceSafe : 1;
  uint8_t reserved : 3;
};
 
enum ROI_QUALITY_CLASS
{
 
  ROI_QUALITY_CLASS_INVALID  = 0,
  ROI_QUALITY_CLASS_HIGH     = 1,
  ROI_QUALITY_CLASS_MODERATE = 2,
  ROI_QUALITY_CLASS_LOW      = 3
 
};
 
union ROI_DATA_SAFETY_DATA
{
  struct
  {
    uint8_t invalidDueToInvalidPixels : 1;
    uint8_t invalidDueToVariance : 1;
    uint8_t invalidDueToOverexposure : 1;
    uint8_t invalidDueToUnderexposure : 1;
    uint8_t invalidDueToTemporalVariance : 1;
    uint8_t invalidDueToOutsideOfMeasurementRange : 1;
    uint8_t invalidDueToRetroReflectorInterference : 1;
    uint8_t contaminationError : 1;
    uint8_t qualityClass : 2;
    uint8_t slotActive : 1;
    uint8_t reserved : 5;
 
  } tMembers; 
  uint16_t u16All;
};
 
struct ROI_ELEMENT
{
  uint8_t id;
  ROI_DATA_RESULT result;
  ROI_DATA_SAFETY_DATA safetyRelatedData;
  uint16_t distanceValue;
};
 
enum class DataHandlerError
{
  OK,
  PARSE_XML_ERROR,
  INVALID_CRC_SEGMENT_DEPTHMAP,
  INVALID_LENGTH_SEGMENT_DEPTHMAP,
  INVALID_VERSION_SEGMENT_DEPTHMAP,
  INVALID_CRC_SEGMENT_DEVICESTATUS,
  INVALID_LENGTH_SEGMENT_DEVICESTATUS,
  INVALID_VERSION_SEGMENT_DEVICESTATUS,
  INVALID_CRC_SEGMENT_ROI,
  INVALID_LENGTH_SEGMENT_ROI,
  INVALID_VERSION_SEGMENT_ROI,
  INVALID_CRC_SEGMENT_LOCALIOS,
  INVALID_LENGTH_SEGMENT_LOCALIOS,
  INVALID_VERSION_SEGMENT_LOCALIOS,
  INVALID_CRC_SEGMENT_FIELDINFORMATION,
  INVALID_LENGTH_SEGMENT_FIELDINFORMATION,
  INVALID_VERSION_SEGMENT_FIELDINFORMATION,
  INVALID_CRC_SEGMENT_LOGICSIGNALS,
  INVALID_LENGTH_SEGMENT_LOGICSIGNALS,
  INVALID_VERSION_SEGMENT_LOGICSIGNALS,
  INVALID_CRC_SEGMENT_IMU,
  INVALID_LENGTH_SEGMENT_IMU,
  INVALID_VERSION_SEGMENT_IMU
};
 
constexpr std::uint32_t MAX_ROI_VALUES = 5u;
 
struct ROI_DATA
{
  ROI_ELEMENT roiData[MAX_ROI_VALUES];
};
#pragma pack(pop)
 
/*
 * Segment 4  Local I/Os
 */
#pragma pack(push, 1)
 
struct LOCALIOS_UNIVERSALIO_CONFIGURED
{
  uint16_t configuredUniIOPin5 : 1;
  uint16_t configuredUniIOPin6 : 1;
  uint16_t configuredUniIOPin7 : 1;
  uint16_t configuredUniIOPin8 : 1;
  uint16_t reserved : 12;
};
 
struct LOCALIOS_UNIVERSALIO_DIRECTION
{
  uint16_t directionValueUniIOPin5 : 1;
  uint16_t directionValueUniIOPin6 : 1;
  uint16_t directionValueUniIOPin7 : 1;
  uint16_t directionValueUniIOPin8 : 1;
  uint16_t reserved : 12;
};
 
struct LOCALIOS_UNIVERSALIO_INPUTVALUES
{
  uint16_t logicalValueUniIOPin5 : 1;
  uint16_t logicalValueUniIOPin6 : 1;
  uint16_t logicalValueUniIOPin7 : 1;
  uint16_t logicalValueUniIOPin8 : 1;
  uint16_t reserved : 12;
};
struct LOCALIOS_UNIVERSALIO_OUTPUTVALUES
{
  uint8_t localOutput1Pin5;
  uint8_t localOutput2Pin6;
  uint8_t localOutput3Pin7;
  uint8_t localOutput4Pin8;
  uint8_t reserved[12]; // Byte 4 bis 15 are reserved
};
 
struct LOCALIOS_OSSDS_STATE
{
  uint8_t stateOSSD1A : 1;
  uint8_t stateOSSD1B : 1;
  uint8_t stateOSSD2A : 1;
  uint8_t stateOSSD2B : 1;
  uint8_t reserved : 4;
};
 
struct LOCALIOS_ELEMENT
{
  LOCALIOS_UNIVERSALIO_CONFIGURED universalIOConfigured;
  LOCALIOS_UNIVERSALIO_DIRECTION universalIODirection;
  LOCALIOS_UNIVERSALIO_INPUTVALUES universalIOInputValue;
  LOCALIOS_UNIVERSALIO_OUTPUTVALUES universalIOOutputValue;
  LOCALIOS_OSSDS_STATE ossdsState;
  uint8_t ossdsDynCount;      // reserved
  uint8_t ossdsCRC;           // reserved
  uint8_t ossdsIOStatus;      // reserved
  uint16_t dynamicSpeedA;     // reserved
  uint16_t dynamicSpeedB;     // reserved
  uint16_t DynamicValidFlags; // reserved
  uint16_t flags;             // reserved
};
 
#pragma pack(pop)
 
/*
 * Segment 5 Field Information
 */
#pragma pack(push, 1)
 
struct FIELDINFORMATION_ELEMENT
{
  uint8_t fieldID;
  uint8_t fieldSetID;
  uint8_t fieldResult;
  uint8_t evalMethod;
  uint8_t fieldActive;
};
constexpr std::uint32_t MAX_FIELDINFORMATION_VALUES = 16u;
 
struct FIELDINFORMATION_DATA
{
  FIELDINFORMATION_ELEMENT fieldInformation[MAX_FIELDINFORMATION_VALUES];
};
#pragma pack(pop)
 
/*
 * Segment 6 Logic Signals
 */
#pragma pack(push, 1)
 
struct LOGICSIGNALS_INSTANCESTATE
{
  uint8_t instanceOSSD1 : 1;
  uint8_t instanceOSSD2 : 1;
  uint8_t reserved : 6;
};
 
struct LOGICSIGNALS_ELEMENT
{
  uint8_t signalType;
  uint8_t instance;
  uint16_t configured : 1;
  uint16_t signalDirection : 1;
  uint16_t reserved : 14;
  uint16_t value;
};
 
constexpr std::uint32_t MAX_LOGICSIGNALS_VALUES = 20u;
 
struct LOGICSIGNALS_DATA
{
  LOGICSIGNALS_ELEMENT logicSignals[MAX_LOGICSIGNALS_VALUES];
};
 
#pragma pack(pop)
 
/*
 * Segment 7  IMU
 */
#pragma pack(push, 1)
 
struct IMU_VECTOR
{
  float X;
  float Y;
  float Z;
  uint8_t accuracy;
};
 
struct IMU_QUATERNION
{
  float X;
  float Y;
  float Z;
  float W;
  float accuracy;
};
 
struct IMU_ELEMENT
{
  IMU_VECTOR acceleration;
  IMU_VECTOR angularVelocity;
  IMU_QUATERNION orientation;
};
 
#pragma pack(pop)
 
class SafeVisionaryData : public VisionaryData
{
public:
  SafeVisionaryData();
  virtual ~SafeVisionaryData();
 
  //-----------------------------------------------
  // Getter Functions
 
  const std::vector<uint16_t>& getDistanceMap() const;
 
  const std::vector<uint16_t>& getIntensityMap() const;
 
  const std::vector<uint8_t>& getStateMap() const;
 
  uint16_t getFlags() const;
 
  DEVICE_STATUS getDeviceStatus() const;
 
  DEVICE_STATUS_ELEMENT getDeviceStatusData() const;
 
  const ROI_DATA& getRoiData() const;
 
  LOCALIOS_ELEMENT getLocalIOData() const;
 
  const FIELDINFORMATION_DATA& getFieldInformationData() const;
 
  const LOGICSIGNALS_DATA& getLogicSignalsData() const;
 
  const IMU_ELEMENT getIMUData() const;
 
  bool isDistanceMapFiltered() const;
 
  bool isIntrudedPixelStateValid() const;
 
  void generatePointCloud(std::vector<PointXYZ>& pointCloud) override;
 
  static const float DISTANCE_MAP_UNIT;
 
  DataHandlerError getLastError();
 
  DataSetsActive getDataSetsActive();
 
  void clearData(uint32_t changedCounter);
 
protected:
  //-----------------------------------------------
  // functions for parsing received blob
 
  bool parseXML(const std::string& xmlString, uint32_t changeCounter);
 
  bool parseBinaryData(std::vector<uint8_t>::iterator itBuf, size_t length);
 
  bool parseRoiData(std::vector<uint8_t>::iterator itBuf, size_t length);
 
  bool parseDeviceStatusData(std::vector<uint8_t>::iterator itBuf, size_t length);
 
  bool parseLocalIOsData(std::vector<uint8_t>::iterator itBuf, size_t length);
 
  bool parseFieldInformationData(std::vector<uint8_t>::iterator itBuf, size_t length);
 
  bool parseLogicSignalsData(std::vector<uint8_t>::iterator itBuf, size_t length);
 
  bool parseIMUData(std::vector<uint8_t>::iterator itBuf, size_t length);
 
private:
  // Indicator for the received data sets
  DataSetsActive m_dataSetsActive;
 
  uint32_t m_distanceByteDepth;
 
  uint32_t m_intensityByteDepth;
 
  uint32_t m_stateByteDepth;
 
  std::vector<uint16_t> m_distanceMap;
 
  std::vector<uint16_t> m_intensityMap;
 
  std::vector<uint8_t> m_stateMap;
 
  DEVICE_STATUS m_deviceStatus;
 
  ROI_DATA m_roiData;
 
  // Contains the Device status data
  DEVICE_STATUS_ELEMENT m_deviceStatusData;
 
  // Contains the Local I/Os
  LOCALIOS_ELEMENT m_localIOsData;
 
  // Contains the Field Information data
  FIELDINFORMATION_DATA m_fieldInformationData;
 
  // Contains the Logic Signal data
  LOGICSIGNALS_DATA m_logicSignalsData;
 
  // Contains the IMU data
  IMU_ELEMENT m_IMUData;
 
  uint16_t m_flags;
 
  DataHandlerError m_lastDataHandlerError;
};
 
} // namespace visionary