lane_sensor_functional.hpp

Go to the documentation of this file.

/*
 * Copyright 2022 Robert Bosch GmbH
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
#pragma once
 
#include <functional>  // for function
#include <memory>      // for shared_ptr<>
#include <string>      // for string
 
#include <cloe/component/lane_boundary.hpp>  // for LaneBoundary
#include <cloe/component/lane_sensor.hpp>    // for LaneBoundarySensor
 
namespace cloe {
 
using LaneBoundaryFilter = std::function<bool(const LaneBoundary&)>;
 
class LaneBoundarySensorFilter : public LaneBoundarySensor {
 public:
  LaneBoundarySensorFilter(std::shared_ptr<LaneBoundarySensor> lbs, LaneBoundaryFilter f)
      : LaneBoundarySensor("lane_sensor_filter"), sensor_(lbs), filter_func_(f) {}
 
  virtual ~LaneBoundarySensorFilter() noexcept = default;
 
  const LaneBoundaries& sensed_lane_boundaries() const override {
    if (!cached_) {
      for (const auto& kv : sensor_->sensed_lane_boundaries()) {
        auto lb = kv.second;
        auto id = kv.first;
        if (filter_func_(lb)) {
          lbs_.insert(std::pair<int, LaneBoundary>(id, lb));
        }
      }
      cached_ = true;
    }
    return lbs_;
  }
 
  const Frustum& frustum() const override { return sensor_->frustum(); }
 
  const Eigen::Isometry3d& mount_pose() const override { return sensor_->mount_pose(); }
 
  Duration process(const Sync& sync) override {
    // This currently shouldn't do anything, but this class acts as a prototype
    // for How It Should Be Done.
    Duration t = LaneBoundarySensor::process(sync);
    if (t < sync.time()) {
      return t;
    }
 
    // Process the underlying sensor and clear the cache.
    t = sensor_->process(sync);
    clear_cache();
    return t;
  }
 
  void reset() override {
    LaneBoundarySensor::reset();
    sensor_->reset();
    clear_cache();
  }
 
  void abort() override {
    LaneBoundarySensor::abort();
    sensor_->abort();
  }
 
 protected:
  void clear_cache() {
    lbs_.clear();
    cached_ = false;
  }
 
 private:
  mutable bool cached_;
  mutable LaneBoundaries lbs_;
  std::shared_ptr<LaneBoundarySensor> sensor_;
  LaneBoundaryFilter filter_func_;
};
 
}  // namespace cloe