osi_omni_sensor.hpp

Go to the documentation of this file.

/*
 * Copyright 2020 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 <map>      // for map<>
#include <memory>   // for shared_ptr<>, unique_ptr<>
#include <string>   // for string
#include <utility>  // for move

#include <Eigen/Geometry>  // for Isometry3d, Vector3d

#include <cloe/component/lane_boundary.hpp>  // for LaneBoundary
#include <cloe/component/object.hpp>         // for Object
#include <cloe/core.hpp>                     // for Json, Duration
#include <cloe/simulator.hpp>                // for ModelError
#include <cloe/sync.hpp>                     // for Sync

#include "osi_common.pb.h"           // for Timestamp, Identifier, BaseMoving, ..
#include "osi_detectedobject.pb.h"   // for DetectedMovingObject
#include "osi_hostvehicledata.pb.h"  // for HostVehicleData
#include "osi_object.pb.h"           // for MovingObject
#include "osi_sensordata.pb.h"       // for SensorData, DetectedEntityHeader

#include "osi_ground_truth.hpp"  // for OsiGroundTruth
#include "osi_transceiver.hpp"   // for OsiTransceiver
#include "osi_utils.hpp"

namespace osii {

cloe::Duration osi_timestamp_to_time(const osi3::Timestamp& timestamp);

void from_osi_identifier(const osi3::Identifier& osi_id, int& id);

void from_osi_host_vehicle_data(const osi3::HostVehicleData& osi_hv, cloe::Object& obj);

void from_osi_base_moving(const osi3::BaseMoving& osi_bm, cloe::Object& obj);

void from_osi_base_moving_alt(const osi3::BaseMoving& osi_bm,
                              const osi3::BaseMoving& osi_bm_gt,
                              cloe::Object& obj);

template <typename T>
void from_osi_mov_obj_type_classification(const T& osi_mo, cloe::Object::Class& oc);
void from_osi_mov_obj_type_classification(
    const osi3::MovingObject::Type& osi_ot,
    const osi3::MovingObject::VehicleClassification::Type& osi_vt,
    cloe::Object::Class& oc);

void from_osi_detected_moving_object_alt(const osi3::DetectedMovingObject& osi_mo,
                                         const OsiGroundTruth& ground_truth, cloe::Object& obj);

void from_osi_boundary_points(const osi3::LaneBoundary& osi_lb, cloe::LaneBoundary& lb);

void transform_ego_coord_from_osi_data(const Eigen::Vector3d& dimensions_gt, cloe::Object& obj);

void transform_obj_coord_from_osi_data(const Eigen::Isometry3d& sensor_pose,
                                       const Eigen::Vector3d& dimensions_gt,
                                       cloe::Object& obj);

Eigen::Isometry3d osi_position_orientation_to_pose_alt(const osi3::BaseMoving& base,
                                                       const osi3::BaseMoving& base_gt);

Eigen::Vector3d osi_vehicle_attrib_rear_offset_to_vector3d(
    const osi3::MovingObject::VehicleAttributes& osi_va);

enum class SensorMockTarget { MountingPosition, DetectedMovingObject, DetectedLaneBoundary };

enum class SensorMockLevel { OverwriteNone, InterpolateMissing, OverwriteAll };

// clang-format off
ENUM_SERIALIZATION(SensorMockLevel, ({
  {SensorMockLevel::OverwriteNone, "overwrite_none"},
  {SensorMockLevel::InterpolateMissing, "interpolate_missing"},
  {SensorMockLevel::OverwriteAll, "overwrite_all"},
}))
// clang-format on


struct SensorMockConf : public cloe::Confable {
  using Target = SensorMockTarget;
  using Level = SensorMockLevel;

  SensorMockConf() = default;

  virtual ~SensorMockConf() noexcept = default;

  std::map<Target, Level> level = {{Target::MountingPosition, Level::OverwriteNone},
                                   {Target::DetectedMovingObject, Level::OverwriteNone},
                                   {Target::DetectedLaneBoundary, Level::OverwriteNone}};

  CONFABLE_SCHEMA(SensorMockConf) {
    return fable::Schema{
        // clang-format off
        {"mounting_position", cloe::Schema(&level[Target::MountingPosition], "mock level for sensor mounting position")},
        {"detected_moving_objects", cloe::Schema(&level[Target::DetectedMovingObject], "mock level for detected moving objects")},
        {"detected_lane_boundaries", cloe::Schema(&level[Target::DetectedLaneBoundary], "mock level for detected lane boundaries")},
        // clang-format on
    };
  }

  void to_json(cloe::Json& j) const override {
    j["mounting_position"] = level.at(SensorMockTarget::MountingPosition);
    j["detected_moving_objects"] = level.at(SensorMockTarget::DetectedMovingObject);
    j["detected_lane_boundaries"] = level.at(SensorMockTarget::DetectedLaneBoundary);
  }
};

class OsiOmniSensor {
 public:
  virtual ~OsiOmniSensor() = default;
  OsiOmniSensor() = delete;

  OsiOmniSensor(std::unique_ptr<OsiTransceiver>&& osi_transceiver, uint64_t owner_id)
      : osi_comm_(std::move(osi_transceiver)), owner_id_(owner_id) {
    ground_truth_ = std::make_unique<OsiGroundTruth>();
  }

  OsiOmniSensor(OsiTransceiver* osi_transceiver, uint64_t owner_id)
      : osi_comm_(osi_transceiver), owner_id_(owner_id) {
    ground_truth_ = std::make_unique<OsiGroundTruth>();
  }

  virtual void step(const cloe::Sync& s, const bool& restart, cloe::Duration& sim_time);

  virtual void process(const osi3::Timestamp& timestamp);

  virtual void process(osi3::SensorData* osi_sd, cloe::Duration& sim_time);

  virtual void process(const osi3::SensorView& osi_sv);

  virtual void process(const bool has_veh_data,
                       const osi3::HostVehicleData& osi_hv,
                       const osi3::MovingObject& osi_ego);

  virtual void process(const bool has_eh,
                       const osi3::DetectedEntityHeader& osi_eh,
                       const osi3::DetectedMovingObject& osi_mo);

  void mock_detected_lane_boundaries();

  void from_osi_boundary_points(const osi3::LaneBoundary& osi_lb, cloe::LaneBoundary& lb);

  virtual void store_ego_object(std::shared_ptr<cloe::Object> ego_obj) = 0;

  virtual void store_object(std::shared_ptr<cloe::Object> obj) = 0;

  virtual void store_lane_boundary(const cloe::LaneBoundary& lb) = 0;

  virtual void store_sensor_meta_data(const Eigen::Vector3d& bbcenter_to_veh_origin,
                                      const Eigen::Vector3d& ego_dimensions) = 0;

  cloe::Duration osi_timestamp_to_simtime(const osi3::Timestamp& timestamp) const;

  virtual Eigen::Isometry3d get_static_mounting_position(
      const Eigen::Vector3d& bbcenter_to_veh_origin, const Eigen::Vector3d& ego_dimensions) = 0;

  virtual void set_mock_conf(std::shared_ptr<const SensorMockConf> mock) = 0;

  SensorMockLevel get_mock_level(SensorMockTarget trg_type) const {
    return mock_->level.at(trg_type);
  }

  friend void to_json(cloe::Json& j, const OsiOmniSensor& c) {
    j = cloe::Json{
        {"osi_connection", *c.osi_comm_},
    };
  }

 protected:
  std::unique_ptr<OsiTransceiver> osi_comm_;

  std::unique_ptr<OsiGroundTruth> ground_truth_;

  uint64_t owner_id_;

  Eigen::Isometry3d osi_ego_pose_;

  Eigen::Isometry3d osi_sensor_pose_;

  cloe::Duration init_time_ = cloe::Duration(-1);

  std::shared_ptr<const SensorMockConf> mock_{nullptr};
};
}  // namespace osii