LidarView源码分析（十一）vtkVelodyneMetaPacketInterpreter类

最新推荐文章于 2024-11-07 15:40:17 发布

上官天耀

最新推荐文章于 2024-11-07 15:40:17 发布

阅读量130

点赞数

分类专栏： LidarView 文章标签： c++ 自动驾驶信息可视化 qt

本文链接：https://blog.csdn.net/Luiiiiiiiiiis/article/details/131890051

版权

LidarView 专栏收录该内容

19 篇文章 4 订阅

订阅专栏

vtkVelodyneMetaPacketInterpreter的基类是vtkLidarPacketInterpreter，作用为根据情况选择不同的雷达数据包解析器。

vtkVelodyneMetaPacketInterpreter通过vtkCollection保存数据包解析器，使用vtkVelodyneBasePacketInterpreter*来指向当前正在使用的解析器。声明代码如下：

private:  
  //! Collection of interpreter that can be used by this meta interpreter
  vtkNew<vtkCollection> PotentialInterps;
  //! The interpreter actually used
  vtkVelodyneBasePacketInterpreter* SelectedInterp = nullptr;

在vtkVelodyneMetaPacketInterpreter的函数中，使用解析器指针调用相关的解析器方法，如ProcessPacket函数，定义如下：

  void ProcessPacket(unsigned char const * data, unsigned int dataLength) override { redirect_directly_function(ProcessPacket(data, dataLength)) }

其中redirect_directly_function是一个宏定义，功能为：首先确定this->SelectedInterp指针不为空，即存在可用的解析器，然后调用该指针的方法。定义如下：、

#define redirect_directly_function(function_call) \
  assert(this->SelectedInterp); \
  return this->SelectedInterp->function_call;

该类的头文件中还定义了一系列通过指针设定或获取对应解析器属性的宏定义，其基本流程是首先判断this->SelectedInterp是否存在，存在则通过该指针返回解析器或者对应的属性，如果不存在则取this->PoentialInterps中第一个解析器，进行相关的操作。这些宏定义的代码如下：

#define GenericSetMacro(name,type) \
void Set##name (type _arg) override \
{ \
  if (this->SelectedInterp) \
  { \
    this->SelectedInterp->Set##name(_arg);\
  } \
  else \
  { \
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i) \
    { \
      vtkLidarPacketInterpreter* pt = vtkLidarPacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i)); \
      pt->Set##name(_arg); \
    } \
  } \
}

#define GenericGetMacro(name, type) \
type Get##name () override \
{ \
  if (this->SelectedInterp) \
  { \
    return this->SelectedInterp->Get##name();\
  } \
  vtkLidarPacketInterpreter* pt = vtkLidarPacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(0)); \
  return pt->Get##name(); \
}

#define SpecificSetMacro(name,type) \
void Set##name (type _arg) \
{ \
  if (this->SelectedInterp) \
  { \
    this->SelectedInterp->Set##name(_arg); \
  } \
  else \
  { \
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i) \
    { \
      vtkVelodyneBasePacketInterpreter::SafeDownCast(PotentialInterps->GetItemAsObject(i))->Set##name(_arg); \
    } \
  } \
}

#define SpecificGetMacro(name, type) \
type Get##name () \
{ \
  if (this->SelectedInterp) \
  { \
    return this->SelectedInterp->Get##name(); \
  } \
  return vtkVelodyneBasePacketInterpreter::SafeDownCast(PotentialInterps->GetItemAsObject(0))->Get##name(); \
}


#define SpecificGetPtrMacro(name, ptrtype) \
ptrtype Get##name () \
{ \
  if (this->SelectedInterp) \
  { \
    return this->SelectedInterp->Get##name(); \
  } \
  return vtkVelodyneBasePacketInterpreter::SafeDownCast(PotentialInterps->GetItemAsObject(0))->Get##name(); \
}

在其构造函数中创建了对应的解析，使用QCollection::AddItem方法加入到解析器结合中，代码如下：

vtkVelodyneMetaPacketInterpreter::vtkVelodyneMetaPacketInterpreter()
{
  this->PotentialInterps->AddItem(vtkVelodyneLegacyPacketInterpreter::New());
  #if VELOVIEW_HAS_ADVANCED_INTERPRETER
  this->PotentialInterps->AddItem(vtkVelodyneAdvancedPacketInterpreter::New());
  #endif
  #if VELOVIEW_HAS_VELARRAY
  this->PotentialInterps->AddItem(vtkVelodyneSpecialVelarrayPacketInterpreter::New());
  #endif

  this->LaserSelection->SetNumberOfTuples(HDL_MAX_NUM_LASERS);
  this->LaserSelection->Fill(static_cast<int>(true));
}

并在this->LaserSelection设置该解析器的可用状态。this->LaserSelection的声明如下：

  //! Indicate for each laser if the points obtained by this specific laser
  //! should process/display (true) or ignore (false)
  vtkNew<vtkIntArray> LaserSelection;

剩下的代码都是通过指针调用对用解析器的具体方法。

class VELODYNEPACKETINTERPRETERS_EXPORT vtkVelodyneMetaPacketInterpreter : public vtkLidarPacketInterpreter
{
public:
  static vtkVelodyneMetaPacketInterpreter* New();
  vtkTypeMacro(vtkVelodyneMetaPacketInterpreter, vtkLidarPacketInterpreter)

  enum DualFlag
  {
    DUAL_DISTANCE_NEAR = 0x1,  // point with lesser distance
    DUAL_DISTANCE_FAR = 0x2,   // point with greater distance
    DUAL_INTENSITY_HIGH = 0x4, // point with lesser intensity
    DUAL_INTENSITY_LOW = 0x8,  // point with greater intensity
    DUAL_DOUBLED = 0xf,        // point is single return
    DUAL_DISTANCE_MASK = 0x3,
    DUAL_INTENSITY_MASK = 0xc,
  };

  void LoadCalibration(const std::string& filename) override;

  GenericGetMacro( CalibrationTable, vtkSmartPointer<vtkTable>)

  void ProcessPacket(unsigned char const * data, unsigned int dataLength) override { redirect_directly_function(ProcessPacket(data, dataLength)) }

  void ProcessPacketWrapped(unsigned char const * data, unsigned int dataLength, double PacketNetworkTime)
       override { redirect_directly_function(ProcessPacketWrapped(data, dataLength, PacketNetworkTime)) }

  bool SplitFrame(bool force = false, FramingMethod_t framingMethodAskingForSplitFrame = FramingMethod_t::INTERPRETER_FRAMING)
                  override { redirect_directly_function(SplitFrame(force, framingMethodAskingForSplitFrame)) }

  bool IsLidarPacket(unsigned char const * data, unsigned int dataLength) override;

  void ResetCurrentFrame() override;

  bool PreProcessPacket(unsigned char const * data, unsigned int dataLength,
                        fpos_t filePosition = fpos_t(), double packetNetworkTime = 0,
                        std::vector<FrameInformation>* frameCatalog = nullptr) override { redirect_directly_function(PreProcessPacket(data, dataLength, filePosition, packetNetworkTime, frameCatalog)) }

  bool PreProcessPacketWrapped(unsigned char const * data, unsigned int dataLength,
                            fpos_t filePosition = fpos_t(), double packetNetworkTime = 0,
                            std::vector<FrameInformation>* frameCatalog = nullptr)
                            override { redirect_directly_function(PreProcessPacketWrapped(data, dataLength, filePosition, packetNetworkTime, frameCatalog)) }

  bool IsNewFrameReady() override { redirect_directly_function(IsNewFrameReady()) }

  vtkSmartPointer<vtkPolyData> GetLastFrameAvailable() override { redirect_directly_function(GetLastFrameAvailable()) }

  void ClearAllFramesAvailable() override;

  std::string GetSensorInformation(bool shortVersion = false) override;

  std::string GetDefaultRecordFileName() override ;

  GenericGetMacro(ParserMetaData, FrameInformation)

  void ResetParserMetaData() override;

  void SetParserMetaData(const FrameInformation &metaData) override;

  GenericGetMacro(NumberOfChannels, int)

  GenericGetMacro(CalibrationFileName, std::string)
  GenericSetMacro(CalibrationFileName, std::string)

  GenericGetMacro(IsCalibrated, bool)

  GenericGetMacro(TimeOffset, double)
  GenericSetMacro(TimeOffset, double)

  vtkIntArray* GetLaserSelection() override;
  void SetLaserSelection(int index, int value) override;

  GenericGetMacro(DistanceResolutionM, double)

  GenericGetMacro(Frequency, double)
  GenericGetMacro(Rpm, double)

  GenericGetMacro(IgnoreZeroDistances, bool)
  GenericSetMacro(IgnoreZeroDistances, bool)

  void SetHideDropPoints(bool value)
  {
    if (this->SelectedInterp)
    {
      reinterpret_cast<vtkVelodyneBasePacketInterpreter*>(this->SelectedInterp)->SetHideDropPoints(value);
    }
    else
    {
      for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
      {
        vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
        pt->SetHideDropPoints(value);
      }
    }
  }

  GenericGetMacro(IgnoreEmptyFrames, bool)
  GenericSetMacro(IgnoreEmptyFrames, bool)

  GenericGetMacro(ApplyTransform, bool)
  GenericSetMacro(ApplyTransform, bool)

  GenericGetMacro(SensorTransform, vtkTransform*)
  GenericSetMacro(SensorTransform, vtkTransform*)

  GenericGetMacro(CropMode, int)
  GenericSetMacro(CropMode, int)

  GenericGetMacro(CropOutside, bool)
  GenericSetMacro(CropOutside, bool)

  void SetCropRegion(double _arg1, double _arg2, double _arg3, double _arg4, double _arg5, double _arg6) override;
  void SetCropRegion(double _arg[6]);

  void GetXMLColorTable(double XMLColorTable[]);

  void GetLaserCorrections(double verticalCorrection[HDL_MAX_NUM_LASERS],
    double rotationalCorrection[HDL_MAX_NUM_LASERS], double distanceCorrection[HDL_MAX_NUM_LASERS],
    double distanceCorrectionX[HDL_MAX_NUM_LASERS], double distanceCorrectionY[HDL_MAX_NUM_LASERS],
    double verticalOffsetCorrection[HDL_MAX_NUM_LASERS],
    double horizontalOffsetCorrection[HDL_MAX_NUM_LASERS], double focalDistance[HDL_MAX_NUM_LASERS],
    double focalSlope[HDL_MAX_NUM_LASERS], double minIntensity[HDL_MAX_NUM_LASERS],
                           double maxIntensity[HDL_MAX_NUM_LASERS]);

  SpecificGetMacro(HasDualReturn, bool)

  SpecificGetMacro(WantIntensityCorrection, bool)
  SpecificSetMacro(WantIntensityCorrection, bool)

  SpecificGetMacro(FiringsSkip, bool)
  SpecificSetMacro(FiringsSkip, bool)

  SpecificGetMacro(UseIntraFiringAdjustment, bool)
  SpecificSetMacro(UseIntraFiringAdjustment, bool)

  SpecificGetMacro(VLS128_UseIntraFiringTimingFromOldFWVersions, bool)
  SpecificSetMacro(VLS128_UseIntraFiringTimingFromOldFWVersions, bool)

  SpecificGetMacro(DualReturnFilter, bool)
  SpecificSetMacro(DualReturnFilter, bool)

  SpecificGetPtrMacro(_verticalCorrection, vtkDoubleArray*)
  SpecificGetPtrMacro(_rotationalCorrection, vtkDoubleArray*)
  SpecificGetPtrMacro(_distanceCorrection, vtkDoubleArray*)
  SpecificGetPtrMacro(_distanceCorrectionX, vtkDoubleArray*)
  SpecificGetPtrMacro(_distanceCorrectionY, vtkDoubleArray*)
  SpecificGetPtrMacro(_verticalOffsetCorrection, vtkDoubleArray*)
  SpecificGetPtrMacro(_horizontalOffsetCorrection, vtkDoubleArray*)
  SpecificGetPtrMacro(_focalDistance, vtkDoubleArray*)
  SpecificGetPtrMacro(_focalSlope, vtkDoubleArray*)
  SpecificGetPtrMacro(_minIntensity, vtkDoubleArray*)
  SpecificGetPtrMacro(_maxIntensity, vtkDoubleArray*)

  vtkMTimeType GetMTime() override;

  GenericSetMacro(EnableAdvancedArrays, bool)

  GenericSetMacro(FramingMethod, int)
  GenericGetMacro(FramingMethod, int)

  GenericSetMacro(FrameDuration_s, double)
  GenericGetMacro(FrameDuration_s, double)

protected:
    vtkVelodyneMetaPacketInterpreter();

    //! this function should never be called
    vtkSmartPointer<vtkPolyData> CreateNewEmptyFrame(vtkIdType numberOfPoints, vtkIdType prereservedNumberOfPoints = 0) override;

private:
  vtkVelodyneMetaPacketInterpreter(const vtkVelodyneMetaPacketInterpreter&) = delete;
  void operator=(const vtkVelodyneMetaPacketInterpreter&) = delete;

  //! Collection of interpreter that can be used by this meta interpreter
  vtkNew<vtkCollection> PotentialInterps;
  //! The interpreter actually used
  vtkVelodyneBasePacketInterpreter* SelectedInterp = nullptr;

  //! bool to know if a previous switch of the interpreter has been detected
  //! We need to detect the first switch to emit the signal to set a new calib file
  //! We don't need to ask the user the calibration file every time
  //! because every interpreter pt will save its own calibration file
  //! This attribute is only in the Meta interpreter class.
  bool SwitchAlreadyDetected = false;
};

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkPolyData> vtkVelodyneMetaPacketInterpreter::CreateNewEmptyFrame(
  vtkIdType vtkNotUsed(numberOfPoints), vtkIdType vtkNotUsed(prereservedNumberOfPoints))
{
  assert(false && "this function should never be called");
  return vtkSmartPointer<vtkPolyData>::New();
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::LoadCalibration(const std::string &filename)
{
  if (this->SelectedInterp)
  {
    this->SelectedInterp->LoadCalibration(filename);
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      pt->LoadCalibration(filename);
    }
  }
}

//-----------------------------------------------------------------------------
bool vtkVelodyneMetaPacketInterpreter::IsLidarPacket(const unsigned char *data, unsigned int dataLength)
{
  // The current selected interpreter can interpret the packet
  if (this->SelectedInterp && this->SelectedInterp->IsLidarPacket(data, dataLength))
  {
    return true;
  }

  // If there is no selected interpreter yet
  // or if the selected interpreter can not interpret the packet
  // We look if one of the potentials interpreter can interpret it
  for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
  {
    vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
    if (pt->IsLidarPacket(data, dataLength))
    {
      if (!this->SelectedInterp)
      {
        this->SelectedInterp = pt;
        return true;
      }
      else
      {
        // If there already was a Selected interpreter, this is a switch of the packet format
        // If it's the first one detected,
        // we send the signal so the user can select a new calibration file
        // We don't need a new calibration file at every switch because every interpreter
        // will save its own calibration file
        // This only work for a switch between 2 packet formats (tested for APF/Legacy switch)
        if(!this->SwitchAlreadyDetected)
        {
          this->InvokeEvent(FirstSwitchOfInterpreterEvent);
          this->SwitchAlreadyDetected = true;
        }
        this->SelectedInterp = pt;

        return true;
      }
    }
  }
  return false;
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::ResetCurrentFrame()
{
  if (this->SelectedInterp != nullptr)
  {
    this->SelectedInterp->ResetCurrentFrame();
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      pt->ResetCurrentFrame();
    }
  }
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::ClearAllFramesAvailable()
{
  if (this->SelectedInterp != nullptr)
  {
    this->SelectedInterp->ClearAllFramesAvailable();
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      pt->ClearAllFramesAvailable();
    }
  }
}

//-----------------------------------------------------------------------------
std::string vtkVelodyneMetaPacketInterpreter::GetSensorInformation(bool shortVersion)
{
  if (!this->SelectedInterp)
  {
    return "Could not determine the interpreter for now";
  }
  else
  {
    return this->SelectedInterp->GetSensorInformation(shortVersion);
  }
}

//-----------------------------------------------------------------------------
std::string vtkVelodyneMetaPacketInterpreter::GetDefaultRecordFileName()
{
  if (!this->SelectedInterp)
  {
    // YYYY-mm-dd-HH-MM-SS_Velodyne-Data
    return vtkLidarPacketInterpreter::GetDefaultRecordFileName() + "_Velodyne-Data";
  }
  else
  {
    // YYYY-mm-dd-HH-MM-SS_Velodyne-SensorName-Data
    return this->SelectedInterp->GetDefaultRecordFileName();
  }
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::ResetParserMetaData()
{
  if (this->SelectedInterp != nullptr)
  {
    this->SelectedInterp->ResetParserMetaData();
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      pt->ResetParserMetaData();
    }
  }
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::SetParserMetaData(const FrameInformation &metaData)
{
  if (this->SelectedInterp != nullptr)
  {
    this->SelectedInterp->SetParserMetaData(metaData);
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      pt->SetParserMetaData(metaData);
    }
  }
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::SetLaserSelection(int index, int value)
{
  if (this->SelectedInterp != nullptr)
  {

    this->SelectedInterp->SetLaserSelection(index, value);
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      pt->SetLaserSelection(index, value);
    }
  }
}

//-----------------------------------------------------------------------------
vtkIntArray* vtkVelodyneMetaPacketInterpreter::GetLaserSelection()
{
  if (this->SelectedInterp != nullptr)
  {
    return this->SelectedInterp->GetLaserSelection();
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      return pt->GetLaserSelection();
    }
  }

  return nullptr;
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::SetCropRegion(double _arg1, double _arg2, double _arg3, double _arg4, double _arg5, double _arg6)
{
  if (this->SelectedInterp != nullptr)
  {
    this->SelectedInterp->SetCropRegion(_arg1, _arg2, _arg3, _arg4, _arg5, _arg6);
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      pt->SetCropRegion(_arg1, _arg2, _arg3, _arg4, _arg5, _arg6);
    }
  }
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::SetCropRegion(double _arg[6])
{
  this->SetCropRegion(_arg[0], _arg[1], _arg[2], _arg[3], _arg[4], _arg[5]);
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::GetXMLColorTable(double XMLColorTable[])
{
  if (this->SelectedInterp)
  {
   this->SelectedInterp->GetXMLColorTable(XMLColorTable);
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      pt->GetXMLColorTable(XMLColorTable);
    }
  }
}

//-----------------------------------------------------------------------------
void vtkVelodyneMetaPacketInterpreter::GetLaserCorrections(double verticalCorrection[], double rotationalCorrection[], double distanceCorrection[], double distanceCorrectionX[], double distanceCorrectionY[], double verticalOffsetCorrection[], double horizontalOffsetCorrection[], double focalDistance[], double focalSlope[], double minIntensity[], double maxIntensity[])
{
  if (this->SelectedInterp)
  {
    this->SelectedInterp->GetLaserCorrections(verticalCorrection, rotationalCorrection, distanceCorrection, distanceCorrectionX, distanceCorrectionY, verticalOffsetCorrection, horizontalOffsetCorrection, focalDistance, focalSlope, minIntensity, maxIntensity);
  }
  else
  {
    for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
    {
      vtkVelodyneBasePacketInterpreter* pt = vtkVelodyneBasePacketInterpreter::SafeDownCast(this->PotentialInterps->GetItemAsObject(i));
      pt->GetLaserCorrections(verticalCorrection, rotationalCorrection, distanceCorrection, distanceCorrectionX, distanceCorrectionY, verticalOffsetCorrection, horizontalOffsetCorrection, focalDistance, focalSlope, minIntensity, maxIntensity);
    }
  }
}

//-----------------------------------------------------------------------------
vtkMTimeType vtkVelodyneMetaPacketInterpreter::GetMTime()
{
  vtkMTimeType time = this->Superclass::GetMTime();
  for (int i = 0; i < this->PotentialInterps->GetNumberOfItems(); ++i)
  {
    time = std::max(time, this->PotentialInterps->GetItemAsObject(i)->GetMTime());
  }
  return time;
}