LidarData.h

浏览该文件的文档.

// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#pragma once
 
#include "carla/rpc/Location.h"
#include "carla/sensor/data/SemanticLidarData.h"
 
#include <cstdint>
#include <vector>
 
namespace carla {
 
namespace ros2 {
  class ROS2;
}
 
namespace sensor {
 
namespace s11n {
  class LidarSerializer;
  class LidarHeaderView;
}
 
namespace data {
 
  /// Helper class to store and serialize the data generated by a Lidar.
  ///
  /// The header of a Lidar measurement consists of an array of uint32_t's in
  /// the following layout
  ///
  ///    {
  ///      Horizontal angle (float),
  ///      Channel count,
  ///      Point count of channel 0,
  ///      ...
  ///      Point count of channel n,
  ///    }
  ///
  /// The points are stored in an array of floats
  ///
  ///    {
  ///      X0, Y0, Z0, I0
  ///      ...
  ///      Xn, Yn, Zn, In
  ///    }
  ///
 
  class LidarDetection {
    public:
      geom::Location point;
      float intensity;
 
      LidarDetection() :
          point(0.0f, 0.0f, 0.0f), intensity{0.0f} { }
      LidarDetection(float x, float y, float z, float intensity) :
          point(x, y, z), intensity{intensity} { }
      LidarDetection(geom::Location p, float intensity) :
          point(p), intensity{intensity} { }
 
      void WritePlyHeaderInfo(std::ostream& out) const{
        out << "property float32 x\n" \
          "property float32 y\n" \
          "property float32 z\n" \
          "property float32 I";
      }
 
      void WriteDetection(std::ostream& out) const{
        out << point.x << ' ' << point.y << ' ' << point.z << ' ' << intensity;
      }
  };
 
  class LidarData : public SemanticLidarData{
 
  public:
    explicit LidarData(uint32_t ChannelCount = 0u)
      : SemanticLidarData(ChannelCount) {
    }
 
    LidarData &operator=(LidarData &&) = default;
 
    ~LidarData() = default;
 
    virtual void ResetMemory(std::vector<uint32_t> points_per_channel) {
      DEBUG_ASSERT(GetChannelCount() > points_per_channel.size());
      std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount());
 
      uint32_t total_points = static_cast<uint32_t>(
          std::accumulate(points_per_channel.begin(), points_per_channel.end(), 0));
 
      _points.clear();
      _points.reserve(total_points * 4);
    }
 
    void WritePointSync(LidarDetection &detection) {
      _points.emplace_back(detection.point.x);
      _points.emplace_back(detection.point.y);
      _points.emplace_back(detection.point.z);
      _points.emplace_back(detection.intensity);
    }
 
    virtual void WritePointSync(SemanticLidarDetection &detection) {
      (void) detection;
      DEBUG_ASSERT(false);
    }
 
  private:
    std::vector<float> _points;
 
    friend class s11n::LidarSerializer;
    friend class s11n::LidarHeaderView;
    friend class carla::ros2::ROS2;
  };
 
} // namespace s11n
} // namespace sensor
} // namespace carla