<commit_before>/*****************************************************************************************
 *                                                                                *
 * OpenSpace                                                                             *
 *                                                                                *
 * Copyright (c) 2014-2016                                                               *
 *                                                                                *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this  *
 * software and associated documentation files (the "Software"), to deal in the Software *
 * without restriction, including without limitation the rights to use, copy, modify,    *
 * merge, publish, distribute, sublicense, and/or sell copies of the Software, and to    *
 * permit persons to whom the Software is furnished to do so, subject to the following   *
 * conditions:                                                                           *
 *                                                                                *
 * The above copyright notice and this permission notice shall be included in all copies *
 * or substantial portions of the Software.                                              *
 *                                                                                *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   *
 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A         *
 * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT    *
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF  *
 * CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE  *
 * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                                         *
 ****************************************************************************************/

#include <modules/newhorizons/rendering/renderablemodelprojection.h>

#include <openspace/engine/openspaceengine.h>
#include <openspace/rendering/renderengine.h>
#include <openspace/scene/scenegraphnode.h>
#include <openspace/util/spicemanager.h>
#include <openspace/util/time.h>

#include <ghoul/filesystem/filesystem.h>
#include <ghoul/io/texture/texturereader.h>
#include <ghoul/opengl/textureunit.h>

namespace {
    const std::string _loggerCat = "RenderableModelProjection";
    const std::string keySource = "Rotation.Source";
    const std::string keyDestination = "Rotation.Destination";
    const std::string keyBody = "Body";
    const std::string keyGeometry = "Geometry";

    const std::string keyTextureColor = "Textures.Color";
    const std::string keyTextureProject = "Textures.Project";
    const std::string keyTextureDefault = "Textures.Default";
}

namespace openspace {

RenderableModelProjection::RenderableModelProjection(const ghoul::Dictionary& dictionary)
    : Renderable(dictionary)
    , _colorTexturePath("colorTexture", "Color Texture")
    , _rotation("rotation", "Rotation", glm::vec3(0.f), glm::vec3(0.f), glm::vec3(360.f))
    , _programObject(nullptr)
    , _fboProgramObject(nullptr)
    , _baseTexture(nullptr)
    , _geometry(nullptr)
    , _performShading("performShading", "Perform Shading", true)
{
    std::string name;
    bool success = dictionary.getValue(SceneGraphNode::KeyName, name);
    ghoul_assert(success, "Name was not passed to RenderableModelProjection");

    ghoul::Dictionary geometryDictionary;
    success = dictionary.getValue(keyGeometry, geometryDictionary);
    if (success) {
        using modelgeometry::ModelGeometry;
        geometryDictionary.setValue(SceneGraphNode::KeyName, name);
        _geometry = std::unique_ptr<ModelGeometry>(
            ModelGeometry::createFromDictionary(geometryDictionary)
        );
    }

    std::string texturePath = "";
    success = dictionary.getValue(keyTextureColor, texturePath);
    if (success)
        _colorTexturePath = absPath(texturePath);

    success = dictionary.getValue(keyTextureDefault, texturePath);
    if (success)
        _defaultProjImage = absPath(texturePath);

    addPropertySubOwner(_geometry.get());

    addProperty(_projectionFading);

    addProperty(_colorTexturePath);
    _colorTexturePath.onChange(std::bind(&RenderableModelProjection::loadTextures, this));

    dictionary.getValue(keySource, _source);
    dictionary.getValue(keyDestination, _destination);
    dictionary.getValue(keyBody, _target);
    if (_target != "")
        setBody(_target);

    bool completeSuccess = true;
    completeSuccess &= initializeProjectionSettings(dictionary);

    openspace::SpiceManager::ref().addFrame(_target, _source);
    setBoundingSphere(pss(1.f, 9.f));

    addProperty(_performShading);
    addProperty(_performProjection);
    addProperty(_clearAllProjections);
    addProperty(_rotation);

    success = initializeParser(dictionary);
    ghoul_assert(success, "");
}

bool RenderableModelProjection::isReady() const {
    bool ready = true;
    ready &= (_programObject != nullptr);
    ready &= (_baseTexture != nullptr);
    ready &= (_projectionTexture != nullptr);
    return ready;
}

bool RenderableModelProjection::initialize() {
    bool completeSuccess = true;

    RenderEngine& renderEngine = OsEng.renderEngine();
    _programObject = renderEngine.buildRenderProgram("ModelShader",
        "${MODULE_NEWHORIZONS}/shaders/renderableModel_vs.glsl",
        "${MODULE_NEWHORIZONS}/shaders/renderableModel_fs.glsl");

    _fboProgramObject = ghoul::opengl::ProgramObject::Build("ProjectionPass",
        "${MODULE_NEWHORIZONS}/shaders/renderableModelProjection_vs.glsl",
        "${MODULE_NEWHORIZONS}/shaders/renderableModelProjection_fs.glsl");
    _fboProgramObject->setIgnoreUniformLocationError(
        ghoul::opengl::ProgramObject::IgnoreError::Yes
    );

    completeSuccess &= loadTextures();

    completeSuccess &= ProjectionComponent::initialize();

    completeSuccess &= _geometry->initialize(this);
    completeSuccess &= !_source.empty();
    completeSuccess &= !_destination.empty();

    return completeSuccess;
}

bool RenderableModelProjection::deinitialize() {
    if (_geometry)
        _geometry->deinitialize();

    _geometry = nullptr;
    _baseTexture = nullptr;

    ProjectionComponent::deinitialize();

    OsEng.renderEngine().removeRenderProgram(_programObject);
    _programObject = nullptr;

    return true;
}

void RenderableModelProjection::render(const RenderData& data) {
    if (_clearAllProjections)
        clearAllProjections();

    _camScaling = data.camera.scaling();

    if (_capture && _performProjection)
        project();

    _programObject->activate();

    attitudeParameters(_time);
    _imageTimes.clear();

    _programObject->setUniform("_performShading", _performShading);
    _programObject->setUniform("sun_pos", _sunPosition.vec3());
    _programObject->setUniform("ViewProjection", data.camera.viewProjectionMatrix());
    _programObject->setUniform("ModelTransform", _transform);
    _programObject->setUniform("_projectionFading", _projectionFading);
    setPscUniforms(*_programObject, data.camera, data.position);

    _geometry->setUniforms(*_programObject);

    ghoul::opengl::TextureUnit unit[2];
    unit[0].activate();
    _baseTexture->bind();
    _programObject->setUniform("baseTexture", unit[0]);

    unit[1].activate();
    _projectionTexture->bind();
    _programObject->setUniform("projectionTexture", unit[1]);

    _geometry->render();

    _programObject->deactivate();
}

void RenderableModelProjection::update(const UpdateData& data) {
    if (_programObject->isDirty())
        _programObject->rebuildFromFile();

    if (_fboProgramObject->isDirty())
        _fboProgramObject->rebuildFromFile();

    _time = data.time;

    if (openspace::ImageSequencer::ref().isReady() && _performProjection) {
        openspace::ImageSequencer::ref().updateSequencer(_time);
        _capture = openspace::ImageSequencer::ref().getImagePaths(
            _imageTimes, _projecteeID, _instrumentID
        );
    }

    // set spice-orientation in accordance to timestamp
    if (!_source.empty()) {
        _stateMatrix = SpiceManager::ref().positionTransformMatrix(
            _source, _destination, _time
        );
    }

    double lt;
    glm::dvec3 p =
        openspace::SpiceManager::ref().targetPosition(
            "SUN", _target, "GALACTIC", {}, _time, lt
    );
    _sunPosition = PowerScaledCoordinate::CreatePowerScaledCoordinate(p.x, p.y, p.z);
}

void RenderableModelProjection::imageProjectGPU(
                                std::shared_ptr<ghoul::opengl::Texture> projectionTexture)
{
    ProjectionComponent::imageProjectBegin();

    _fboProgramObject->activate();

    ghoul::opengl::TextureUnit unitFbo;
    unitFbo.activate();
    projectionTexture->bind();
    _fboProgramObject->setUniform("projectionTexture", unitFbo);

    _fboProgramObject->setUniform("ProjectorMatrix", _projectorMatrix);
    _fboProgramObject->setUniform("ModelTransform", _transform);
    _fboProgramObject->setUniform("_scaling", _camScaling);
    _fboProgramObject->setUniform("boresight", _boresight);

    _geometry->setUniforms(*_fboProgramObject);
    _geometry->render();

    _fboProgramObject->deactivate();

    ProjectionComponent::imageProjectEnd();
}

void RenderableModelProjection::attitudeParameters(double time) {
    try {
        _stateMatrix = SpiceManager::ref().positionTransformMatrix(_source, _destination, time);
        _instrumentMatrix = SpiceManager::ref().positionTransformMatrix(_instrumentID, _destination, time);
    }
    catch (const SpiceManager::SpiceException& e) {
        return;
    }

    _transform = glm::mat4(1);
    glm::mat4 rotPropX = glm::rotate(
        _transform,
        glm::radians(static_cast<float>(_rotation.value().x)),
        glm::vec3(1, 0, 0)
    );
    glm::mat4 rotPropY = glm::rotate(
        _transform,
        glm::radians(static_cast<float>(_rotation.value().y)),
        glm::vec3(0, 1, 0)
    );
    glm::mat4 rotPropZ = glm::rotate(
        _transform,
        glm::radians(static_cast<float>(_rotation.value().z)),
        glm::vec3(0, 0, 1)
    );

    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            _transform[i][j] = static_cast<float>(_stateMatrix[i][j]);
        }
    }
    _transform = _transform * rotPropX * rotPropY * rotPropZ;

    glm::dvec3 boresight;
    try {
        SpiceManager::FieldOfViewResult res = SpiceManager::ref().fieldOfView(_instrumentID);
        boresight = std::move(res.boresightVector);
    } catch (const SpiceManager::SpiceException& e) {
        return;
    }

    double lightTime;
    glm::dvec3 p =
        SpiceManager::ref().targetPosition(_projectorID, _projecteeID, _destination, _aberration, time, lightTime);
    psc position = PowerScaledCoordinate::CreatePowerScaledCoordinate(p.x, p.y, p.z);

    position[3] += (3 + _camScaling[1]);
    glm::vec3 cpos = position.vec3();

    _projectorMatrix = computeProjectorMatrix(cpos, boresight, _up, _instrumentMatrix,
        _fovy, _aspectRatio, _nearPlane, _farPlane, _boresight
    );
}

void RenderableModelProjection::project() {
    for (auto img : _imageTimes) {
        attitudeParameters(img.startTime);
        imageProjectGPU(loadProjectionTexture(img.path));
    }
    _capture = false;
}

bool RenderableModelProjection::loadTextures() {
    _baseTexture = nullptr;
    if (_colorTexturePath.value() != "") {
        _baseTexture = std::move(
            ghoul::io::TextureReader::ref().loadTexture(absPath(_colorTexturePath))
        );
        if (_baseTexture) {
            LDEBUG("Loaded texture from '" << absPath(_colorTexturePath) << "'");
            _baseTexture->uploadTexture();
            _baseTexture->setFilter(ghoul::opengl::Texture::FilterMode::Linear);
        }
    }

    return _baseTexture != nullptr;
}

}  // namespace openspace
<commit_msg>Reenable up-vector definition in RenderableModelProjection<commit_after>/*****************************************************************************************
 *                                                                                *
 * OpenSpace                                                                             *
 *                                                                                *
 * Copyright (c) 2014-2016                                                               *
 *                                                                                *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this  *
 * software and associated documentation files (the "Software"), to deal in the Software *
 * without restriction, including without limitation the rights to use, copy, modify,    *
 * merge, publish, distribute, sublicense, and/or sell copies of the Software, and to    *
 * permit persons to whom the Software is furnished to do so, subject to the following   *
 * conditions:                                                                           *
 *                                                                                *
 * The above copyright notice and this permission notice shall be included in all copies *
 * or substantial portions of the Software.                                              *
 *                                                                                *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   *
 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A         *
 * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT    *
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF  *
 * CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE  *
 * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                                         *
 ****************************************************************************************/

#include <modules/newhorizons/rendering/renderablemodelprojection.h>

#include <openspace/engine/openspaceengine.h>
#include <openspace/rendering/renderengine.h>
#include <openspace/scene/scenegraphnode.h>
#include <openspace/util/spicemanager.h>
#include <openspace/util/time.h>

#include <ghoul/filesystem/filesystem.h>
#include <ghoul/io/texture/texturereader.h>
#include <ghoul/opengl/textureunit.h>

namespace {
    const std::string _loggerCat = "RenderableModelProjection";
    const std::string keySource = "Rotation.Source";
    const std::string keyDestination = "Rotation.Destination";
    const std::string keyBody = "Body";
    const std::string keyGeometry = "Geometry";

    const std::string keyTextureColor = "Textures.Color";
    const std::string keyTextureProject = "Textures.Project";
    const std::string keyTextureDefault = "Textures.Default";
}

namespace openspace {

RenderableModelProjection::RenderableModelProjection(const ghoul::Dictionary& dictionary)
    : Renderable(dictionary)
    , _colorTexturePath("colorTexture", "Color Texture")
    , _rotation("rotation", "Rotation", glm::vec3(0.f), glm::vec3(0.f), glm::vec3(360.f))
    , _programObject(nullptr)
    , _fboProgramObject(nullptr)
    , _baseTexture(nullptr)
    , _geometry(nullptr)
    , _performShading("performShading", "Perform Shading", true)
{
    std::string name;
    bool success = dictionary.getValue(SceneGraphNode::KeyName, name);
    ghoul_assert(success, "Name was not passed to RenderableModelProjection");

    ghoul::Dictionary geometryDictionary;
    success = dictionary.getValue(keyGeometry, geometryDictionary);
    if (success) {
        using modelgeometry::ModelGeometry;
        geometryDictionary.setValue(SceneGraphNode::KeyName, name);
        _geometry = std::unique_ptr<ModelGeometry>(
            ModelGeometry::createFromDictionary(geometryDictionary)
        );
    }

    std::string texturePath = "";
    success = dictionary.getValue(keyTextureColor, texturePath);
    if (success)
        _colorTexturePath = absPath(texturePath);

    success = dictionary.getValue(keyTextureDefault, texturePath);
    if (success)
        _defaultProjImage = absPath(texturePath);

    addPropertySubOwner(_geometry.get());

    addProperty(_projectionFading);

    addProperty(_colorTexturePath);
    _colorTexturePath.onChange(std::bind(&RenderableModelProjection::loadTextures, this));

    dictionary.getValue(keySource, _source);
    dictionary.getValue(keyDestination, _destination);
    dictionary.getValue(keyBody, _target);
    if (_target != "")
        setBody(_target);

    bool completeSuccess = true;
    completeSuccess &= initializeProjectionSettings(dictionary);

    openspace::SpiceManager::ref().addFrame(_target, _source);
    setBoundingSphere(pss(1.f, 9.f));

    addProperty(_performShading);
    addProperty(_performProjection);
    addProperty(_clearAllProjections);
    addProperty(_rotation);

    success = initializeParser(dictionary);
    ghoul_assert(success, "");
}

bool RenderableModelProjection::isReady() const {
    bool ready = true;
    ready &= (_programObject != nullptr);
    ready &= (_baseTexture != nullptr);
    ready &= (_projectionTexture != nullptr);
    return ready;
}

bool RenderableModelProjection::initialize() {
    bool completeSuccess = true;

    RenderEngine& renderEngine = OsEng.renderEngine();
    _programObject = renderEngine.buildRenderProgram("ModelShader",
        "${MODULE_NEWHORIZONS}/shaders/renderableModel_vs.glsl",
        "${MODULE_NEWHORIZONS}/shaders/renderableModel_fs.glsl");

    _fboProgramObject = ghoul::opengl::ProgramObject::Build("ProjectionPass",
        "${MODULE_NEWHORIZONS}/shaders/renderableModelProjection_vs.glsl",
        "${MODULE_NEWHORIZONS}/shaders/renderableModelProjection_fs.glsl");
    _fboProgramObject->setIgnoreUniformLocationError(
        ghoul::opengl::ProgramObject::IgnoreError::Yes
    );

    completeSuccess &= loadTextures();

    completeSuccess &= ProjectionComponent::initialize();

    completeSuccess &= _geometry->initialize(this);
    completeSuccess &= !_source.empty();
    completeSuccess &= !_destination.empty();

    return completeSuccess;
}

bool RenderableModelProjection::deinitialize() {
    if (_geometry)
        _geometry->deinitialize();

    _geometry = nullptr;
    _baseTexture = nullptr;

    ProjectionComponent::deinitialize();

    OsEng.renderEngine().removeRenderProgram(_programObject);
    _programObject = nullptr;

    return true;
}

void RenderableModelProjection::render(const RenderData& data) {
    if (_clearAllProjections)
        clearAllProjections();

    _camScaling = data.camera.scaling();
    _up = data.camera.lookUpVectorCameraSpace();

    if (_capture && _performProjection)
        project();

    _programObject->activate();

    attitudeParameters(_time);
    _imageTimes.clear();

    _programObject->setUniform("_performShading", _performShading);
    _programObject->setUniform("sun_pos", _sunPosition.vec3());
    _programObject->setUniform("ViewProjection", data.camera.viewProjectionMatrix());
    _programObject->setUniform("ModelTransform", _transform);
    _programObject->setUniform("_projectionFading", _projectionFading);
    setPscUniforms(*_programObject, data.camera, data.position);

    _geometry->setUniforms(*_programObject);

    ghoul::opengl::TextureUnit unit[2];
    unit[0].activate();
    _baseTexture->bind();
    _programObject->setUniform("baseTexture", unit[0]);

    unit[1].activate();
    _projectionTexture->bind();
    _programObject->setUniform("projectionTexture", unit[1]);

    _geometry->render();

    _programObject->deactivate();
}

void RenderableModelProjection::update(const UpdateData& data) {
    if (_programObject->isDirty())
        _programObject->rebuildFromFile();

    if (_fboProgramObject->isDirty())
        _fboProgramObject->rebuildFromFile();

    _time = data.time;

    if (openspace::ImageSequencer::ref().isReady() && _performProjection) {
        openspace::ImageSequencer::ref().updateSequencer(_time);
        _capture = openspace::ImageSequencer::ref().getImagePaths(
            _imageTimes, _projecteeID, _instrumentID
        );
    }

    // set spice-orientation in accordance to timestamp
    if (!_source.empty()) {
        _stateMatrix = SpiceManager::ref().positionTransformMatrix(
            _source, _destination, _time
        );
    }

    double lt;
    glm::dvec3 p =
        openspace::SpiceManager::ref().targetPosition(
            "SUN", _target, "GALACTIC", {}, _time, lt
    );
    _sunPosition = PowerScaledCoordinate::CreatePowerScaledCoordinate(p.x, p.y, p.z);
}

void RenderableModelProjection::imageProjectGPU(
                                std::shared_ptr<ghoul::opengl::Texture> projectionTexture)
{
    ProjectionComponent::imageProjectBegin();

    _fboProgramObject->activate();

    ghoul::opengl::TextureUnit unitFbo;
    unitFbo.activate();
    projectionTexture->bind();
    _fboProgramObject->setUniform("projectionTexture", unitFbo);

    _fboProgramObject->setUniform("ProjectorMatrix", _projectorMatrix);
    _fboProgramObject->setUniform("ModelTransform", _transform);
    _fboProgramObject->setUniform("_scaling", _camScaling);
    _fboProgramObject->setUniform("boresight", _boresight);

    _geometry->setUniforms(*_fboProgramObject);
    _geometry->render();

    _fboProgramObject->deactivate();

    ProjectionComponent::imageProjectEnd();
}

void RenderableModelProjection::attitudeParameters(double time) {
    try {
        _stateMatrix = SpiceManager::ref().positionTransformMatrix(_source, _destination, time);
        _instrumentMatrix = SpiceManager::ref().positionTransformMatrix(_instrumentID, _destination, time);
    }
    catch (const SpiceManager::SpiceException& e) {
        return;
    }

    _transform = glm::mat4(1);
    glm::mat4 rotPropX = glm::rotate(
        _transform,
        glm::radians(static_cast<float>(_rotation.value().x)),
        glm::vec3(1, 0, 0)
    );
    glm::mat4 rotPropY = glm::rotate(
        _transform,
        glm::radians(static_cast<float>(_rotation.value().y)),
        glm::vec3(0, 1, 0)
    );
    glm::mat4 rotPropZ = glm::rotate(
        _transform,
        glm::radians(static_cast<float>(_rotation.value().z)),
        glm::vec3(0, 0, 1)
    );

    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            _transform[i][j] = static_cast<float>(_stateMatrix[i][j]);
        }
    }
    _transform = _transform * rotPropX * rotPropY * rotPropZ;

    glm::dvec3 boresight;
    try {
        SpiceManager::FieldOfViewResult res = SpiceManager::ref().fieldOfView(_instrumentID);
        boresight = std::move(res.boresightVector);
    } catch (const SpiceManager::SpiceException& e) {
        return;
    }

    double lightTime;
    glm::dvec3 p =
        SpiceManager::ref().targetPosition(_projectorID, _projecteeID, _destination, _aberration, time, lightTime);
    psc position = PowerScaledCoordinate::CreatePowerScaledCoordinate(p.x, p.y, p.z);

    position[3] += (3 + _camScaling[1]);
    glm::vec3 cpos = position.vec3();

    _projectorMatrix = computeProjectorMatrix(cpos, boresight, _up, _instrumentMatrix,
        _fovy, _aspectRatio, _nearPlane, _farPlane, _boresight
    );
}

void RenderableModelProjection::project() {
    for (auto img : _imageTimes) {
        attitudeParameters(img.startTime);
        imageProjectGPU(loadProjectionTexture(img.path));
    }
    _capture = false;
}

bool RenderableModelProjection::loadTextures() {
    _baseTexture = nullptr;
    if (_colorTexturePath.value() != "") {
        _baseTexture = std::move(
            ghoul::io::TextureReader::ref().loadTexture(absPath(_colorTexturePath))
        );
        if (_baseTexture) {
            LDEBUG("Loaded texture from '" << absPath(_colorTexturePath) << "'");
            _baseTexture->uploadTexture();
            _baseTexture->setFilter(ghoul::opengl::Texture::FilterMode::Linear);
        }
    }

    return _baseTexture != nullptr;
}

}  // namespace openspace
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

#include "modules/perception/obstacle/onboard/async_fusion_subnode.h"

#include <unordered_map>

#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/configs/config_gflags.h"
#include "modules/common/log.h"
#include "modules/common/time/timer.h"
#include "modules/perception/common/perception_gflags.h"
#include "modules/perception/onboard/event_manager.h"
#include "modules/perception/onboard/shared_data_manager.h"
#include "modules/perception/onboard/subnode_helper.h"

namespace apollo {
namespace perception {

using apollo::canbus::Chassis;
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::adapter::AdapterManager;

bool AsyncFusionSubnode::InitInternal() {
  RegistAllAlgorithm();

  AdapterManager::Init(FLAGS_perception_adapter_config_filename);

  CHECK(AdapterManager::GetChassis()) << "Chassis is not initialized.";
  AdapterManager::AddChassisCallback(&AsyncFusionSubnode::OnChassis, this);

  CHECK(shared_data_manager_ != nullptr);
  fusion_.reset(BaseFusionRegisterer::GetInstanceByName(FLAGS_onboard_fusion));
  if (fusion_ == nullptr) {
    AERROR << "Failed to get fusion instance: " << FLAGS_onboard_fusion;
    return false;
  }
  if (!fusion_->Init()) {
    AERROR << "Failed to init fusion:" << FLAGS_onboard_fusion;
    return false;
  }
  radar_object_data_ = dynamic_cast<RadarObjectData *>(
      shared_data_manager_->GetSharedData("RadarObjectData"));
  if (radar_object_data_ == nullptr) {
    AWARN << "Failed to get RadarObjectData.";
  }
  camera_object_data_ = dynamic_cast<CameraObjectData *>(
      shared_data_manager_->GetSharedData("CameraObjectData"));
  if (camera_object_data_ == nullptr) {
    AWARN << "Failed to get CameraObjectData.";
  }
  lane_shared_data_ = dynamic_cast<LaneSharedData *>(
      shared_data_manager_->GetSharedData("LaneSharedData"));
  if (lane_shared_data_ == nullptr) {
    AERROR << "failed to get shared data instance: LaneSharedData ";
    return false;
  }

  fusion_data_ = dynamic_cast<FusionSharedData *>(
      shared_data_manager_->GetSharedData("FusionSharedData"));
  if (fusion_data_ == nullptr) {
    AWARN << "Failed to get FusionSharedData.";
  }

  if (!InitOutputStream()) {
    AERROR << "Failed to init output stream.";
    return false;
  }

  AINFO << "Init AsyncFusionSubnode succ. Using fusion:" << fusion_->name();
  return true;
}

bool AsyncFusionSubnode::InitOutputStream() {
  // expect reserve_ format:
  //       pub_driven_event_id:n
  //       lidar_output_stream : event_id=n&sink_type=m&sink_name=x
  //       radar_output_stream : event_id=n&sink_type=m&sink_name=x
  std::unordered_map<std::string, std::string> reserve_field_map;
  if (!SubnodeHelper::ParseReserveField(reserve_, &reserve_field_map)) {
    AERROR << "Failed to parse reserve string: " << reserve_;
    return false;
  }

  auto radar_iter = reserve_field_map.find("radar_event_id");
  if (radar_iter == reserve_field_map.end()) {
    AWARN << "Failed to find radar_event_id:" << reserve_;
    AINFO << "radar_event_id will be set -1";
    radar_event_id_ = -1;
  } else {
    radar_event_id_ = static_cast<EventID>(atoi((radar_iter->second).c_str()));
  }

  auto camera_iter = reserve_field_map.find("camera_event_id");
  if (camera_iter == reserve_field_map.end()) {
    AWARN << "Failed to find camera_event_id:" << reserve_;
    AINFO << "camera_event_id will be set -1";
    camera_event_id_ = -1;
  } else {
    camera_event_id_ =
        static_cast<EventID>(atoi((camera_iter->second).c_str()));
  }

  auto lane_iter = reserve_field_map.find("lane_event_id");
  if (lane_iter == reserve_field_map.end()) {
    AWARN << "Failed to find camera_event_id:" << reserve_;
    AINFO << "camera_event_id will be set -1";
    lane_event_id_ = -1;
  } else {
    lane_event_id_ = static_cast<EventID>(atoi((lane_iter->second).c_str()));
  }
  return true;
}

Status AsyncFusionSubnode::ProcEvents() {
  for (auto event_meta : sub_meta_events_) {
    std::vector<Event> events;
    if (!SubscribeEvents(event_meta, &events)) {
      AERROR << "event meta id:" << event_meta.event_id << " "
             << event_meta.from_node << " " << event_meta.to_node;
      return Status(ErrorCode::PERCEPTION_ERROR, "Subscribe event fail.");
    }

    if (events.empty()) {
      usleep(500);
      continue;
    }

    apollo::common::time::Timer timer;
    timer.Start();
    Process(event_meta, events);
    ADEBUG << "time elapsed for async fusion process " << timer.End("");

    // public obstacle message
    PerceptionObstacles obstacles;
    if (GeneratePbMsg(&obstacles)) {
      // Assume FLU coordinate system
      if (FLAGS_use_navigation_mode) {
        for (auto obstacle : obstacles.perception_obstacle()) {
          obstacle.mutable_velocity()->set_x(obstacle.velocity().x() +
                                             chassis_speed_mps_);
        }
      }
      common::adapter::AdapterManager::PublishPerceptionObstacles(obstacles);
    }
    AINFO << "Publish 3d perception fused msg. timestamp:"
          << GLOG_TIMESTAMP(timestamp_) << " obj_cnt:" << objects_.size();
  }
  return Status::OK();
}

void AsyncFusionSubnode::PublishDataAndEvent(
    const double &timestamp, const std::string &device_id,
    const SharedDataPtr<FusionItem> &data) {
  CommonSharedDataKey key(timestamp, device_id);
  fusion_data_->Add(key, data);
  for (size_t idx = 0; idx < pub_meta_events_.size(); ++idx) {
    const EventMeta &event_meta = pub_meta_events_[idx];
    Event event;
    event.event_id = event_meta.event_id;
    event.timestamp = timestamp;
    event.reserve = device_id;
    event_manager_->Publish(event);
  }
}

Status AsyncFusionSubnode::Process(const EventMeta &event_meta,
                                   const std::vector<Event> &events) {
  const std::string &device_id = events[0].reserve;
  // const double timestamp = events[0].timestamp;

  std::vector<SensorObjects> sensor_objs;
  if (!BuildSensorObjs(events, &sensor_objs)) {
    AERROR << "Failed to build_sensor_objs";
    error_code_ = common::PERCEPTION_ERROR_PROCESS;
    return Status(ErrorCode::PERCEPTION_ERROR, "Failed to build_sensor_objs.");
  }
  PERF_BLOCK_START();
  objects_.clear();
  // if (!fusion_->Fuse(sensor_objs, &objects_)) {
  //   AWARN << "Failed to call fusion plugin."
  //         << " event_meta: [" << event_meta.to_string()
  //         << "] event_cnt:" << events.size() << " event_0: ["
  //         << events[0].to_string() << "]";
  //   error_code_ = common::PERCEPTION_ERROR_PROCESS;
  //   return Status(ErrorCode::PERCEPTION_ERROR,
  //                 "Failed to call fusion plugin.");
  // }

  if (event_meta.event_id == radar_event_id_) {
    PERF_BLOCK_END("fusion_radar");
  } else if (event_meta.event_id == camera_event_id_) {
    PERF_BLOCK_END("fusion_camera");
  }

  if (objects_.size() > 0 && FLAGS_publish_fusion_event) {
    SharedDataPtr<FusionItem> fusion_item_ptr(new FusionItem);
    fusion_item_ptr->timestamp = objects_[0]->latest_tracked_time;
    for (auto obj : objects_) {
      ObjectPtr objclone(new Object());
      objclone->clone(*obj);
      fusion_item_ptr->obstacles.push_back(objclone);
    }
    AINFO << "publishing event for timestamp deviceid and size of fusion object"
          << fusion_item_ptr->timestamp << " " << device_id << " "
          << fusion_item_ptr->obstacles.size();
    PublishDataAndEvent(fusion_item_ptr->timestamp, device_id, fusion_item_ptr);
  }

  // publishing result to pnc
  for (auto sensor_obj : sensor_objs) {
    PublishPerceptionPb(sensor_obj);
  }

  error_code_ = common::OK;
  return Status::OK();
}

void AsyncFusionSubnode::PublishPerceptionPb(
    const SensorObjects &sensor_objects) {
  AINFO << "Camera publish perception pb data";
  PerceptionObstacles obstacles;
  // Header
  common::adapter::AdapterManager::FillPerceptionObstaclesHeader(
      "perception_obstacle", &obstacles);
  common::Header *header = obstacles.mutable_header();
  header->set_lidar_timestamp(0);
  // use timestamp in nanoseconds
  header->set_camera_timestamp(sensor_objects.timestamp * 1e9);
  header->set_radar_timestamp(0);
  obstacles.set_error_code(sensor_objects.error_code);

  // generate lane marker protobuf messages
  LaneMarkers *lane_markers = obstacles.mutable_lane_marker();
  LaneObjectsToLaneMarkerProto(*(sensor_objects.lane_objects), lane_markers);
  // Serialize each Object
  for (const auto &obj : sensor_objects.objects) {
    PerceptionObstacle *obstacle = obstacles.add_perception_obstacle();
    obj->Serialize(obstacle);
  }

  // Relative speed of objects + latest ego car speed in X
  for (auto obstacle : obstacles.perception_obstacle()) {
    obstacle.mutable_velocity()->set_x(obstacle.velocity().x() +
                                       chassis_.speed_mps());
  }

  common::adapter::AdapterManager::PublishPerceptionObstacles(obstacles);
  ADEBUG << "Camera Obstacles: " << obstacles.ShortDebugString();
}

bool AsyncFusionSubnode::SubscribeEvents(const EventMeta &event_meta,
                                         std::vector<Event> *events) const {
  Event event;
  // blocking call for each of these events
  while (event_manager_->Subscribe(event_meta.event_id, &event, true)) {
    AINFO << "starting subscribing event " << event_meta.event_id;
    events->push_back(event);
  }
  return true;
}

bool AsyncFusionSubnode::BuildSensorObjs(
    const std::vector<Event> &events,
    std::vector<SensorObjects> *multi_sensor_objs) {
  PERF_FUNCTION();
  for (auto event : events) {
    std::shared_ptr<SensorObjects> sensor_objects;
    if (!GetSharedData(event, &sensor_objects)) {
      return false;
    }
    // Make sure timestamp and type are filled.
    sensor_objects->timestamp = event.timestamp;

    if (event.event_id == radar_event_id_) {
      sensor_objects->sensor_type = SensorType::RADAR;
    } else if (event.event_id == camera_event_id_) {
      sensor_objects->sensor_type = SensorType::CAMERA;
    } else {
      AERROR << "Event id is not supported. event:" << event.to_string();
      return false;
    }
    sensor_objects->sensor_id = GetSensorType(sensor_objects->sensor_type);
    multi_sensor_objs->push_back(*sensor_objects);
    ADEBUG << "get sensor objs:" << sensor_objects->ToString();
  }
  return true;
}

bool AsyncFusionSubnode::GetSharedData(const Event &event,
                                       std::shared_ptr<SensorObjects> *objs) {
  double timestamp = event.timestamp;
  const std::string &device_id = event.reserve;
  std::string data_key;
  if (!SubnodeHelper::ProduceSharedDataKey(timestamp, device_id, &data_key)) {
    AERROR << "Failed to produce shared data key. EventID:" << event.event_id
           << " timestamp:" << timestamp << " device_id:" << device_id;
    return false;
  }
  bool get_data_succ = false;

  if (event.event_id == radar_event_id_ && radar_object_data_ != nullptr) {
    get_data_succ = radar_object_data_->Get(data_key, objs);
  } else if (event.event_id == camera_event_id_ &&
             camera_object_data_ != nullptr) {
    get_data_succ = camera_object_data_->Get(data_key, objs);
    // trying to get lane shared data as well
    Event lane_event;
    if (event_manager_->Subscribe(lane_event_id_, &lane_event, false)) {
      get_data_succ =
          lane_shared_data_->Get(data_key, &((*objs)->lane_objects));
      AINFO << "getting lane data successfully for data key " << data_key;
    }
  } else {
    AERROR << "Event id is not supported. event:" << event.to_string();
    return false;
  }

  if (!get_data_succ) {
    AERROR << "Failed to get shared data. event:" << event.to_string();
    return false;
  }

  return true;
}

bool AsyncFusionSubnode::GeneratePbMsg(PerceptionObstacles *obstacles) {
  common::adapter::AdapterManager::FillPerceptionObstaclesHeader(
      FLAGS_obstacle_module_name, obstacles);
  common::Header *header = obstacles->mutable_header();
  header->set_lidar_timestamp(timestamp_ * 1e9);  // in ns
  header->set_camera_timestamp(0);
  header->set_radar_timestamp(0);

  obstacles->set_error_code(error_code_);

  for (const auto &obj : objects_) {
    PerceptionObstacle *obstacle = obstacles->add_perception_obstacle();
    obj->Serialize(obstacle);
  }

  ADEBUG << "PerceptionObstacles: " << obstacles->ShortDebugString();
  return true;
}

void AsyncFusionSubnode::RegistAllAlgorithm() {
  RegisterFactoryAsyncFusion();
}

void AsyncFusionSubnode::OnChassis(const Chassis &chassis) {
  ADEBUG << "Received chassis data: run chassis callback.";
  chassis_.CopyFrom(chassis);
  chassis_speed_mps_ = chassis.speed_mps();
}

}  // namespace perception
}  // namespace apollo
<commit_msg>fix perception slow bug<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

#include "modules/perception/obstacle/onboard/async_fusion_subnode.h"

#include <unordered_map>

#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/configs/config_gflags.h"
#include "modules/common/log.h"
#include "modules/common/time/timer.h"
#include "modules/perception/common/perception_gflags.h"
#include "modules/perception/onboard/event_manager.h"
#include "modules/perception/onboard/shared_data_manager.h"
#include "modules/perception/onboard/subnode_helper.h"

namespace apollo {
namespace perception {

using apollo::canbus::Chassis;
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::adapter::AdapterManager;

bool AsyncFusionSubnode::InitInternal() {
  RegistAllAlgorithm();

  AdapterManager::Init(FLAGS_perception_adapter_config_filename);

  CHECK(AdapterManager::GetChassis()) << "Chassis is not initialized.";
  AdapterManager::AddChassisCallback(&AsyncFusionSubnode::OnChassis, this);

  CHECK(shared_data_manager_ != nullptr);
  fusion_.reset(BaseFusionRegisterer::GetInstanceByName(FLAGS_onboard_fusion));
  if (fusion_ == nullptr) {
    AERROR << "Failed to get fusion instance: " << FLAGS_onboard_fusion;
    return false;
  }
  if (!fusion_->Init()) {
    AERROR << "Failed to init fusion:" << FLAGS_onboard_fusion;
    return false;
  }
  radar_object_data_ = dynamic_cast<RadarObjectData *>(
      shared_data_manager_->GetSharedData("RadarObjectData"));
  if (radar_object_data_ == nullptr) {
    AWARN << "Failed to get RadarObjectData.";
  }
  camera_object_data_ = dynamic_cast<CameraObjectData *>(
      shared_data_manager_->GetSharedData("CameraObjectData"));
  if (camera_object_data_ == nullptr) {
    AWARN << "Failed to get CameraObjectData.";
  }
  lane_shared_data_ = dynamic_cast<LaneSharedData *>(
      shared_data_manager_->GetSharedData("LaneSharedData"));
  if (lane_shared_data_ == nullptr) {
    AERROR << "failed to get shared data instance: LaneSharedData ";
    return false;
  }

  fusion_data_ = dynamic_cast<FusionSharedData *>(
      shared_data_manager_->GetSharedData("FusionSharedData"));
  if (fusion_data_ == nullptr) {
    AWARN << "Failed to get FusionSharedData.";
  }

  if (!InitOutputStream()) {
    AERROR << "Failed to init output stream.";
    return false;
  }

  AINFO << "Init AsyncFusionSubnode succ. Using fusion:" << fusion_->name();
  return true;
}

bool AsyncFusionSubnode::InitOutputStream() {
  // expect reserve_ format:
  //       pub_driven_event_id:n
  //       lidar_output_stream : event_id=n&sink_type=m&sink_name=x
  //       radar_output_stream : event_id=n&sink_type=m&sink_name=x
  std::unordered_map<std::string, std::string> reserve_field_map;
  if (!SubnodeHelper::ParseReserveField(reserve_, &reserve_field_map)) {
    AERROR << "Failed to parse reserve string: " << reserve_;
    return false;
  }

  auto radar_iter = reserve_field_map.find("radar_event_id");
  if (radar_iter == reserve_field_map.end()) {
    AWARN << "Failed to find radar_event_id:" << reserve_;
    AINFO << "radar_event_id will be set -1";
    radar_event_id_ = -1;
  } else {
    radar_event_id_ = static_cast<EventID>(atoi((radar_iter->second).c_str()));
  }

  auto camera_iter = reserve_field_map.find("camera_event_id");
  if (camera_iter == reserve_field_map.end()) {
    AWARN << "Failed to find camera_event_id:" << reserve_;
    AINFO << "camera_event_id will be set -1";
    camera_event_id_ = -1;
  } else {
    camera_event_id_ =
        static_cast<EventID>(atoi((camera_iter->second).c_str()));
  }

  auto lane_iter = reserve_field_map.find("lane_event_id");
  if (lane_iter == reserve_field_map.end()) {
    AWARN << "Failed to find camera_event_id:" << reserve_;
    AINFO << "camera_event_id will be set -1";
    lane_event_id_ = -1;
  } else {
    lane_event_id_ = static_cast<EventID>(atoi((lane_iter->second).c_str()));
  }
  return true;
}

Status AsyncFusionSubnode::ProcEvents() {
  for (auto event_meta : sub_meta_events_) {
    std::vector<Event> events;
    if (event_meta.event_id == lane_event_id_) {
        continue;
    }

    if (!SubscribeEvents(event_meta, &events)) {
      AERROR << "event meta id:" << event_meta.event_id << " "
             << event_meta.from_node << " " << event_meta.to_node;
      return Status(ErrorCode::PERCEPTION_ERROR, "Subscribe event fail.");
    }

    if (events.empty()) {
      usleep(500);
      continue;
    }

    apollo::common::time::Timer timer;
    timer.Start();
    Process(event_meta, events);
    ADEBUG << "time elapsed for async fusion process " << timer.End("");

    // public obstacle message
    PerceptionObstacles obstacles;
    if (GeneratePbMsg(&obstacles)) {
      // Assume FLU coordinate system
      if (FLAGS_use_navigation_mode) {
        for (auto obstacle : obstacles.perception_obstacle()) {
          obstacle.mutable_velocity()->set_x(obstacle.velocity().x() +
                                             chassis_speed_mps_);
        }
      }
      common::adapter::AdapterManager::PublishPerceptionObstacles(obstacles);
    }
    AINFO << "Publish 3d perception fused msg. timestamp:"
          << GLOG_TIMESTAMP(timestamp_) << " obj_cnt:" << objects_.size();
  }
  return Status::OK();
}

void AsyncFusionSubnode::PublishDataAndEvent(
    const double &timestamp, const std::string &device_id,
    const SharedDataPtr<FusionItem> &data) {
  CommonSharedDataKey key(timestamp, device_id);
  bool fusion_succ = fusion_data_->Add(key, data);
  if (!fusion_succ) {
      AERROR <<"fusion shared data addkey failure";
  }

  ADEBUG << "adding key in fusion shared data " << key.ToString();

  for (size_t idx = 0; idx < pub_meta_events_.size(); ++idx) {
    const EventMeta &event_meta = pub_meta_events_[idx];
    Event event;
    event.event_id = event_meta.event_id;
    event.timestamp = timestamp;
    event.reserve = device_id;
    event_manager_->Publish(event);
  }
}

Status AsyncFusionSubnode::Process(const EventMeta &event_meta,
                                   const std::vector<Event> &events) {
  const std::string &device_id = events[0].reserve;
  // const double timestamp = events[0].timestamp;

  std::vector<SensorObjects> sensor_objs;
  if (!BuildSensorObjs(events, &sensor_objs)) {
    AERROR << "Failed to build_sensor_objs";
    error_code_ = common::PERCEPTION_ERROR_PROCESS;
    return Status(ErrorCode::PERCEPTION_ERROR, "Failed to build_sensor_objs.");
  }
  PERF_BLOCK_START();
  objects_.clear();
  /*
  if (!fusion_->Fuse(sensor_objs, &objects_)) {
    AWARN << "Failed to call fusion plugin."
          << " event_meta: [" << event_meta.to_string()
          << "] event_cnt:" << events.size() << " event_0: ["
          << events[0].to_string() << "]";
    error_code_ = common::PERCEPTION_ERROR_PROCESS;
    return Status(ErrorCode::PERCEPTION_ERROR, "Failed to call fusion plugin.");
  }*/

  if (event_meta.event_id == radar_event_id_) {
    PERF_BLOCK_END("fusion_radar");
  } else if (event_meta.event_id == camera_event_id_) {
    PERF_BLOCK_END("fusion_camera");
  }

  if (objects_.size() > 0 && FLAGS_publish_fusion_event) {
    SharedDataPtr<FusionItem> fusion_item_ptr(new FusionItem);
    fusion_item_ptr->timestamp = objects_[0]->latest_tracked_time;
    for (auto obj : objects_) {
      ObjectPtr objclone(new Object());
      objclone->clone(*obj);
      fusion_item_ptr->obstacles.push_back(objclone);
    }
    AINFO << "publishing event for timestamp deviceid and size of fusion object"
          << fusion_item_ptr->timestamp << " " << device_id << " "
          << fusion_item_ptr->obstacles.size();
    PublishDataAndEvent(fusion_item_ptr->timestamp, device_id, fusion_item_ptr);
  }

  // publishing result to pnc
  if (event_meta.event_id == camera_event_id_) {
      for (auto sensor_obj : sensor_objs) {
          PublishPerceptionPb(sensor_obj);
      }
  }

  error_code_ = common::OK;
  return Status::OK();
}

void AsyncFusionSubnode::PublishPerceptionPb(
    const SensorObjects &sensor_objects) {
  AINFO << "Camera publish perception pb data";
  PerceptionObstacles obstacles;
  // Header
  common::adapter::AdapterManager::FillPerceptionObstaclesHeader(
      "perception_obstacle", &obstacles);
  common::Header *header = obstacles.mutable_header();
  header->set_lidar_timestamp(0);
  // use timestamp in nanoseconds
  header->set_camera_timestamp(sensor_objects.timestamp * 1e9);
  header->set_radar_timestamp(0);
  obstacles.set_error_code(sensor_objects.error_code);

  // generate lane marker protobuf messages
  LaneMarkers *lane_markers = obstacles.mutable_lane_marker();
  LaneObjectsToLaneMarkerProto(*(sensor_objects.lane_objects), lane_markers);
  // Serialize each Object
  for (const auto &obj : sensor_objects.objects) {
    PerceptionObstacle *obstacle = obstacles.add_perception_obstacle();
    obj->Serialize(obstacle);
  }

  // Relative speed of objects + latest ego car speed in X
  for (auto obstacle : obstacles.perception_obstacle()) {
    obstacle.mutable_velocity()->set_x(obstacle.velocity().x() +
                                       chassis_.speed_mps());
  }

  common::adapter::AdapterManager::PublishPerceptionObstacles(obstacles);
  ADEBUG << "Camera Obstacles: " << obstacles.ShortDebugString();
}

bool AsyncFusionSubnode::SubscribeEvents(const EventMeta &event_meta,
                                         std::vector<Event> *events) const {
  Event event;
  // blocking call for each of these events
  while (event_manager_->Subscribe(event_meta.event_id, &event, true)) {
    ADEBUG << "starting subscribing event " << event_meta.event_id;
    // events->push_back(event);
  }

  // only obtain latest event from a sensor queue
  if (event.event_id !=0 && event.timestamp !=0.0) {
      events->push_back(event);
  }
  return true;
}

bool AsyncFusionSubnode::BuildSensorObjs(
    const std::vector<Event> &events,
    std::vector<SensorObjects> *multi_sensor_objs) {
  PERF_FUNCTION();
  for (auto event : events) {
    std::shared_ptr<SensorObjects> sensor_objects;
    if (!GetSharedData(event, &sensor_objects)) {
      return false;
    }
    // Make sure timestamp and type are filled.
    sensor_objects->timestamp = event.timestamp;

    if (event.event_id == radar_event_id_) {
      sensor_objects->sensor_type = SensorType::RADAR;
    } else if (event.event_id == camera_event_id_) {
      sensor_objects->sensor_type = SensorType::CAMERA;
    } else {
      AERROR << "Event id is not supported. event:" << event.to_string();
      return false;
    }
    sensor_objects->sensor_id = GetSensorType(sensor_objects->sensor_type);
    multi_sensor_objs->push_back(*sensor_objects);
    ADEBUG << "get sensor objs:" << sensor_objects->ToString();
  }
  return true;
}

bool AsyncFusionSubnode::GetSharedData(const Event &event,
                                       std::shared_ptr<SensorObjects> *objs) {
  double timestamp = event.timestamp;
  const std::string &device_id = event.reserve;
  std::string data_key;
  if (!SubnodeHelper::ProduceSharedDataKey(timestamp, device_id, &data_key)) {
    AERROR << "Failed to produce shared data key. EventID:" << event.event_id
           << " timestamp:" << timestamp << " device_id:" << device_id;
    return false;
  }
  bool get_data_succ = false;

  if (event.event_id == radar_event_id_ && radar_object_data_ != nullptr) {
    get_data_succ = radar_object_data_->Get(data_key, objs);
  } else if (event.event_id == camera_event_id_ &&
             camera_object_data_ != nullptr) {
    get_data_succ = camera_object_data_->Get(data_key, objs);
    // trying to get lane shared data as well
    Event lane_event;
    if (event_manager_->Subscribe(lane_event_id_, &lane_event, false)) {
      get_data_succ =
          lane_shared_data_->Get(data_key, &((*objs)->lane_objects));
      AINFO << "getting lane data successfully for data key " << data_key;
    }
  } else {
    AERROR << "Event id is not supported. event:" << event.to_string();
    return false;
  }

  if (!get_data_succ) {
    AERROR << "Failed to get shared data. event:" << event.to_string();
    return false;
  }

  return true;
}

bool AsyncFusionSubnode::GeneratePbMsg(PerceptionObstacles *obstacles) {
  common::adapter::AdapterManager::FillPerceptionObstaclesHeader(
      FLAGS_obstacle_module_name, obstacles);
  common::Header *header = obstacles->mutable_header();
  header->set_lidar_timestamp(timestamp_ * 1e9);  // in ns
  header->set_camera_timestamp(0);
  header->set_radar_timestamp(0);

  obstacles->set_error_code(error_code_);

  for (const auto &obj : objects_) {
    PerceptionObstacle *obstacle = obstacles->add_perception_obstacle();
    obj->Serialize(obstacle);
  }

  ADEBUG << "PerceptionObstacles: " << obstacles->ShortDebugString();
  return true;
}

void AsyncFusionSubnode::RegistAllAlgorithm() {
  RegisterFactoryAsyncFusion();
}

void AsyncFusionSubnode::OnChassis(const Chassis &chassis) {
  ADEBUG << "Received chassis data: run chassis callback.";
  chassis_.CopyFrom(chassis);
  chassis_speed_mps_ = chassis.speed_mps();
}

}  // namespace perception
}  // namespace apollo
<|endoftext|>


<commit_before>#include "entity_system.h"
#include "enumerate.h"
#include <entt.hpp>
#include "components/basic_info.h"
#include "components/computed_values.h"
#include "components/faction.h"
#include "components/graphics.h"
#include "components/guild.h"
#include "components/hotbar.h"
#include "components/inventory.h"
#include "components/item.h"
#include "components/level.h"
#include "components/life.h"
#include "components/magic.h"
#include "components/npc.h"
#include "components/owner.h"
#include "components/position.h"
#include "components/skills.h"
#include "components/stamina.h"
#include "components/stats.h"
#include "components/status_effects.h"
#include "components/wishlist.h"

EntitySystem::EntitySystem(std::chrono::milliseconds maxTimePerUpdate) : maxTimePerUpdate(maxTimePerUpdate) {
	logger = Core::CLog::GetLogger(Core::log_type::NETWORK).lock();
}

EntitySystem::~EntitySystem() {
    work_queue.kill();
}

void EntitySystem::add_task(std::function<void(RoseCommon::Registry&, std::chrono::milliseconds)>&& task) {
    work_queue.push_back(std::move(task));
}

void EntitySystem::update(std::chrono::milliseconds dt) {
    auto start = Core::Time::GetTickCount();
    for (auto [res, task] = work_queue.pop_front(); res;) {
        std::lock_guard<std::mutex> lock(access);
        task(registry, dt);
        const std::chrono::milliseconds diff = std::chrono::duration_cast<std::chrono::milliseconds>(Core::Time::GetTickCount() - start);
        if (diff >= maxTimePerUpdate) {
            logger->warn("Stopping after {}ms, {} tasks remaining", maxTimePerUpdate.count(), work_queue.size());
            break;
        }
        auto [tmp_res, tmp_task] = work_queue.pop_front();
        res = tmp_res;
        task = std::move(tmp_task);
    }
}

RoseCommon::Entity EntitySystem::load_character(uint32_t charId, bool platinium) {
    using namespace Component;
    auto conn = Core::connectionPool.getConnection(Core::osirose);
    Core::CharacterTable characters{};
    Core::InventoryTable inventoryTable{};
    Core::SkillTable skillsTable{};
    Core::WishTable wish{};

    auto charRes = conn(sqlpp::select(sqlpp::count(characters.id), sqlpp::all_of(characters))
                          .from(characters).where(characters.id == charId));

    if (static_cast<long>(charRes.front().count) != 1L) {
        return entt::null;
    }
    const auto& charRow = charRes.front();

    entt::prototype prototype(registry);

    auto& basicInfo = prototype.set<BasicInfo>();
    basicInfo.name = charRow.name;
    basicInfo.id = idManager.get_free_id();
    basicInfo.tag = basicInfo.id;
    basicInfo.teamId = basicInfo.id;
    basicInfo.job = charRow.job;
    basicInfo.statPoints = charRow.stat_points;
    basicInfo.skillPoints = charRow.skill_points;
    basicInfo.pkFlag = charRow.pk_flag;

    auto& computedValues = prototype.set<ComputedValues>();
    computedValues.command = RoseCommon::Command::STOP;
    computedValues.isOnMap.store(false);
    computedValues.moveMode = RoseCommon::MoveMode::WALK;
    computedValues.runSpeed = 0;
    computedValues.atkSpeed = 0;
    computedValues.weightRate = 0;

    auto& faction = prototype.set<Faction>();
    faction.id = charRow.factionid;
    faction.rank = charRow.faction_rank;
    faction.fame = charRow.fame;
    faction.factionFame[0] = charRow.faction_fame1;
    faction.factionFame[1] = charRow.faction_fame2;
    faction.points[0] = charRow.faction_points1;
    faction.points[1] = charRow.faction_points2;
    faction.points[2] = charRow.faction_points3;

    auto& graphics = prototype.set<Graphics>();
    graphics.face = charRow.face;
    graphics.hair = charRow.hair;
    graphics.race = charRow.race;

    auto& guild = prototype.set<Guild>();
    guild.id = charRow.clanid;
    guild.contribution = charRow.clan_contribution;
    guild.rank = charRow.clan_rank;

    prototype.set<Hotbar>();

    auto invRes =
      conn(sqlpp::select(sqlpp::all_of(inventoryTable)).from(inventoryTable).where(inventoryTable.charId == charId));
    auto& inventory = prototype.set<Inventory>();

    auto& level = prototype.set<Level>();
    level.xp = charRow.exp;
    level.level = charRow.level;
    level.penaltyXp = charRow.penalty_exp;

    auto& life = prototype.set<Life>();
    life.hp = charRow.maxHp / 3; // you only get 30% of your health when login in
    life.maxHp = charRow.maxHp;

    auto& magic = prototype.set<Magic>();
    magic.mp = charRow.maxMp / 3;
    magic.maxMp = charRow.maxMp;

    auto& pos = prototype.set<Position>();
    pos.x = charRow.x;
    pos.y = charRow.y;
    pos.z = 0;
    pos.spawn = charRow.reviveMap;

    auto skillRes =
      conn(sqlpp::select(skillsTable.id, skillsTable.level).from(skillsTable).where(skillsTable.charId == charId));
    auto& skills = prototype.set<Skills>();
    for (const auto& [i, row] : Core::enumerate(skillRes)) {
        skills[i].set_id(row.id);
        skills[i].set_level(row.level);
    }

    auto& stamina = prototype.set<Stamina>();
    stamina.stamina = charRow.stamina;

    auto& stats = prototype.set<Stats>();
    stats.str = charRow.str;
    stats.dex = charRow.dex;
    stats.int_ = charRow.int_;
    stats.con = charRow.con;
    stats.charm = charRow.charm;
    stats.sense = charRow.sense;
    stats.bodySize = 100;
    stats.headSize = 100;

    prototype.set<StatusEffects>();

    auto wishRes = conn(sqlpp::select(sqlpp::all_of(wish)).from(wish).where(wish.charId == charId));
    auto& wishlist = prototype.set<Wishlist>();
    for (const auto& row : wishRes) {
        if (row.slot >= RoseCommon::MAX_WISHLIST) {
            continue;
        }
        // TODO: add load_item from database (from row??)
    }

    std::lock_guard<std::mutex> lock(access);
    return prototype();
}

void EntitySystem::save_character(RoseCommon::Entity character) const {
}

RoseCommon::Entity EntitySystem::create_item(uint8_t type, uint16_t id) {
    using namespace Component;
    entt::prototype prototype(registry);

    const auto &itemDb = ItemDatabase::getInstance();
    if (!itemDb.hasItemDef(type, id)) {
        return entt::null;
    }
    const auto def = itemDb.getItemDef(type, id);

    auto& item = prototype.set<Item>();
    item.type = type;
    item.id = id;
    item.isCreated = false;
    item.life = 1000;
    item.hasSocket = false;
    item.isAppraised = false;
    item.refine = 0;
    item.count = 0;
    item.gemOpt = 0;

    std::lock_guard<std::mutex> lock(access);
    return prototype();
}
<commit_msg>Update entity_system.cpp<commit_after>#include "entity_system.h"
#include "enumerate.h"
#include <entt.hpp>
#include "components/basic_info.h"
#include "components/computed_values.h"
#include "components/faction.h"
#include "components/graphics.h"
#include "components/guild.h"
#include "components/hotbar.h"
#include "components/inventory.h"
#include "components/item.h"
#include "components/level.h"
#include "components/life.h"
#include "components/magic.h"
#include "components/npc.h"
#include "components/owner.h"
#include "components/position.h"
#include "components/skills.h"
#include "components/stamina.h"
#include "components/stats.h"
#include "components/status_effects.h"
#include "components/wishlist.h"

EntitySystem::EntitySystem(std::chrono::milliseconds maxTimePerUpdate) : maxTimePerUpdate(maxTimePerUpdate) {
	logger = Core::CLog::GetLogger(Core::log_type::NETWORK).lock();
}

EntitySystem::~EntitySystem() {
    work_queue.kill();
}

void EntitySystem::add_task(std::function<void(RoseCommon::Registry&, std::chrono::milliseconds)>&& task) {
    work_queue.push_back(std::move(task));
}

void EntitySystem::update(std::chrono::milliseconds dt) {
    auto start = Core::Time::GetTickCount();
    for (auto [res, task] = work_queue.pop_front(); res;) {
        std::lock_guard<std::mutex> lock(access);
        task(registry, dt);
        const std::chrono::milliseconds diff = std::chrono::duration_cast<std::chrono::milliseconds>(Core::Time::GetTickCount() - start);
        if (diff >= maxTimePerUpdate) {
            logger->warn("Stopping after {}ms, {} tasks remaining", maxTimePerUpdate.count(), work_queue.size());
            break;
        }
        auto [tmp_res, tmp_task] = work_queue.pop_front();
        res = tmp_res;
        task = std::move(tmp_task);
    }
}

RoseCommon::Entity EntitySystem::load_character(uint32_t charId, bool platinium) {
    using namespace Component;
    auto conn = Core::connectionPool.getConnection(Core::osirose);
    Core::CharacterTable characters{};
    Core::InventoryTable inventoryTable{};
    Core::SkillTable skillsTable{};
    Core::WishTable wish{};

    auto charRes = conn(sqlpp::select(sqlpp::count(characters.id), sqlpp::all_of(characters))
                          .from(characters).where(characters.id == charId));

    if (static_cast<long>(charRes.front().count) != 1L) {
        return entt::null;
    }
    const auto& charRow = charRes.front();

    entt::prototype prototype(registry);

    auto& basicInfo = prototype.set<BasicInfo>();
    basicInfo.name = charRow.name;
    basicInfo.id = idManager.get_free_id();
    basicInfo.tag = basicInfo.id;
    basicInfo.teamId = basicInfo.id;
    basicInfo.job = charRow.job;
    basicInfo.statPoints = charRow.stat_points;
    basicInfo.skillPoints = charRow.skill_points;
    basicInfo.pkFlag = charRow.pk_flag;

    auto& computedValues = prototype.set<ComputedValues>();
    computedValues.command = RoseCommon::Command::STOP;
    computedValues.isOnMap.store(false);
    computedValues.moveMode = RoseCommon::MoveMode::WALK;
    computedValues.runSpeed = 0;
    computedValues.atkSpeed = 0;
    computedValues.weightRate = 0;

    auto& faction = prototype.set<Faction>();
    faction.id = charRow.factionid;
    faction.rank = charRow.faction_rank;
    faction.fame = charRow.fame;
    faction.factionFame[0] = charRow.faction_fame1;
    faction.factionFame[1] = charRow.faction_fame2;
    faction.points[0] = charRow.faction_points1;
    faction.points[1] = charRow.faction_points2;
    faction.points[2] = charRow.faction_points3;

    auto& graphics = prototype.set<Graphics>();
    graphics.face = charRow.face;
    graphics.hair = charRow.hair;
    graphics.race = charRow.race;

    auto& guild = prototype.set<Guild>();
    guild.id = charRow.clanid;
    guild.contribution = charRow.clan_contribution;
    guild.rank = charRow.clan_rank;

    prototype.set<Hotbar>();

    auto invRes =
      conn(sqlpp::select(sqlpp::all_of(inventoryTable)).from(inventoryTable).where(inventoryTable.charId == charId));
    auto& inventory = prototype.set<Inventory>();

    auto& level = prototype.set<Level>();
    level.xp = charRow.exp;
    level.level = charRow.level;
    level.penaltyXp = charRow.penalty_exp;

    auto& life = prototype.set<Life>();
    life.hp = charRow.maxHp / 3; // you only get 30% of your health when login in
    life.maxHp = charRow.maxHp;

    auto& magic = prototype.set<Magic>();
    magic.mp = charRow.maxMp / 3;
    magic.maxMp = charRow.maxMp;

    auto& pos = prototype.set<Position>();
    pos.x = charRow.x;
    pos.y = charRow.y;
    pos.z = 0;
    pos.spawn = charRow.reviveMap;

    auto skillRes =
      conn(sqlpp::select(skillsTable.id, skillsTable.level).from(skillsTable).where(skillsTable.charId == charId));
    auto& skills = prototype.set<Skills>();
    for (const auto& [i, row] : Core::enumerate(skillRes)) {
        skills[i].set_id(row.id);
        skills[i].set_level(row.level);
    }

    auto& stamina = prototype.set<Stamina>();
    stamina.stamina = charRow.stamina;

    auto& stats = prototype.set<Stats>();
    stats.str = charRow.str;
    stats.dex = charRow.dex;
    stats.int_ = charRow.int_;
    stats.con = charRow.con;
    stats.charm = charRow.charm;
    stats.sense = charRow.sense;
    stats.bodySize = 100;
    stats.headSize = 100;

    prototype.set<StatusEffects>();

    auto wishRes = conn(sqlpp::select(sqlpp::all_of(wish)).from(wish).where(wish.charId == charId));
    auto& wishlist = prototype.set<Wishlist>();
    for (const auto& row : wishRes) {
        if (row.slot >= RoseCommon::MAX_WISHLIST) {
            continue;
        }
        // TODO: add load_item from database (from row??)
    }

    std::lock_guard<std::mutex> lock(access);
    return prototype();
}

void EntitySystem::save_character(RoseCommon::Entity character) const {
}

RoseCommon::Entity EntitySystem::create_item(uint8_t type, uint16_t id) {
    using namespace Component;
    entt::prototype prototype(registry);

    const auto &itemDb = RoseCommon::ItemDatabase::getInstance();
    if (!itemDb.hasItemDef(type, id)) {
        return entt::null;
    }
    const auto& def = itemDb.getItemDef(type, id);

    auto& item = prototype.set<Item>();
    item.type = type;
    item.id = id;
    item.isCreated = false;
    item.life = 1000;
    item.hasSocket = false;
    item.isAppraised = false;
    item.refine = 0;
    item.count = 0;
    item.gemOpt = 0;

    prototype.set<RoseCommon::ItemDatabase::ItemDef>(def);

    std::lock_guard<std::mutex> lock(access);
    return prototype();
}
<|endoftext|>


<commit_before>#include "Serial.hpp"
#include <stdexcept>

using namespace TurtlebotLibrary;

SerialPort::SerialPort()
{
    this->port = nullptr;
    this->baud = 0;
    this->opened = false;
	this->handle = INVALID_SERIAL_PORT_HANDLE;
}

SerialPort::SerialPort(const char *port, int baud)
{
    this->port = port;
    this->baud = baud;
    this->opened = false;
	this->handle = INVALID_SERIAL_PORT_HANDLE;
}

SerialPort::~SerialPort()
{
    this->Close();
}

bool SerialPort::IsOpen() const
{
    return this->opened;
}

const char * SerialPort::GetPort() const
{
    return this->port;
}

int SerialPort::GetBaud() const
{
    return this->baud;
}

void SerialPort::SetPortName(const char *portname)
{
    this->port = portname;
}

void SerialPort::SetBaud(int baudrate)
{
    this->baud = baudrate;
}

#ifdef _WIN32
bool SerialPort::Open()
{
    DCB dcb;
    COMMTIMEOUTS commTO;

    if (!this->port)
        return false;

    handle = CreateFile(
        port,
        GENERIC_READ | GENERIC_WRITE,
        0,
        0,
        OPEN_EXISTING,
        FILE_FLAG_WRITE_THROUGH,
        0
    );

    if (handle == INVALID_HANDLE_VALUE)
        return false;

    memset(&dcb, 0, sizeof(DCB));
    if (!GetCommState(handle, &dcb))
    {
        CloseHandle(handle);
        return false;
    }

    dcb.BaudRate = baud;
    dcb.fParity = FALSE;
    dcb.StopBits = ONESTOPBIT;
    dcb.ByteSize = 8;
    dcb.DCBlength = sizeof(dcb);
    if (!SetCommState(handle, &dcb))
    {
        CloseHandle(handle);
        return false;
    }

    memset(&commTO, 0, sizeof(COMMTIMEOUTS));
    commTO.ReadIntervalTimeout = MAXDWORD;
    if (!SetCommTimeouts(handle, &commTO))
    {
        CloseHandle(handle);
        return INVALID_HANDLE_VALUE;
    }

    this->opened = true;
    return true;
}

void SerialPort::Close()
{
	if (this->opened)
        CloseHandle(handle);

    this->opened = false;
}

int SerialPort::WriteData(const void *data, int size_bytes)
{
    DWORD bytesWritten;

    if (handle == INVALID_HANDLE_VALUE)
        return -1;

    if (!WriteFile(handle, data, size_bytes, &bytesWritten, nullptr))
        return -1;

    return bytesWritten;
}

int SerialPort::ReadData(void *data, int max_size_bytes)
{
    DWORD bytesRead;

    if (handle == INVALID_HANDLE_VALUE)
        return -1;

    if (!ReadFile(handle, data, max_size_bytes, &bytesRead, nullptr))
        return -1;

    return bytesRead;
}

#else
bool SerialPort::Open()
{
    struct termios serialPortProperties;

    if (!this->port)
        return false;

    handle = open(port, O_RDWR | O_NOCTTY);
    if (handle < 0)
        return false;

    memset(&serialPortProperties, 0, sizeof(struct termios));
    tcgetattr(handle, &serialPortProperties);
    cfsetospeed(&serialPortProperties, baud);
    cfsetispeed(&serialPortProperties, baud);

    serialPortProperties.c_cflag = (serialPortProperties.c_cflag & ~CSIZE) | CS8;
    serialPortProperties.c_cflag &= ~(PARENB | PARODD);
    serialPortProperties.c_cflag &= ~CSTOPB;
    tcflush(handle, TCIFLUSH);
    tcsetattr(handle, TCSANOW, &serialPortProperties);

    this->opened = true;
    return true;
}

void SerialPort::Close()
{
    if (handle > 0)
        close(handle);
}

int SerialPort::WriteData(const void *data, int size_bytes)
{
    if (handle < 0)
        return -1;

    return write(handle, data, size_bytes);
}

int SerialPort::ReadData(void *data, int max_size_bytes)
{
    if (handle < 0)
        return -1;

    return read(handle, data, max_size_bytes);
}

#endif<commit_msg>Port fixes for serial port destructor to linux.<commit_after>#include "Serial.hpp"
#include <stdexcept>

using namespace TurtlebotLibrary;

SerialPort::SerialPort()
{
    this->port = nullptr;
    this->baud = 0;
    this->opened = false;
	this->handle = INVALID_SERIAL_PORT_HANDLE;
}

SerialPort::SerialPort(const char *port, int baud)
{
    this->port = port;
    this->baud = baud;
    this->opened = false;
	this->handle = INVALID_SERIAL_PORT_HANDLE;
}

SerialPort::~SerialPort()
{
    this->Close();
}

bool SerialPort::IsOpen() const
{
    return this->opened;
}

const char * SerialPort::GetPort() const
{
    return this->port;
}

int SerialPort::GetBaud() const
{
    return this->baud;
}

void SerialPort::SetPortName(const char *portname)
{
    this->port = portname;
}

void SerialPort::SetBaud(int baudrate)
{
    this->baud = baudrate;
}

#ifdef _WIN32
bool SerialPort::Open()
{
    DCB dcb;
    COMMTIMEOUTS commTO;

    if (!this->port)
        return false;

    handle = CreateFile(
        port,
        GENERIC_READ | GENERIC_WRITE,
        0,
        0,
        OPEN_EXISTING,
        FILE_FLAG_WRITE_THROUGH,
        0
    );

    if (handle == INVALID_HANDLE_VALUE)
        return false;

    memset(&dcb, 0, sizeof(DCB));
    if (!GetCommState(handle, &dcb))
    {
        CloseHandle(handle);
        return false;
    }

    dcb.BaudRate = baud;
    dcb.fParity = FALSE;
    dcb.StopBits = ONESTOPBIT;
    dcb.ByteSize = 8;
    dcb.DCBlength = sizeof(dcb);
    if (!SetCommState(handle, &dcb))
    {
        CloseHandle(handle);
        return false;
    }

    memset(&commTO, 0, sizeof(COMMTIMEOUTS));
    commTO.ReadIntervalTimeout = MAXDWORD;
    if (!SetCommTimeouts(handle, &commTO))
    {
        CloseHandle(handle);
        return false;
    }

    this->opened = true;
    return true;
}

void SerialPort::Close()
{
	if (this->opened)
        CloseHandle(handle);

    this->opened = false;
}

int SerialPort::WriteData(const void *data, int size_bytes)
{
    DWORD bytesWritten;

    if (handle == INVALID_HANDLE_VALUE)
        return -1;

    if (!WriteFile(handle, data, size_bytes, &bytesWritten, nullptr))
        return -1;

    return bytesWritten;
}

int SerialPort::ReadData(void *data, int max_size_bytes)
{
    DWORD bytesRead;

    if (handle == INVALID_HANDLE_VALUE)
        return -1;

    if (!ReadFile(handle, data, max_size_bytes, &bytesRead, nullptr))
        return -1;

    return bytesRead;
}

#else
bool SerialPort::Open()
{
    struct termios serialPortProperties;

    if (!this->port)
        return false;

    handle = open(port, O_RDWR | O_NOCTTY);
    if (handle < 0)
        return false;

    memset(&serialPortProperties, 0, sizeof(struct termios));
    tcgetattr(handle, &serialPortProperties);
    cfsetospeed(&serialPortProperties, baud);
    cfsetispeed(&serialPortProperties, baud);

    serialPortProperties.c_cflag = (serialPortProperties.c_cflag & ~CSIZE) | CS8;
    serialPortProperties.c_cflag &= ~(PARENB | PARODD);
    serialPortProperties.c_cflag &= ~CSTOPB;
    tcflush(handle, TCIFLUSH);
    tcsetattr(handle, TCSANOW, &serialPortProperties);

    this->opened = true;
    return true;
}

void SerialPort::Close()
{
    if (this->opened)
        close(handle);

    this->opened = false;
}

int SerialPort::WriteData(const void *data, int size_bytes)
{
    if (handle < 0)
        return -1;

    return write(handle, data, size_bytes);
}

int SerialPort::ReadData(void *data, int max_size_bytes)
{
    if (handle < 0)
        return -1;

    return read(handle, data, max_size_bytes);
}

#endif<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

#include "modules/prediction/container/obstacles/obstacle_clusters.h"

#include <algorithm>
#include <limits>

#include "modules/prediction/common/road_graph.h"

namespace apollo {
namespace prediction {

using ::apollo::hdmap::LaneInfo;

std::unordered_map<std::string, LaneGraph> ObstacleClusters::lane_graphs_;
std::unordered_map<std::string, std::vector<LaneObstacle>>
    ObstacleClusters::lane_obstacles_;
std::unordered_map<std::string, StopSign>
    ObstacleClusters::lane_id_stop_sign_map_;

void ObstacleClusters::Clear() {
  lane_graphs_.clear();
  lane_obstacles_.clear();
  lane_id_stop_sign_map_.clear();
}

void ObstacleClusters::Init() { Clear(); }

const LaneGraph& ObstacleClusters::GetLaneGraph(
    const double start_s, const double length,
    std::shared_ptr<const LaneInfo> lane_info_ptr) {
  std::string lane_id = lane_info_ptr->id().id();
  if (lane_graphs_.find(lane_id) != lane_graphs_.end()) {
    // If this lane_segment has been used for constructing LaneGraph,
    // fetch the previously saved LaneGraph, modify its start_s,
    // then return this (save the time to construct the entire LaneGraph).
    LaneGraph* lane_graph = &lane_graphs_[lane_id];
    for (int i = 0; i < lane_graph->lane_sequence_size(); ++i) {
      LaneSequence* lane_seq_ptr = lane_graph->mutable_lane_sequence(i);
      if (lane_seq_ptr->lane_segment_size() == 0) {
        continue;
      }
      LaneSegment* first_lane_seg_ptr = lane_seq_ptr->mutable_lane_segment(0);
      if (first_lane_seg_ptr->lane_id() != lane_id) {
        continue;
      }
      first_lane_seg_ptr->set_start_s(start_s);
    }
  } else {
    // If this lane_segment has not been used for constructing LaneGraph,
    // construct the LaneGraph and return.
    RoadGraph road_graph(start_s, length, lane_info_ptr);
    LaneGraph lane_graph;
    road_graph.BuildLaneGraph(&lane_graph);
    lane_graphs_[lane_id] = std::move(lane_graph);
  }
  return lane_graphs_[lane_id];
}

LaneGraph ObstacleClusters::GetLaneGraphWithoutMemorizing(
    const double start_s, const double length,
    std::shared_ptr<const LaneInfo> lane_info_ptr) {
  RoadGraph road_graph(start_s, length, lane_info_ptr);
  LaneGraph lane_graph;
  road_graph.BuildLaneGraph(&lane_graph);
  return lane_graph;
}

void ObstacleClusters::AddObstacle(
    const int obstacle_id,
    const std::string& lane_id,
    const double lane_s,
    const double lane_l) {
  LaneObstacle lane_obstacle;
  lane_obstacle.set_obstacle_id(obstacle_id);
  lane_obstacle.set_lane_id(lane_id);
  lane_obstacle.set_lane_s(lane_s);
  lane_obstacle.set_lane_l(lane_l);
  lane_obstacles_[lane_id].push_back(std::move(lane_obstacle));
}

void ObstacleClusters::SortObstacles() {
  for (auto iter = lane_obstacles_.begin();
       iter != lane_obstacles_.end(); ++iter) {
    std::sort(iter->second.begin(), iter->second.end(),
      [](const LaneObstacle& obs0, const LaneObstacle& obs1) -> bool {
        return obs0.lane_s() < obs1.lane_s();
      });
  }
}

bool ObstacleClusters::ForwardNearbyObstacle(
    const LaneSequence& lane_sequence,
    const int obstacle_id,
    const double obstacle_s,
    const double obstacle_l,
    NearbyObstacle* const nearby_obstacle_ptr) {
  double accumulated_s = 0.0;
  for (const LaneSegment& lane_segment : lane_sequence.lane_segment()) {
    std::string lane_id = lane_segment.lane_id();
    double lane_length = lane_segment.total_length();
    if (lane_obstacles_.find(lane_id) == lane_obstacles_.end() ||
        lane_obstacles_[lane_id].empty()) {
      accumulated_s += lane_length;
      continue;
    }
    for (const LaneObstacle& lane_obstacle : lane_obstacles_[lane_id]) {
      if (lane_obstacle.obstacle_id() == obstacle_id) {
        continue;
      }
      double relative_s = accumulated_s + lane_obstacle.lane_s() - obstacle_s;
      double relative_l = lane_obstacle.lane_l() - obstacle_l;
      if (relative_s > 0.0) {
        nearby_obstacle_ptr->set_id(lane_obstacle.obstacle_id());
        nearby_obstacle_ptr->set_s(relative_s);
        nearby_obstacle_ptr->set_l(relative_l);
        return true;
      }
    }
  }
  return false;
}

bool ObstacleClusters::BackwardNearbyObstacle(
    const LaneSequence& lane_sequence,
    const int obstacle_id,
    const double obstacle_s,
    const double obstacle_l,
    NearbyObstacle* const nearby_obstacle_ptr) {
  if (lane_sequence.lane_segment_size() == 0) {
    AERROR << "Empty lane sequence found.";
    return false;
  }
  const LaneSegment& lane_segment = lane_sequence.lane_segment(0);
  std::string lane_id = lane_segment.lane_id();

  // Search current lane
  if (lane_obstacles_.find(lane_id) != lane_obstacles_.end() &&
      !lane_obstacles_[lane_id].empty()) {
    for (std::size_t i = lane_obstacles_[lane_id].size() - 1; i >= 0; --i) {
      const LaneObstacle& lane_obstacle = lane_obstacles_[lane_id][i];
      if (lane_obstacle.obstacle_id() == obstacle_id) {
        continue;
      }
      double relative_s = lane_obstacle.lane_s() - obstacle_s;
      double relative_l = lane_obstacle.lane_l() - obstacle_l;
      if (relative_s < 0.0) {
        nearby_obstacle_ptr->set_id(lane_obstacle.obstacle_id());
        nearby_obstacle_ptr->set_s(relative_s);
        nearby_obstacle_ptr->set_l(relative_l);
        return true;
      }
    }
  }

  // Search backward lanes
  std::shared_ptr<const LaneInfo> lane_info_ptr =
      PredictionMap::LaneById(lane_id);
  bool found_one_behind = false;
  double relative_s = -std::numeric_limits<double>::infinity();
  double relative_l = 0.0;
  for (const auto& predecessor_lane_id :
       lane_info_ptr->lane().predecessor_id()) {
    std::string lane_id = predecessor_lane_id.id();
    if (lane_obstacles_.find(lane_id) == lane_obstacles_.end() ||
        lane_obstacles_[lane_id].empty()) {
      continue;
    }
    std::shared_ptr<const LaneInfo> pred_lane_info_ptr =
      PredictionMap::LaneById(predecessor_lane_id.id());
    const LaneObstacle& backward_obs = lane_obstacles_[lane_id].back();
    double delta_s = backward_obs.lane_s() -
                     (obstacle_s + pred_lane_info_ptr->total_length());
    found_one_behind = true;
    if (delta_s > relative_s) {
      relative_s = delta_s;
      relative_l = backward_obs.lane_l() - obstacle_l;
      nearby_obstacle_ptr->set_id(backward_obs.obstacle_id());
      nearby_obstacle_ptr->set_s(relative_s);
      nearby_obstacle_ptr->set_l(relative_l);
    }
  }

  return found_one_behind;
}

StopSign ObstacleClusters::QueryStopSignByLaneId(const std::string& lane_id) {
  StopSign stop_sign;
  // Find the stop_sign by lane_id in the hashtable
  if (lane_id_stop_sign_map_.find(lane_id) != lane_id_stop_sign_map_.end()) {
    return lane_id_stop_sign_map_[lane_id];
  }
  std::shared_ptr<const LaneInfo> lane_info_ptr =
      PredictionMap::LaneById(lane_id);
  CHECK_NOTNULL(lane_info_ptr);
  for (const auto &overlap_id : lane_info_ptr->lane().overlap_id()) {
    auto overlap_info_ptr = PredictionMap::OverlapById(overlap_id.id());
    if (overlap_info_ptr == nullptr) {
      continue;
    }
    for (const auto &object : overlap_info_ptr->overlap().object()) {
      // find the overlap with stop_sign
      if (object.has_stop_sign_overlap_info()) {
        for (const auto &obj : overlap_info_ptr->overlap().object()) {
          // find the obj of in the overlap
          if (obj.has_lane_overlap_info()) {
            if (!stop_sign.has_lane_s() ||
                stop_sign.lane_s() > obj.lane_overlap_info().start_s()) {
              stop_sign.set_stop_sign_id(object.id().id());
              stop_sign.set_lane_id(lane_id);
              stop_sign.set_lane_s(obj.lane_overlap_info().start_s());
              lane_id_stop_sign_map_[lane_id] = stop_sign;
            }
          }
        }
      }
    }
  }
  return lane_id_stop_sign_map_[lane_id];
}

}  // namespace prediction
}  // namespace apollo
<commit_msg>Prediction: fix infinite loop on search curr_lane<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

#include "modules/prediction/container/obstacles/obstacle_clusters.h"

#include <algorithm>
#include <limits>

#include "modules/prediction/common/road_graph.h"

namespace apollo {
namespace prediction {

using ::apollo::hdmap::LaneInfo;

std::unordered_map<std::string, LaneGraph> ObstacleClusters::lane_graphs_;
std::unordered_map<std::string, std::vector<LaneObstacle>>
    ObstacleClusters::lane_obstacles_;
std::unordered_map<std::string, StopSign>
    ObstacleClusters::lane_id_stop_sign_map_;

void ObstacleClusters::Clear() {
  lane_graphs_.clear();
  lane_obstacles_.clear();
  lane_id_stop_sign_map_.clear();
}

void ObstacleClusters::Init() { Clear(); }

const LaneGraph& ObstacleClusters::GetLaneGraph(
    const double start_s, const double length,
    std::shared_ptr<const LaneInfo> lane_info_ptr) {
  std::string lane_id = lane_info_ptr->id().id();
  if (lane_graphs_.find(lane_id) != lane_graphs_.end()) {
    // If this lane_segment has been used for constructing LaneGraph,
    // fetch the previously saved LaneGraph, modify its start_s,
    // then return this (save the time to construct the entire LaneGraph).
    LaneGraph* lane_graph = &lane_graphs_[lane_id];
    for (int i = 0; i < lane_graph->lane_sequence_size(); ++i) {
      LaneSequence* lane_seq_ptr = lane_graph->mutable_lane_sequence(i);
      if (lane_seq_ptr->lane_segment_size() == 0) {
        continue;
      }
      LaneSegment* first_lane_seg_ptr = lane_seq_ptr->mutable_lane_segment(0);
      if (first_lane_seg_ptr->lane_id() != lane_id) {
        continue;
      }
      first_lane_seg_ptr->set_start_s(start_s);
    }
  } else {
    // If this lane_segment has not been used for constructing LaneGraph,
    // construct the LaneGraph and return.
    RoadGraph road_graph(start_s, length, lane_info_ptr);
    LaneGraph lane_graph;
    road_graph.BuildLaneGraph(&lane_graph);
    lane_graphs_[lane_id] = std::move(lane_graph);
  }
  return lane_graphs_[lane_id];
}

LaneGraph ObstacleClusters::GetLaneGraphWithoutMemorizing(
    const double start_s, const double length,
    std::shared_ptr<const LaneInfo> lane_info_ptr) {
  RoadGraph road_graph(start_s, length, lane_info_ptr);
  LaneGraph lane_graph;
  road_graph.BuildLaneGraph(&lane_graph);
  return lane_graph;
}

void ObstacleClusters::AddObstacle(
    const int obstacle_id,
    const std::string& lane_id,
    const double lane_s,
    const double lane_l) {
  LaneObstacle lane_obstacle;
  lane_obstacle.set_obstacle_id(obstacle_id);
  lane_obstacle.set_lane_id(lane_id);
  lane_obstacle.set_lane_s(lane_s);
  lane_obstacle.set_lane_l(lane_l);
  lane_obstacles_[lane_id].push_back(std::move(lane_obstacle));
}

void ObstacleClusters::SortObstacles() {
  for (auto iter = lane_obstacles_.begin();
       iter != lane_obstacles_.end(); ++iter) {
    std::sort(iter->second.begin(), iter->second.end(),
      [](const LaneObstacle& obs0, const LaneObstacle& obs1) -> bool {
        return obs0.lane_s() < obs1.lane_s();
      });
  }
}

bool ObstacleClusters::ForwardNearbyObstacle(
    const LaneSequence& lane_sequence,
    const int obstacle_id,
    const double obstacle_s,
    const double obstacle_l,
    NearbyObstacle* const nearby_obstacle_ptr) {
  double accumulated_s = 0.0;
  for (const LaneSegment& lane_segment : lane_sequence.lane_segment()) {
    std::string lane_id = lane_segment.lane_id();
    double lane_length = lane_segment.total_length();
    if (lane_obstacles_.find(lane_id) == lane_obstacles_.end() ||
        lane_obstacles_[lane_id].empty()) {
      accumulated_s += lane_length;
      continue;
    }
    for (const LaneObstacle& lane_obstacle : lane_obstacles_[lane_id]) {
      if (lane_obstacle.obstacle_id() == obstacle_id) {
        continue;
      }
      double relative_s = accumulated_s + lane_obstacle.lane_s() - obstacle_s;
      double relative_l = lane_obstacle.lane_l() - obstacle_l;
      if (relative_s > 0.0) {
        nearby_obstacle_ptr->set_id(lane_obstacle.obstacle_id());
        nearby_obstacle_ptr->set_s(relative_s);
        nearby_obstacle_ptr->set_l(relative_l);
        return true;
      }
    }
  }
  return false;
}

bool ObstacleClusters::BackwardNearbyObstacle(
    const LaneSequence& lane_sequence,
    const int obstacle_id,
    const double obstacle_s,
    const double obstacle_l,
    NearbyObstacle* const nearby_obstacle_ptr) {
  if (lane_sequence.lane_segment_size() == 0) {
    AERROR << "Empty lane sequence found.";
    return false;
  }
  const LaneSegment& lane_segment = lane_sequence.lane_segment(0);
  std::string lane_id = lane_segment.lane_id();

  // Search current lane
  if (lane_obstacles_.find(lane_id) != lane_obstacles_.end() &&
      !lane_obstacles_[lane_id].empty()) {
    for (int i = static_cast<int>(lane_obstacles_[lane_id].size()) - 1;
        i >= 0; --i) {
      const LaneObstacle& lane_obstacle = lane_obstacles_[lane_id][i];
      if (lane_obstacle.obstacle_id() == obstacle_id) {
        continue;
      }
      double relative_s = lane_obstacle.lane_s() - obstacle_s;
      double relative_l = lane_obstacle.lane_l() - obstacle_l;
      if (relative_s < 0.0) {
        nearby_obstacle_ptr->set_id(lane_obstacle.obstacle_id());
        nearby_obstacle_ptr->set_s(relative_s);
        nearby_obstacle_ptr->set_l(relative_l);
        return true;
      }
    }
  }

  // Search backward lanes
  std::shared_ptr<const LaneInfo> lane_info_ptr =
      PredictionMap::LaneById(lane_id);
  bool found_one_behind = false;
  double relative_s = -std::numeric_limits<double>::infinity();
  double relative_l = 0.0;
  for (const auto& predecessor_lane_id :
       lane_info_ptr->lane().predecessor_id()) {
    std::string lane_id = predecessor_lane_id.id();
    if (lane_obstacles_.find(lane_id) == lane_obstacles_.end() ||
        lane_obstacles_[lane_id].empty()) {
      continue;
    }
    std::shared_ptr<const LaneInfo> pred_lane_info_ptr =
      PredictionMap::LaneById(predecessor_lane_id.id());
    const LaneObstacle& backward_obs = lane_obstacles_[lane_id].back();
    double delta_s = backward_obs.lane_s() -
                     (obstacle_s + pred_lane_info_ptr->total_length());
    found_one_behind = true;
    if (delta_s > relative_s) {
      relative_s = delta_s;
      relative_l = backward_obs.lane_l() - obstacle_l;
      nearby_obstacle_ptr->set_id(backward_obs.obstacle_id());
      nearby_obstacle_ptr->set_s(relative_s);
      nearby_obstacle_ptr->set_l(relative_l);
    }
  }

  return found_one_behind;
}

StopSign ObstacleClusters::QueryStopSignByLaneId(const std::string& lane_id) {
  StopSign stop_sign;
  // Find the stop_sign by lane_id in the hashtable
  if (lane_id_stop_sign_map_.find(lane_id) != lane_id_stop_sign_map_.end()) {
    return lane_id_stop_sign_map_[lane_id];
  }
  std::shared_ptr<const LaneInfo> lane_info_ptr =
      PredictionMap::LaneById(lane_id);
  CHECK_NOTNULL(lane_info_ptr);
  for (const auto &overlap_id : lane_info_ptr->lane().overlap_id()) {
    auto overlap_info_ptr = PredictionMap::OverlapById(overlap_id.id());
    if (overlap_info_ptr == nullptr) {
      continue;
    }
    for (const auto &object : overlap_info_ptr->overlap().object()) {
      // find the overlap with stop_sign
      if (object.has_stop_sign_overlap_info()) {
        for (const auto &obj : overlap_info_ptr->overlap().object()) {
          // find the obj of in the overlap
          if (obj.has_lane_overlap_info()) {
            if (!stop_sign.has_lane_s() ||
                stop_sign.lane_s() > obj.lane_overlap_info().start_s()) {
              stop_sign.set_stop_sign_id(object.id().id());
              stop_sign.set_lane_id(lane_id);
              stop_sign.set_lane_s(obj.lane_overlap_info().start_s());
              lane_id_stop_sign_map_[lane_id] = stop_sign;
            }
          }
        }
      }
    }
  }
  return lane_id_stop_sign_map_[lane_id];
}

}  // namespace prediction
}  // namespace apollo
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2013 Pavlo Lavrenenko
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <Shader.h>
#include <unordered_map>
#include <stdexcept>
#include <sstream>
#include <fstream>
#include <vector>
#include <memory>
#ifdef GRAPHENE_DEBUG
#include <iostream>
#endif

namespace Graphene {

GLuint Shader::activeProgram = 0;

Shader::Shader(const std::string& name) {
    std::ifstream file(name, std::ios::binary);
    if (!file.good()) {
        throw std::runtime_error("Failed to open `" + name + "'");
    }

    file.seekg(0, std::ios::end);
    int sourceLength = file.tellg();
    file.seekg(0, std::ios::beg);

    std::unique_ptr<char[]> source(new char[sourceLength]);
    file.read(source.get(), sourceLength);
    file.close();

    this->source = std::string(source.get(), sourceLength);
    this->buildShader();
}

void Shader::buildShader() {
    std::vector<GLuint> shaders;
    std::unordered_map<std::string, GLenum> shaderTypes = {
        { "#define TYPE_VERTEX\n",   GL_VERTEX_SHADER },
        { "#define TYPE_FRAGMENT\n", GL_FRAGMENT_SHADER }
    };

    for (auto& shaderType: shaderTypes) {
        std::stringstream modifiedSource;
        modifiedSource << shaderType.first;
        modifiedSource << this->source;
        shaders.push_back(this->compile(modifiedSource.str(), shaderType.second));
    }

    this->program = this->link(shaders);
}

GLuint Shader::compile(const std::string& source, GLenum type) {
    GLuint shader = glCreateShader(type);
    const char* sourceStrings = source.c_str();

    glShaderSource(shader, 1, &sourceStrings, nullptr);
    glCompileShader(shader);

    GLint infoLogLength;
    glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &infoLogLength);

    std::unique_ptr<GLchar[]> infoLog;
    if (infoLogLength > 0) {  // ATI workaround, successful compilation
        infoLog.reset(new GLchar[infoLogLength]);
        glGetShaderInfoLog(shader, infoLogLength, nullptr, infoLog.get());
    }

    GLint compileStatus;
    glGetShaderiv(shader, GL_COMPILE_STATUS, &compileStatus);

    if (compileStatus == GL_FALSE) {
        glDeleteShader(shader);

        std::stringstream errorMessage;
        errorMessage << "Failed to compile shader\n" << infoLog.get();
        throw std::runtime_error(errorMessage.str());
    }

#ifdef GRAPHENE_DEBUG
    if (infoLogLength > 0) {
        std::cout << "Shader compiled\n" << infoLog.get() << std::endl;
    }
#endif

    return shader;
}

GLuint Shader::link(const std::vector<GLuint>& shaders) {
    GLuint program = glCreateProgram();

    for (auto& shader: shaders) {
        glAttachShader(program, shader);
        glDeleteShader(shader);
    }

    glLinkProgram(program);

    GLint infoLogLength;
    glGetProgramiv(program, GL_INFO_LOG_LENGTH, &infoLogLength);

    std::unique_ptr<GLchar[]> infoLog;
    if (infoLogLength > 0) {  // ATI workaround, successful linking
        infoLog.reset(new GLchar[infoLogLength]);
        glGetProgramInfoLog(program, infoLogLength, nullptr, infoLog.get());
    }

    GLint linkStatus;
    glGetProgramiv(program, GL_LINK_STATUS, &linkStatus);

    if (linkStatus == GL_FALSE) {
        glDeleteProgram(program);

        std::stringstream errorMessage;
        errorMessage << "Failed to link shader\n" << infoLog.get();
        throw std::runtime_error(errorMessage.str());
    }

#ifdef GRAPHENE_DEBUG
    if (infoLogLength > 0) {
        std::cout << "Shader linked\n" << infoLog.get() << std::endl;
    }
#endif

    return program;
}

}  // namespace Graphene
<commit_msg>Fix shader compilation output<commit_after>/*
 * Copyright (c) 2013 Pavlo Lavrenenko
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <Shader.h>
#include <unordered_map>
#include <stdexcept>
#include <sstream>
#include <fstream>
#include <vector>
#include <memory>
#ifdef GRAPHENE_DEBUG
#include <iostream>
#endif

namespace Graphene {

GLuint Shader::activeProgram = 0;

Shader::Shader(const std::string& name) {
    std::ifstream file(name, std::ios::binary);
    if (!file.good()) {
        throw std::runtime_error("Failed to open `" + name + "'");
    }

    file.seekg(0, std::ios::end);
    int sourceLength = file.tellg();
    file.seekg(0, std::ios::beg);

    std::unique_ptr<char[]> source(new char[sourceLength]);
    file.read(source.get(), sourceLength);
    file.close();

    this->source = std::string(source.get(), sourceLength);
    this->buildShader();
}

void Shader::buildShader() {
    std::vector<GLuint> shaders;
    std::unordered_map<std::string, GLenum> shaderTypes = {
        { "#define TYPE_VERTEX\n",   GL_VERTEX_SHADER },
        { "#define TYPE_FRAGMENT\n", GL_FRAGMENT_SHADER }
    };

    for (auto& shaderType: shaderTypes) {
        std::stringstream modifiedSource;
        modifiedSource << shaderType.first;
        modifiedSource << this->source;
        shaders.push_back(this->compile(modifiedSource.str(), shaderType.second));
    }

    this->program = this->link(shaders);
}

GLuint Shader::compile(const std::string& source, GLenum type) {
    GLuint shader = glCreateShader(type);
    const char* sourceStrings = source.c_str();

    glShaderSource(shader, 1, &sourceStrings, nullptr);
    glCompileShader(shader);

    GLint infoLogLength;
    glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &infoLogLength);

    std::unique_ptr<GLchar[]> infoLog;
    if (infoLogLength > 1) {
        infoLog.reset(new GLchar[infoLogLength]);
        glGetShaderInfoLog(shader, infoLogLength, nullptr, infoLog.get());
#ifdef GRAPHENE_DEBUG
        std::cout << "Shader compiled\n" << infoLog.get() << std::endl;
#endif
    }

    GLint compileStatus;
    glGetShaderiv(shader, GL_COMPILE_STATUS, &compileStatus);

    if (compileStatus == GL_FALSE) {
        glDeleteShader(shader);

        std::stringstream errorMessage;
        errorMessage << "Failed to compile shader\n" << infoLog.get();
        throw std::runtime_error(errorMessage.str());
    }

    return shader;
}

GLuint Shader::link(const std::vector<GLuint>& shaders) {
    GLuint program = glCreateProgram();

    for (auto& shader: shaders) {
        glAttachShader(program, shader);
        glDeleteShader(shader);
    }

    glLinkProgram(program);

    GLint infoLogLength;
    glGetProgramiv(program, GL_INFO_LOG_LENGTH, &infoLogLength);

    std::unique_ptr<GLchar[]> infoLog;
    if (infoLogLength > 1) {
        infoLog.reset(new GLchar[infoLogLength]);
        glGetProgramInfoLog(program, infoLogLength, nullptr, infoLog.get());
#ifdef GRAPHENE_DEBUG
        std::cout << "Shader linked\n" << infoLog.get() << std::endl;
#endif
    }

    GLint linkStatus;
    glGetProgramiv(program, GL_LINK_STATUS, &linkStatus);

    if (linkStatus == GL_FALSE) {
        glDeleteProgram(program);

        std::stringstream errorMessage;
        errorMessage << "Failed to link shader\n" << infoLog.get();
        throw std::runtime_error(errorMessage.str());
    }

    return program;
}

}  // namespace Graphene
<|endoftext|>