Program Listing for File SafeVisionaryData.h

↰ Return to documentation for file (include/sick_safevisionary_base/SafeVisionaryData.h)

// -- 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