<commit_before>/*
 * OSPRayStructuredVolume.cpp
 * Copyright (C) 2009-2018 by MegaMol Team
 * Alle Rechte vorbehalten.
 */

#include "OSPRayStructuredVolume.h"
#include "geometry_calls/VolumetricDataCall.h"
#include "mmcore/Call.h"
#include "mmcore/param/BoolParam.h"
#include "mmcore/param/EnumParam.h"
#include "mmcore/param/FloatParam.h"
#include "mmcore/param/StringParam.h"
#include "mmcore/param/Vector3fParam.h"
#include "mmcore/utility/log/Log.h"
#include "mmcore/view/CallGetTransferFunction.h"
#include "stdafx.h"

using namespace megamol::ospray;

OSPRayStructuredVolume::OSPRayStructuredVolume(void)
        : AbstractOSPRayStructure()
        , getDataSlot("getdata", "Connects to the data source")
        , getTFSlot("gettransferfunction", "Connects to a color transfer function module")
        , clippingBoxLower("ClippingBox::Left", "Left corner of the clipping Box")
        , clippingBoxUpper("ClippingBox::Right", "Right corner of the clipping Box")
        , clippingBoxActive("ClippingBox::Active", "Activates the clipping Box")
        , repType(
              "Representation", "Activates one of the three different volume representations: Volume, Isosurfae, Slice")
        , IsoValue("Isosurface::Isovalue", "Sets the isovalue of the isosurface")
        , showBoundingBox("showBoundingBox", "Bounding box of the volume data set") {
    core::param::EnumParam* rt = new core::param::EnumParam(VOLUMEREP);
    rt->SetTypePair(VOLUMEREP, "Volume");
    rt->SetTypePair(ISOSURFACE, "Isosurface");
    this->repType << rt;
    this->MakeSlotAvailable(&this->repType);

    this->clippingBoxActive << new core::param::BoolParam(false);
    this->clippingBoxLower << new core::param::Vector3fParam({-5.0f, -5.0f, -5.0f});
    this->clippingBoxUpper << new core::param::Vector3fParam({0.0f, 5.0f, 5.0f});
    this->MakeSlotAvailable(&this->clippingBoxActive);
    this->MakeSlotAvailable(&this->clippingBoxLower);
    this->MakeSlotAvailable(&this->clippingBoxUpper);

    this->IsoValue << new core::param::FloatParam(0.1f);
    this->MakeSlotAvailable(&this->IsoValue);

    this->getDataSlot.SetCompatibleCall<geocalls::VolumetricDataCallDescription>();
    this->MakeSlotAvailable(&this->getDataSlot);

    this->getTFSlot.SetCompatibleCall<core::view::CallGetTransferFunctionDescription>();
    this->MakeSlotAvailable(&this->getTFSlot);

    this->showBoundingBox << new core::param::StringParam("");
    this->showBoundingBox.Parameter()->SetGUIReadOnly(true);
    this->MakeSlotAvailable(&this->showBoundingBox);

    // this->SetSlotUnavailable(&this->getMaterialSlot);
}

bool OSPRayStructuredVolume::readData(core::Call& call) {

    // fill material container
    this->processMaterial();

    // get transformation parameter
    //this->processTransformation();

    // fill clipping plane container
    this->processClippingPlane();

    // read Data, calculate  shape parameters, fill data vectors
    auto os = dynamic_cast<CallOSPRayStructure*>(&call);
    auto cd = this->getDataSlot.CallAs<geocalls::VolumetricDataCall>();
    auto const cgtf = this->getTFSlot.CallAs<core::view::CallGetTransferFunction>();

    this->structureContainer.dataChanged = false;
    if (cd == nullptr)
        return false;
    if (cgtf == nullptr) {
        core::utility::log::Log::DefaultLog.WriteError("[OSPRayStructuredVolume] No transferfunction connected.");
        return false;
    }

    uint32_t requested_frame = os->getTime();
    if (requested_frame >= cd->FrameCount()) {
        requested_frame = cd->FrameCount() - 1;
    }
    do {
        cd->SetFrameID(requested_frame, true);
        if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_EXTENTS))
            return false;
        if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_METADATA))
            return false;
        if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_DATA))
            return false;
    } while (cd->FrameID() != requested_frame);

    // do the callback to set the dirty flag
    if (!(*cgtf)(0))
        return false;
    auto interface_diry = this->InterfaceIsDirty();
    auto tf_dirty = cgtf->IsDirty();
    if (this->datahash != cd->DataHash() || this->time != os->getTime() || interface_diry || tf_dirty) {
        this->datahash = cd->DataHash();
        this->time = os->getTime();
        this->structureContainer.dataChanged = true;
    } else {
        return true;
    }

    auto const metadata = cd->GetMetadata();

    if (metadata->GridType != geocalls::CARTESIAN) {
        core::utility::log::Log::DefaultLog.WriteError(
            "OSPRayStructuredVolume only works with cartesian grids (for now)");
        return false;
    }

    unsigned int const voxelCount = metadata->Resolution[0] * metadata->Resolution[1] * metadata->Resolution[2];
    std::array<float, 3> gridOrigin = {metadata->Origin[0], metadata->Origin[1], metadata->Origin[2]};
    std::array<float, 3> gridSpacing = {
        metadata->SliceDists[0][0], metadata->SliceDists[1][0], metadata->SliceDists[2][0]};
    std::array<int, 3> dimensions = {static_cast<int>(metadata->Resolution[0]),
        static_cast<int>(metadata->Resolution[1]),
        static_cast<int>(metadata->Resolution[2])}; //< TODO HAZARD explicit narrowing

    unsigned int const maxDim =
        std::max<size_t>(metadata->Resolution[0], std::max<size_t>(metadata->Resolution[1], metadata->Resolution[2]));

    voxelDataType voxelType = {};

    switch (metadata->ScalarType) {
    case geocalls::FLOATING_POINT:
        if (metadata->ScalarLength == 4) {
            voxelType = voxelDataType::FLOAT;
        } else {
            voxelType = voxelDataType::DOUBLE;
        }
        break;
    case geocalls::UNSIGNED_INTEGER:
        if (metadata->ScalarLength == 1) {
            voxelType = voxelDataType::UCHAR;
        } else if (metadata->ScalarLength == 2) {
            voxelType = voxelDataType::USHORT;
        } else {
            core::utility::log::Log::DefaultLog.WriteError(
                "Unsigned integers with a length greater than 2 are invalid.");
            return false;
        }
        break;
    case geocalls::SIGNED_INTEGER:
        if (metadata->ScalarLength == 2) {
            voxelType = voxelDataType::SHORT;
        } else {
            core::utility::log::Log::DefaultLog.WriteError("Integers with a length != 2 are invalid.");
            return false;
        }
        break;
    case geocalls::BITS:
        core::utility::log::Log::DefaultLog.WriteError("Invalid datatype.");
        return false;
        break;
    }

    // get color transfer function
    std::vector<float> rgb;
    std::vector<float> a;

    if ((*cgtf)(0)) {
        if (cgtf->TFTextureFormat() == core::view::CallGetTransferFunction::TextureFormat::TEXTURE_FORMAT_RGBA) {
            auto const numColors = cgtf->TextureSize();
            rgb.resize(3 * numColors);
            a.resize(numColors);
            auto const texture = cgtf->GetTextureData();

            for (unsigned int i = 0; i < numColors; ++i) {
                rgb[i * 3 + 0] = texture[i * 4 + 0];
                rgb[i * 3 + 1] = texture[i * 4 + 1];
                rgb[i * 3 + 2] = texture[i * 4 + 2];
                a[i] = texture[i * 4 + 3];
            }
        } else {
            auto const numColors = cgtf->TextureSize();
            rgb.resize(3 * numColors);
            a.resize(numColors);
            auto const texture = cgtf->GetTextureData();

            for (unsigned int i = 0; i < numColors; ++i) {
                rgb[i * 3 + 0] = texture[i * 4 + 0];
                rgb[i * 3 + 1] = texture[i * 4 + 1];
                rgb[i * 3 + 2] = texture[i * 4 + 2];
                a[i] = i / (numColors - 1.0f);
            }
            core::utility::log::Log::DefaultLog.WriteWarn(
                "OSPRayStructuredVolume: No alpha channel in transfer function "
                "connected to module. Adding alpha ramp to RGB colors.\n");
        }
    } else {
        core::utility::log::Log::DefaultLog.WriteError(
            "OSPRayStructuredVolume: No transfer function connected to module");
        return false;
    }
    cgtf->ResetDirty();

    // Write stuff into the structureContainer

    this->structureContainer.type = structureTypeEnum::VOLUME;
    this->structureContainer.volumeType = volumeTypeEnum::STRUCTUREDVOLUME;
    structuredVolumeStructure svs;

    svs.volRepType = (volumeRepresentationType)this->repType.Param<core::param::EnumParam>()->Value();
    svs.voxels = cd->GetData();
    svs.gridOrigin = gridOrigin;
    svs.gridSpacing = gridSpacing;
    svs.dimensions = dimensions;
    svs.voxelCount = voxelCount;
    svs.maxDim = maxDim;
    svs.valueRange = {static_cast<float>(metadata->MinValues[0]), static_cast<float>(metadata->MaxValues[0])};
    svs.tfRGB = std::make_shared<std::vector<float>>(std::move(rgb));
    svs.tfA = std::make_shared<std::vector<float>>(std::move(a));
    svs.voxelDType = voxelType;

    svs.clippingBoxActive = this->clippingBoxActive.Param<core::param::BoolParam>()->Value();
    std::array<float, 3> cbl = {this->clippingBoxLower.Param<core::param::Vector3fParam>()->Value().GetX(),
        this->clippingBoxLower.Param<core::param::Vector3fParam>()->Value().GetY(),
        this->clippingBoxLower.Param<core::param::Vector3fParam>()->Value().GetZ()};
    svs.clippingBoxLower = cbl;
    std::array<float, 3> cbu = {this->clippingBoxUpper.Param<core::param::Vector3fParam>()->Value().GetX(),
        this->clippingBoxUpper.Param<core::param::Vector3fParam>()->Value().GetY(),
        this->clippingBoxUpper.Param<core::param::Vector3fParam>()->Value().GetZ()};
    svs.clippingBoxUpper = cbu;

    svs.isoValue = this->IsoValue.Param<core::param::FloatParam>()->Value();

    this->structureContainer.structure = svs;

    return true;
}

OSPRayStructuredVolume::~OSPRayStructuredVolume() {
    this->Release();
}

bool OSPRayStructuredVolume::create() {
    return true;
}

void OSPRayStructuredVolume::release() {}

/*
ospray::OSPRaySphereGeometry::InterfaceIsDirty()
*/
bool OSPRayStructuredVolume::InterfaceIsDirty() {
    if (this->clippingBoxActive.IsDirty() || this->clippingBoxLower.IsDirty() || this->clippingBoxUpper.IsDirty() ||
        this->IsoValue.IsDirty() || this->repType.IsDirty()) {
        this->clippingBoxActive.ResetDirty();
        this->clippingBoxLower.ResetDirty();
        this->clippingBoxUpper.ResetDirty();
        this->IsoValue.ResetDirty();
        this->repType.ResetDirty();
        return true;
    } else {
        return false;
    }
}

bool OSPRayStructuredVolume::getExtends(core::Call& call) {
    auto os = dynamic_cast<CallOSPRayStructure*>(&call);
    auto cd = this->getDataSlot.CallAs<geocalls::VolumetricDataCall>();

    if (cd == nullptr)
        return false;
    if (os->getTime() > cd->FrameCount()) {
        cd->SetFrameID(cd->FrameCount() - 1, true); // isTimeForced flag set to true
    } else {
        cd->SetFrameID(os->getTime(), true); // isTimeForced flag set to true
    }

    if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_EXTENTS))
        return false;
    if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_METADATA))
        return false;

    this->extendContainer.boundingBox = std::make_shared<core::BoundingBoxes_2>();
    this->extendContainer.boundingBox->SetBoundingBox(cd->AccessBoundingBoxes().ObjectSpaceBBox());
    std::string bbox_string = "LEFT: " + std::to_string(extendContainer.boundingBox->BoundingBox().Left()) +
                              ";\nBOTTOM: " + std::to_string(extendContainer.boundingBox->BoundingBox().Bottom()) +
                              ";\nBACK: " + std::to_string(extendContainer.boundingBox->BoundingBox().Back()) +
                              ";\nRIGHT: " + std::to_string(extendContainer.boundingBox->BoundingBox().Right()) +
                              ";\nTOP: " + std::to_string(extendContainer.boundingBox->BoundingBox().Top()) +
                              ";\nFRONT: " + std::to_string(extendContainer.boundingBox->BoundingBox().Bottom());
    this->showBoundingBox.Param<core::param::StringParam>()->SetValue(bbox_string.c_str());
    this->extendContainer.timeFramesCount = cd->FrameCount();
    this->extendContainer.isValid = true;

    return true;
}
<commit_msg>constructor thingy<commit_after>/*
 * OSPRayStructuredVolume.cpp
 * Copyright (C) 2009-2018 by MegaMol Team
 * Alle Rechte vorbehalten.
 */

#include "OSPRayStructuredVolume.h"
#include "geometry_calls/VolumetricDataCall.h"
#include "mmcore/Call.h"
#include "mmcore/param/BoolParam.h"
#include "mmcore/param/EnumParam.h"
#include "mmcore/param/FloatParam.h"
#include "mmcore/param/StringParam.h"
#include "mmcore/param/Vector3fParam.h"
#include "mmcore/utility/log/Log.h"
#include "mmcore/view/CallGetTransferFunction.h"
#include "stdafx.h"

using namespace megamol::ospray;

OSPRayStructuredVolume::OSPRayStructuredVolume(void)
        : AbstractOSPRayStructure()
        , getDataSlot("getdata", "Connects to the data source")
        , getTFSlot("gettransferfunction", "Connects to a color transfer function module")
        , clippingBoxLower("ClippingBox::Left", "Left corner of the clipping Box")
        , clippingBoxUpper("ClippingBox::Right", "Right corner of the clipping Box")
        , clippingBoxActive("ClippingBox::Active", "Activates the clipping Box")
        , repType(
              "Representation", "Activates one of the three different volume representations: Volume, Isosurfae, Slice")
        , IsoValue("Isosurface::Isovalue", "Sets the isovalue of the isosurface")
        , showBoundingBox("showBoundingBox", "Bounding box of the volume data set") {
    core::param::EnumParam* rt = new core::param::EnumParam(VOLUMEREP);
    rt->SetTypePair(VOLUMEREP, "Volume");
    rt->SetTypePair(ISOSURFACE, "Isosurface");
    this->repType << rt;
    this->MakeSlotAvailable(&this->repType);

    this->clippingBoxActive << new core::param::BoolParam(false);
    this->clippingBoxLower << new core::param::Vector3fParam(-5.0f, -5.0f, -5.0f);
    this->clippingBoxUpper << new core::param::Vector3fParam(0.0f, 5.0f, 5.0f);
    this->MakeSlotAvailable(&this->clippingBoxActive);
    this->MakeSlotAvailable(&this->clippingBoxLower);
    this->MakeSlotAvailable(&this->clippingBoxUpper);

    this->IsoValue << new core::param::FloatParam(0.1f);
    this->MakeSlotAvailable(&this->IsoValue);

    this->getDataSlot.SetCompatibleCall<geocalls::VolumetricDataCallDescription>();
    this->MakeSlotAvailable(&this->getDataSlot);

    this->getTFSlot.SetCompatibleCall<core::view::CallGetTransferFunctionDescription>();
    this->MakeSlotAvailable(&this->getTFSlot);

    this->showBoundingBox << new core::param::StringParam("");
    this->showBoundingBox.Parameter()->SetGUIReadOnly(true);
    this->MakeSlotAvailable(&this->showBoundingBox);

    // this->SetSlotUnavailable(&this->getMaterialSlot);
}

bool OSPRayStructuredVolume::readData(core::Call& call) {

    // fill material container
    this->processMaterial();

    // get transformation parameter
    //this->processTransformation();

    // fill clipping plane container
    this->processClippingPlane();

    // read Data, calculate  shape parameters, fill data vectors
    auto os = dynamic_cast<CallOSPRayStructure*>(&call);
    auto cd = this->getDataSlot.CallAs<geocalls::VolumetricDataCall>();
    auto const cgtf = this->getTFSlot.CallAs<core::view::CallGetTransferFunction>();

    this->structureContainer.dataChanged = false;
    if (cd == nullptr)
        return false;
    if (cgtf == nullptr) {
        core::utility::log::Log::DefaultLog.WriteError("[OSPRayStructuredVolume] No transferfunction connected.");
        return false;
    }

    uint32_t requested_frame = os->getTime();
    if (requested_frame >= cd->FrameCount()) {
        requested_frame = cd->FrameCount() - 1;
    }
    do {
        cd->SetFrameID(requested_frame, true);
        if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_EXTENTS))
            return false;
        if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_METADATA))
            return false;
        if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_DATA))
            return false;
    } while (cd->FrameID() != requested_frame);

    // do the callback to set the dirty flag
    if (!(*cgtf)(0))
        return false;
    auto interface_diry = this->InterfaceIsDirty();
    auto tf_dirty = cgtf->IsDirty();
    if (this->datahash != cd->DataHash() || this->time != os->getTime() || interface_diry || tf_dirty) {
        this->datahash = cd->DataHash();
        this->time = os->getTime();
        this->structureContainer.dataChanged = true;
    } else {
        return true;
    }

    auto const metadata = cd->GetMetadata();

    if (metadata->GridType != geocalls::CARTESIAN) {
        core::utility::log::Log::DefaultLog.WriteError(
            "OSPRayStructuredVolume only works with cartesian grids (for now)");
        return false;
    }

    unsigned int const voxelCount = metadata->Resolution[0] * metadata->Resolution[1] * metadata->Resolution[2];
    std::array<float, 3> gridOrigin = {metadata->Origin[0], metadata->Origin[1], metadata->Origin[2]};
    std::array<float, 3> gridSpacing = {
        metadata->SliceDists[0][0], metadata->SliceDists[1][0], metadata->SliceDists[2][0]};
    std::array<int, 3> dimensions = {static_cast<int>(metadata->Resolution[0]),
        static_cast<int>(metadata->Resolution[1]),
        static_cast<int>(metadata->Resolution[2])}; //< TODO HAZARD explicit narrowing

    unsigned int const maxDim =
        std::max<size_t>(metadata->Resolution[0], std::max<size_t>(metadata->Resolution[1], metadata->Resolution[2]));

    voxelDataType voxelType = {};

    switch (metadata->ScalarType) {
    case geocalls::FLOATING_POINT:
        if (metadata->ScalarLength == 4) {
            voxelType = voxelDataType::FLOAT;
        } else {
            voxelType = voxelDataType::DOUBLE;
        }
        break;
    case geocalls::UNSIGNED_INTEGER:
        if (metadata->ScalarLength == 1) {
            voxelType = voxelDataType::UCHAR;
        } else if (metadata->ScalarLength == 2) {
            voxelType = voxelDataType::USHORT;
        } else {
            core::utility::log::Log::DefaultLog.WriteError(
                "Unsigned integers with a length greater than 2 are invalid.");
            return false;
        }
        break;
    case geocalls::SIGNED_INTEGER:
        if (metadata->ScalarLength == 2) {
            voxelType = voxelDataType::SHORT;
        } else {
            core::utility::log::Log::DefaultLog.WriteError("Integers with a length != 2 are invalid.");
            return false;
        }
        break;
    case geocalls::BITS:
        core::utility::log::Log::DefaultLog.WriteError("Invalid datatype.");
        return false;
        break;
    }

    // get color transfer function
    std::vector<float> rgb;
    std::vector<float> a;

    if ((*cgtf)(0)) {
        if (cgtf->TFTextureFormat() == core::view::CallGetTransferFunction::TextureFormat::TEXTURE_FORMAT_RGBA) {
            auto const numColors = cgtf->TextureSize();
            rgb.resize(3 * numColors);
            a.resize(numColors);
            auto const texture = cgtf->GetTextureData();

            for (unsigned int i = 0; i < numColors; ++i) {
                rgb[i * 3 + 0] = texture[i * 4 + 0];
                rgb[i * 3 + 1] = texture[i * 4 + 1];
                rgb[i * 3 + 2] = texture[i * 4 + 2];
                a[i] = texture[i * 4 + 3];
            }
        } else {
            auto const numColors = cgtf->TextureSize();
            rgb.resize(3 * numColors);
            a.resize(numColors);
            auto const texture = cgtf->GetTextureData();

            for (unsigned int i = 0; i < numColors; ++i) {
                rgb[i * 3 + 0] = texture[i * 4 + 0];
                rgb[i * 3 + 1] = texture[i * 4 + 1];
                rgb[i * 3 + 2] = texture[i * 4 + 2];
                a[i] = i / (numColors - 1.0f);
            }
            core::utility::log::Log::DefaultLog.WriteWarn(
                "OSPRayStructuredVolume: No alpha channel in transfer function "
                "connected to module. Adding alpha ramp to RGB colors.\n");
        }
    } else {
        core::utility::log::Log::DefaultLog.WriteError(
            "OSPRayStructuredVolume: No transfer function connected to module");
        return false;
    }
    cgtf->ResetDirty();

    // Write stuff into the structureContainer

    this->structureContainer.type = structureTypeEnum::VOLUME;
    this->structureContainer.volumeType = volumeTypeEnum::STRUCTUREDVOLUME;
    structuredVolumeStructure svs;

    svs.volRepType = (volumeRepresentationType)this->repType.Param<core::param::EnumParam>()->Value();
    svs.voxels = cd->GetData();
    svs.gridOrigin = gridOrigin;
    svs.gridSpacing = gridSpacing;
    svs.dimensions = dimensions;
    svs.voxelCount = voxelCount;
    svs.maxDim = maxDim;
    svs.valueRange = {static_cast<float>(metadata->MinValues[0]), static_cast<float>(metadata->MaxValues[0])};
    svs.tfRGB = std::make_shared<std::vector<float>>(std::move(rgb));
    svs.tfA = std::make_shared<std::vector<float>>(std::move(a));
    svs.voxelDType = voxelType;

    svs.clippingBoxActive = this->clippingBoxActive.Param<core::param::BoolParam>()->Value();
    std::array<float, 3> cbl = {this->clippingBoxLower.Param<core::param::Vector3fParam>()->Value().GetX(),
        this->clippingBoxLower.Param<core::param::Vector3fParam>()->Value().GetY(),
        this->clippingBoxLower.Param<core::param::Vector3fParam>()->Value().GetZ()};
    svs.clippingBoxLower = cbl;
    std::array<float, 3> cbu = {this->clippingBoxUpper.Param<core::param::Vector3fParam>()->Value().GetX(),
        this->clippingBoxUpper.Param<core::param::Vector3fParam>()->Value().GetY(),
        this->clippingBoxUpper.Param<core::param::Vector3fParam>()->Value().GetZ()};
    svs.clippingBoxUpper = cbu;

    svs.isoValue = this->IsoValue.Param<core::param::FloatParam>()->Value();

    this->structureContainer.structure = svs;

    return true;
}

OSPRayStructuredVolume::~OSPRayStructuredVolume() {
    this->Release();
}

bool OSPRayStructuredVolume::create() {
    return true;
}

void OSPRayStructuredVolume::release() {}

/*
ospray::OSPRaySphereGeometry::InterfaceIsDirty()
*/
bool OSPRayStructuredVolume::InterfaceIsDirty() {
    if (this->clippingBoxActive.IsDirty() || this->clippingBoxLower.IsDirty() || this->clippingBoxUpper.IsDirty() ||
        this->IsoValue.IsDirty() || this->repType.IsDirty()) {
        this->clippingBoxActive.ResetDirty();
        this->clippingBoxLower.ResetDirty();
        this->clippingBoxUpper.ResetDirty();
        this->IsoValue.ResetDirty();
        this->repType.ResetDirty();
        return true;
    } else {
        return false;
    }
}

bool OSPRayStructuredVolume::getExtends(core::Call& call) {
    auto os = dynamic_cast<CallOSPRayStructure*>(&call);
    auto cd = this->getDataSlot.CallAs<geocalls::VolumetricDataCall>();

    if (cd == nullptr)
        return false;
    if (os->getTime() > cd->FrameCount()) {
        cd->SetFrameID(cd->FrameCount() - 1, true); // isTimeForced flag set to true
    } else {
        cd->SetFrameID(os->getTime(), true); // isTimeForced flag set to true
    }

    if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_EXTENTS))
        return false;
    if (!(*cd)(geocalls::VolumetricDataCall::IDX_GET_METADATA))
        return false;

    this->extendContainer.boundingBox = std::make_shared<core::BoundingBoxes_2>();
    this->extendContainer.boundingBox->SetBoundingBox(cd->AccessBoundingBoxes().ObjectSpaceBBox());
    std::string bbox_string = "LEFT: " + std::to_string(extendContainer.boundingBox->BoundingBox().Left()) +
                              ";\nBOTTOM: " + std::to_string(extendContainer.boundingBox->BoundingBox().Bottom()) +
                              ";\nBACK: " + std::to_string(extendContainer.boundingBox->BoundingBox().Back()) +
                              ";\nRIGHT: " + std::to_string(extendContainer.boundingBox->BoundingBox().Right()) +
                              ";\nTOP: " + std::to_string(extendContainer.boundingBox->BoundingBox().Top()) +
                              ";\nFRONT: " + std::to_string(extendContainer.boundingBox->BoundingBox().Bottom());
    this->showBoundingBox.Param<core::param::StringParam>()->SetValue(bbox_string.c_str());
    this->extendContainer.timeFramesCount = cd->FrameCount();
    this->extendContainer.isValid = true;

    return true;
}
<|endoftext|>


<commit_before>/*
 * This file is part of the µOS++ distribution.
 *   (https://github.com/micro-os-plus)
 * Copyright (c) 2017 Liviu Ionescu.
 *
 * 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 <micro-os-plus/board.h>
{% if trace != 'NONE' %}
#include <micro-os-plus/diag/trace.h>
{% endif %}

#include <sysclock.h>
{% if content == 'blinky' %}
#include <led.h>
{% endif %}

#include <stdint.h>
#include <stdio.h>
{% if language == 'c' %}
#include <stdbool.h>
{% endif %}

// ----------------------------------------------------------------------------
//
{% if content == 'blinky' %}
// This program blinks all LEDs on the SiFive {{ boardDescription }} board,
// at about 1 Hz. Pushing the button interrupts the automatic sequence,
// and each push will turn off and on the next LED. No special debouncing
// is implemented, so the results may be jumpy and erratic.
{% if boardName == 'hifive1' %}
// For the button to work, an external connection from WAKE to DIG2
// is required.
{% endif %}
{% elsif content == 'empty' %}
// This program counts seconds on the SiFive {{ boardDescription }} board.
{% endif %}
//
// ----------------------------------------------------------------------------

{% if content == 'blinky' %}
{% if language == 'cpp' %}
// Definitions visible only within this translation unit.
namespace
{
  // ----- Timing definitions -------------------------------------------------

  // Keep the LED on for 3/4 of a second.
  constexpr clock::duration_t BLINK_ON_TICKS = sysclock.frequency_hz * 3 / 4;
  constexpr clock::duration_t BLINK_OFF_TICKS = sysclock.frequency_hz
      - BLINK_ON_TICKS;
}
{% elsif language == 'c' %}
// ----- Timing definitions ---------------------------------------------------
// Keep the LED on for 3/4 of a second.
#define BLINK_ON_TICKS (OS_INTEGER_SYSCLOCK_FREQUENCY_HZ * 3 / 4)
#define BLINK_OFF_TICKS (OS_INTEGER_SYSCLOCK_FREQUENCY_HZ - BLINK_ON_TICKS)
{% endif %}

// ----- LED definitions ------------------------------------------------------

#define BLINK_PORT_NUMBER         (0)
#define BLINK_ACTIVE_LOW          (true)
#define BLINK_ACTIVE_HIGH         (false)

// Instantiate a static array of led objects.
{% if language == 'cpp' %}
led blink_leds[] =
  {
{% if boardName == 'hifive1' %}
    { BLINK_PORT_NUMBER, RED_LED_OFFSET, BLINK_ACTIVE_LOW },
    { BLINK_PORT_NUMBER, GREEN_LED_OFFSET, BLINK_ACTIVE_LOW },
    { BLINK_PORT_NUMBER, BLUE_LED_OFFSET, BLINK_ACTIVE_LOW },
{% else %}
    { BLINK_PORT_NUMBER, RED_LED_OFFSET, BLINK_ACTIVE_HIGH },
    { BLINK_PORT_NUMBER, GREEN_LED_OFFSET, BLINK_ACTIVE_HIGH },
    { BLINK_PORT_NUMBER, BLUE_LED_OFFSET, BLINK_ACTIVE_HIGH },
{% endif %}
  /**/
  };
{% elsif language == 'c' %}
led_t blink_leds[3];

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmissing-declarations"
#pragma GCC diagnostic ignored "-Wmissing-prototypes"

// Automatically called during startup, as for the C++ static
// constructors, before reaching `main()`.
void
__attribute__((constructor))
led_array_construct (void)
{
{% if boardName == 'hifive1' %}
  led_construct (&blink_leds[0], BLINK_PORT_NUMBER, RED_LED_OFFSET,
                 BLINK_ACTIVE_LOW);
  led_construct (&blink_leds[1], BLINK_PORT_NUMBER, GREEN_LED_OFFSET,
                 BLINK_ACTIVE_LOW);
  led_construct (&blink_leds[2], BLINK_PORT_NUMBER, BLUE_LED_OFFSET,
                 BLINK_ACTIVE_LOW);
{% else %}
  led_construct (&blink_leds[0], BLINK_PORT_NUMBER, RED_LED_OFFSET,
                 BLINK_ACTIVE_HIGH);
  led_construct (&blink_leds[1], BLINK_PORT_NUMBER, GREEN_LED_OFFSET,
                 BLINK_ACTIVE_HIGH);
  led_construct (&blink_leds[2], BLINK_PORT_NUMBER, BLUE_LED_OFFSET,
                 BLINK_ACTIVE_HIGH);
{% endif %}
}

#pragma GCC diagnostic pop
{% endif %}

// ----------------------------------------------------------------------------

bool button_pushed = false;
bool button_released = false;

{% endif %}
// ----------------------------------------------------------------------------

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wmissing-declarations"

int
main (int argc, char* argv[])
{
{% if trace != 'NONE' %}
  // Send a greeting to the trace device (skipped on Release).
{% if language == 'cpp' %}
  os::trace::puts ("Hello RISC-V World!");
{% elsif language == 'c' %}
  trace_puts ("Hello RISC-V World!");
{% endif %}

{% endif %}
{% if syscalls != 'none' %}
  // Send a message to the standard output.
  puts ("Standard output message.");

  // Send a message to the standard error.
  fprintf (stderr, "Standard error message.\n");

{% endif %}
  // At this stage the system clock should have already been configured
  // at high speed.
{% if trace != 'NONE' %}
{% if language == 'cpp' %}
  os::trace::printf ("System clock: %u Hz\n", riscv::core::running_frequency_hz ());
{% elsif language == 'c' %}
  trace_printf ("System clock: %u Hz\n", riscv_core_get_running_frequency_hz ());
{% endif %}
{% endif %}

{% if content == 'blinky' %}
  // Power up all LEDs.
  for (size_t i = 0; i < (sizeof(blink_leds) / sizeof(blink_leds[0])); ++i)
    {
{% if language == 'cpp' %}
      blink_leds[i].power_up ();
{% elsif language == 'c' %}
      led_power_up (&blink_leds[i]);
{% endif %}
    }

{% endif %}
{% if trace != 'NONE' %}
  uint32_t seconds = 0;

{% endif %}
{% if content == 'blinky' %}
  // Turn on all LEDs.
  for (size_t i = 0; i < (sizeof(blink_leds) / sizeof(blink_leds[0])); ++i)
    {
{% if language == 'cpp' %}
      blink_leds[i].turn_on ();
{% elsif language == 'c' %}
      led_turn_on (&blink_leds[i]);
{% endif %}
    }

  // First interval is longer (one full second).
{% if language == 'cpp' %}
  sysclock.sleep_for (sysclock.frequency_hz);
{% elsif language == 'c' %}
  os_sysclock_sleep_for (os_sysclock_get_frequency_hz ());
{% endif %}

  // Turn off all LEDs.
  for (size_t i = 0; i < (sizeof(blink_leds) / sizeof(blink_leds[0])); ++i)
    {
{% if language == 'cpp' %}
      blink_leds[i].turn_off ();
{% elsif language == 'c' %}
      led_turn_off (&blink_leds[i]);
{% endif %}
    }

{% if language == 'cpp' %}
  sysclock.sleep_for (BLINK_OFF_TICKS);
{% if trace != 'NONE' %}

  ++seconds;
  os::trace::printf ("Second %u\n", seconds);
{% endif %}
{% elsif language == 'c' %}
  os_sysclock_sleep_for (BLINK_OFF_TICKS);
{% if trace != 'NONE' %}

  ++seconds;
  trace_printf ("Second %u\n", seconds);
{% endif %}
{% endif %}

{% endif %}
{% if content == 'blinky' %}
  size_t count = 0;
{% endif %}
  // Loop forever, one second at a time.
  while (true)
    {
{% if content == 'blinky' %}
      // Blink individual LEDs.
{% if language == 'cpp' %}
      blink_leds[count].turn_on ();
      sysclock.sleep_for (BLINK_ON_TICKS);

      blink_leds[count].turn_off ();
      if (button_pushed)
        {
          break; // Quit loop with LED turned off.
        }
      sysclock.sleep_for (BLINK_OFF_TICKS);

      if (button_pushed)
        {
          break; // Quit loop with LED turned off.
        }
{% if trace != 'NONE' %}

      ++seconds;
      os::trace::printf ("Second %u\n", seconds);
{% endif %}
{% elsif language == 'c' %}
      led_turn_on (&blink_leds[count]);
      os_sysclock_sleep_for (BLINK_ON_TICKS);

      led_turn_off (&blink_leds[count]);
      if (button_pushed)
        {
          break; // Quit loop with LED turned off.
        }
      os_sysclock_sleep_for (BLINK_OFF_TICKS);

      if (button_pushed)
        {
          break; // Quit loop with LED turned off.
        }
{% if trace != 'NONE' %}

      ++seconds;
      trace_printf ("Second %u\n", seconds);
{% endif %}
{% endif %}

      ++count;
      if (count >= (sizeof(blink_leds) / sizeof(blink_leds[0])))
        {
          count = 0;
        }
{% elsif content == 'empty' %}
{% if language == 'cpp' %}
      sysclock.sleep_for (sysclock.frequency_hz);
{% if trace != 'NONE' %}

      ++seconds;
      os::trace::printf ("Second %u\n", seconds);
{% endif %}
{% elsif language == 'c' %}
      os_sysclock_sleep_for (os_sysclock_get_frequency_hz ());
{% if trace != 'NONE' %}

      ++seconds;
      trace_printf ("Second %u\n", seconds);
{% endif %}
{% endif %}
{% endif %}
    }
{% if content == 'blinky' %}

  while (true)
    {
      // Advance to next LED.
      ++count;
      if (count >= (sizeof(blink_leds) / sizeof(blink_leds[0])))
        {
          count = 0;
        }

      // Wait for button to be released.
      while (!button_released)
        {
{% if language == 'cpp' %}
          riscv::arch::wfi ();
{% elsif language == 'c' %}
          riscv_arch_wfi ();
{% endif %}
        }

{% if language == 'cpp' %}
      blink_leds[count].turn_on ();
      sysclock.sleep_for (BLINK_ON_TICKS);
{% elsif language == 'c' %}
      led_turn_on (&blink_leds[count]);
      os_sysclock_sleep_for (BLINK_ON_TICKS);
{% endif %}
      button_released = false;

      // Wait for button to be pushed.
      while (!button_pushed)
        {
{% if language == 'cpp' %}
          riscv::arch::wfi ();
{% elsif language == 'c' %}
          riscv_arch_wfi ();
{% endif %}
        }

{% if language == 'cpp' %}
      blink_leds[count].turn_off ();
{% elsif language == 'c' %}
      led_turn_off (&blink_leds[count]);
{% endif %}
      button_pushed = false;
    }
{% endif %}
}

#pragma GCC diagnostic pop

// ----------------------------------------------------------------------------

<commit_msg>templates.sifive: main-liquid.c.cpp cosmetics<commit_after>/*
 * This file is part of the µOS++ distribution.
 *   (https://github.com/micro-os-plus)
 * Copyright (c) 2017 Liviu Ionescu.
 *
 * 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 <micro-os-plus/board.h>
{% if trace != 'NONE' %}
#include <micro-os-plus/diag/trace.h>
{% endif %}

#include <sysclock.h>
{% if content == 'blinky' %}
#include <led.h>
{% endif %}

#include <stdint.h>
#include <stdio.h>
{% if language == 'c' %}
#include <stdbool.h>
{% endif %}

// ----------------------------------------------------------------------------
//
{% if content == 'blinky' %}
// This program blinks all LEDs on the SiFive {{ boardDescription }} board,
// at about 1 Hz. Pushing the button interrupts the automatic sequence,
// and each push will turn off and on the next LED. No special debouncing
// is implemented, so the results may be jumpy and erratic.
{% if boardName == 'hifive1' %}
// For the button to work, an external connection from WAKE to DIG2
// is required.
{% endif %}
{% elsif content == 'empty' %}
// This program counts seconds on the SiFive {{ boardDescription }} board.
{% endif %}
//
// ----------------------------------------------------------------------------

{% if content == 'blinky' %}
{% if language == 'cpp' %}
// Definitions visible only within this translation unit.
namespace
{
  // ----- Timing definitions -------------------------------------------------

  // Keep the LED on for 3/4 of a second.
  constexpr clock::duration_t BLINK_ON_TICKS = sysclock.frequency_hz * 3 / 4;
  constexpr clock::duration_t BLINK_OFF_TICKS = sysclock.frequency_hz
      - BLINK_ON_TICKS;
}
{% elsif language == 'c' %}
// ----- Timing definitions ---------------------------------------------------
// Keep the LED on for 3/4 of a second.
#define BLINK_ON_TICKS (OS_INTEGER_SYSCLOCK_FREQUENCY_HZ * 3 / 4)
#define BLINK_OFF_TICKS (OS_INTEGER_SYSCLOCK_FREQUENCY_HZ - BLINK_ON_TICKS)
{% endif %}

// ----- LED definitions ------------------------------------------------------

#define BLINK_PORT_NUMBER         (0)
#define BLINK_ACTIVE_LOW          (true)
#define BLINK_ACTIVE_HIGH         (false)

// Instantiate a static array of led objects.
{% if language == 'cpp' %}
led blink_leds[] =
  {
{% if boardName == 'hifive1' %}
    { BLINK_PORT_NUMBER, RED_LED_OFFSET, BLINK_ACTIVE_LOW },
    { BLINK_PORT_NUMBER, GREEN_LED_OFFSET, BLINK_ACTIVE_LOW },
    { BLINK_PORT_NUMBER, BLUE_LED_OFFSET, BLINK_ACTIVE_LOW },
{% else %}
    { BLINK_PORT_NUMBER, RED_LED_OFFSET, BLINK_ACTIVE_HIGH },
    { BLINK_PORT_NUMBER, GREEN_LED_OFFSET, BLINK_ACTIVE_HIGH },
    { BLINK_PORT_NUMBER, BLUE_LED_OFFSET, BLINK_ACTIVE_HIGH },
{% endif %}
  /**/
  };
{% elsif language == 'c' %}
led_t blink_leds[3];

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmissing-declarations"
#pragma GCC diagnostic ignored "-Wmissing-prototypes"

// Automatically called during startup, as for the C++ static
// constructors, before reaching `main()`.
void
__attribute__((constructor))
led_array_construct (void)
{
{% if boardName == 'hifive1' %}
  led_construct (&blink_leds[0], BLINK_PORT_NUMBER, RED_LED_OFFSET,
                 BLINK_ACTIVE_LOW);
  led_construct (&blink_leds[1], BLINK_PORT_NUMBER, GREEN_LED_OFFSET,
                 BLINK_ACTIVE_LOW);
  led_construct (&blink_leds[2], BLINK_PORT_NUMBER, BLUE_LED_OFFSET,
                 BLINK_ACTIVE_LOW);
{% else %}
  led_construct (&blink_leds[0], BLINK_PORT_NUMBER, RED_LED_OFFSET,
                 BLINK_ACTIVE_HIGH);
  led_construct (&blink_leds[1], BLINK_PORT_NUMBER, GREEN_LED_OFFSET,
                 BLINK_ACTIVE_HIGH);
  led_construct (&blink_leds[2], BLINK_PORT_NUMBER, BLUE_LED_OFFSET,
                 BLINK_ACTIVE_HIGH);
{% endif %}
}

#pragma GCC diagnostic pop
{% endif %}

// ----------------------------------------------------------------------------

bool button_pushed = false;
bool button_released = false;

{% endif %}
// ----------------------------------------------------------------------------

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wmissing-declarations"

int
main (int argc, char* argv[])
{
{% if trace != 'NONE' %}
  // Send a greeting to the trace device (skipped on Release).
{% if language == 'cpp' %}
  os::trace::puts ("Hello RISC-V World!");
{% elsif language == 'c' %}
  trace_puts ("Hello RISC-V World!");
{% endif %}

{% endif %}
{% if syscalls != 'none' %}
  // Send a message to the standard output.
  puts ("Standard output message.");

  // Send a message to the standard error.
  fprintf (stderr, "Standard error message.\n");

{% endif %}
  // At this stage the system clock should have already been configured
  // at high speed.
{% if trace != 'NONE' %}
{% if language == 'cpp' %}
  os::trace::printf ("System clock: %u Hz\n",
                     riscv::core::running_frequency_hz ());
{% elsif language == 'c' %}
  trace_printf ("System clock: %u Hz\n",
                riscv_core_get_running_frequency_hz ());
{% endif %}
{% endif %}

{% if content == 'blinky' %}
  // Power up all LEDs.
  for (size_t i = 0; i < (sizeof(blink_leds) / sizeof(blink_leds[0])); ++i)
    {
{% if language == 'cpp' %}
      blink_leds[i].power_up ();
{% elsif language == 'c' %}
      led_power_up (&blink_leds[i]);
{% endif %}
    }

{% endif %}
{% if trace != 'NONE' %}
  uint32_t seconds = 0;

{% endif %}
{% if content == 'blinky' %}
  // Turn on all LEDs.
  for (size_t i = 0; i < (sizeof(blink_leds) / sizeof(blink_leds[0])); ++i)
    {
{% if language == 'cpp' %}
      blink_leds[i].turn_on ();
{% elsif language == 'c' %}
      led_turn_on (&blink_leds[i]);
{% endif %}
    }

  // First interval is longer (one full second).
{% if language == 'cpp' %}
  sysclock.sleep_for (sysclock.frequency_hz);
{% elsif language == 'c' %}
  os_sysclock_sleep_for (os_sysclock_get_frequency_hz ());
{% endif %}

  // Turn off all LEDs.
  for (size_t i = 0; i < (sizeof(blink_leds) / sizeof(blink_leds[0])); ++i)
    {
{% if language == 'cpp' %}
      blink_leds[i].turn_off ();
{% elsif language == 'c' %}
      led_turn_off (&blink_leds[i]);
{% endif %}
    }

{% if language == 'cpp' %}
  sysclock.sleep_for (BLINK_OFF_TICKS);
{% if trace != 'NONE' %}

  ++seconds;
  os::trace::printf ("Second %u\n", seconds);
{% endif %}
{% elsif language == 'c' %}
  os_sysclock_sleep_for (BLINK_OFF_TICKS);
{% if trace != 'NONE' %}

  ++seconds;
  trace_printf ("Second %u\n", seconds);
{% endif %}
{% endif %}

{% endif %}
{% if content == 'blinky' %}
  size_t count = 0;
{% endif %}
  // Loop forever, one second at a time.
  while (true)
    {
{% if content == 'blinky' %}
      // Blink individual LEDs.
{% if language == 'cpp' %}
      blink_leds[count].turn_on ();
      sysclock.sleep_for (BLINK_ON_TICKS);

      blink_leds[count].turn_off ();
      if (button_pushed)
        {
          break; // Quit loop with LED turned off.
        }
      sysclock.sleep_for (BLINK_OFF_TICKS);

      if (button_pushed)
        {
          break; // Quit loop with LED turned off.
        }
{% if trace != 'NONE' %}

      ++seconds;
      os::trace::printf ("Second %u\n", seconds);
{% endif %}
{% elsif language == 'c' %}
      led_turn_on (&blink_leds[count]);
      os_sysclock_sleep_for (BLINK_ON_TICKS);

      led_turn_off (&blink_leds[count]);
      if (button_pushed)
        {
          break; // Quit loop with LED turned off.
        }
      os_sysclock_sleep_for (BLINK_OFF_TICKS);

      if (button_pushed)
        {
          break; // Quit loop with LED turned off.
        }
{% if trace != 'NONE' %}

      ++seconds;
      trace_printf ("Second %u\n", seconds);
{% endif %}
{% endif %}

      ++count;
      if (count >= (sizeof(blink_leds) / sizeof(blink_leds[0])))
        {
          count = 0;
        }
{% elsif content == 'empty' %}
{% if language == 'cpp' %}
      sysclock.sleep_for (sysclock.frequency_hz);
{% if trace != 'NONE' %}

      ++seconds;
      os::trace::printf ("Second %u\n", seconds);
{% endif %}
{% elsif language == 'c' %}
      os_sysclock_sleep_for (os_sysclock_get_frequency_hz ());
{% if trace != 'NONE' %}

      ++seconds;
      trace_printf ("Second %u\n", seconds);
{% endif %}
{% endif %}
{% endif %}
    }
{% if content == 'blinky' %}

  while (true)
    {
      // Advance to next LED.
      ++count;
      if (count >= (sizeof(blink_leds) / sizeof(blink_leds[0])))
        {
          count = 0;
        }

      // Wait for button to be released.
      while (!button_released)
        {
{% if language == 'cpp' %}
          riscv::arch::wfi ();
{% elsif language == 'c' %}
          riscv_arch_wfi ();
{% endif %}
        }

{% if language == 'cpp' %}
      blink_leds[count].turn_on ();
      sysclock.sleep_for (BLINK_ON_TICKS);
{% elsif language == 'c' %}
      led_turn_on (&blink_leds[count]);
      os_sysclock_sleep_for (BLINK_ON_TICKS);
{% endif %}
      button_released = false;

      // Wait for button to be pushed.
      while (!button_pushed)
        {
{% if language == 'cpp' %}
          riscv::arch::wfi ();
{% elsif language == 'c' %}
          riscv_arch_wfi ();
{% endif %}
        }

{% if language == 'cpp' %}
      blink_leds[count].turn_off ();
{% elsif language == 'c' %}
      led_turn_off (&blink_leds[count]);
{% endif %}
      button_pushed = false;
    }
{% endif %}
}

#pragma GCC diagnostic pop

// ----------------------------------------------------------------------------

<|endoftext|>


<commit_before>/*=====================================================================

 QGroundControl Open Source Ground Control Station

 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

 This file is part of the QGROUNDCONTROL project

 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

 ======================================================================*/

#include "MissionControllerManagerTest.h"
#include "LinkManager.h"
#include "MultiVehicleManager.h"

UT_REGISTER_TEST(MissionControllerManagerTest)

MissionControllerManagerTest::MissionControllerManagerTest(void)
    : _mockLink(NULL)
{

}

void MissionControllerManagerTest::cleanup(void)
{
    delete _multiSpyMissionManager;
    _multiSpyMissionManager = NULL;

    LinkManager::instance()->disconnectLink(_mockLink);
    _mockLink = NULL;
    QTest::qWait(1000); // Need to allow signals to move between threads

    UnitTest::cleanup();
}

void MissionControllerManagerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType)
{
    LinkManager* linkMgr = LinkManager::instance();
    Q_CHECK_PTR(linkMgr);

    _mockLink = new MockLink();
    Q_CHECK_PTR(_mockLink);
    _mockLink->setFirmwareType(firmwareType);
    LinkManager::instance()->_addLink(_mockLink);

    linkMgr->connectLink(_mockLink);

    // Wait for the Vehicle to work it's way through the various threads

    QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(activeVehicleChanged(Vehicle*)));
    QCOMPARE(spyVehicle.wait(5000), true);

    // Wait for the Mission Manager to finish it's initial load

    _missionManager = MultiVehicleManager::instance()->activeVehicle()->missionManager();
    QVERIFY(_missionManager);

    _rgMissionManagerSignals[canEditChangedSignalIndex] =             SIGNAL(canEditChanged(bool));
    _rgMissionManagerSignals[newMissionItemsAvailableSignalIndex] =   SIGNAL(newMissionItemsAvailable(void));
    _rgMissionManagerSignals[inProgressChangedSignalIndex] =          SIGNAL(inProgressChanged(bool));
    _rgMissionManagerSignals[errorSignalIndex] =                      SIGNAL(error(int, const QString&));

    _multiSpyMissionManager = new MultiSignalSpy();
    Q_CHECK_PTR(_multiSpyMissionManager);
    QCOMPARE(_multiSpyMissionManager->init(_missionManager, _rgMissionManagerSignals, _cMissionManagerSignals), true);

    if (_missionManager->inProgress()) {
        _multiSpyMissionManager->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, _missionManagerSignalWaitTime);
        _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
        QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
        QCOMPARE(_multiSpyMissionManager->checkNoSignalByMask(canEditChangedSignalIndex), true);
    }

    QVERIFY(!_missionManager->inProgress());
    QCOMPARE(_missionManager->missionItems()->count(), 0);
    _multiSpyMissionManager->clearAllSignals();
}

/// Checks the state of the inProgress value and signal to match the specified value
void MissionControllerManagerTest::_checkInProgressValues(bool inProgress)
{
    QCOMPARE(_missionManager->inProgress(), inProgress);
    QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(inProgressChangedSignalIndex);
    QList<QVariant> signalArgs = spy->takeFirst();
    QCOMPARE(signalArgs.count(), 1);
    QCOMPARE(signalArgs[0].toBool(), inProgress);
}
<commit_msg>Remove incorrect test declaration<commit_after>/*=====================================================================

 QGroundControl Open Source Ground Control Station

 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

 This file is part of the QGROUNDCONTROL project

 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

 ======================================================================*/

#include "MissionControllerManagerTest.h"
#include "LinkManager.h"
#include "MultiVehicleManager.h"

MissionControllerManagerTest::MissionControllerManagerTest(void)
    : _mockLink(NULL)
{

}

void MissionControllerManagerTest::cleanup(void)
{
    delete _multiSpyMissionManager;
    _multiSpyMissionManager = NULL;

    LinkManager::instance()->disconnectLink(_mockLink);
    _mockLink = NULL;
    QTest::qWait(1000); // Need to allow signals to move between threads

    UnitTest::cleanup();
}

void MissionControllerManagerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType)
{
    LinkManager* linkMgr = LinkManager::instance();
    Q_CHECK_PTR(linkMgr);

    _mockLink = new MockLink();
    Q_CHECK_PTR(_mockLink);
    _mockLink->setFirmwareType(firmwareType);
    LinkManager::instance()->_addLink(_mockLink);

    linkMgr->connectLink(_mockLink);

    // Wait for the Vehicle to work it's way through the various threads

    QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(activeVehicleChanged(Vehicle*)));
    QCOMPARE(spyVehicle.wait(5000), true);

    // Wait for the Mission Manager to finish it's initial load

    _missionManager = MultiVehicleManager::instance()->activeVehicle()->missionManager();
    QVERIFY(_missionManager);

    _rgMissionManagerSignals[canEditChangedSignalIndex] =             SIGNAL(canEditChanged(bool));
    _rgMissionManagerSignals[newMissionItemsAvailableSignalIndex] =   SIGNAL(newMissionItemsAvailable(void));
    _rgMissionManagerSignals[inProgressChangedSignalIndex] =          SIGNAL(inProgressChanged(bool));
    _rgMissionManagerSignals[errorSignalIndex] =                      SIGNAL(error(int, const QString&));

    _multiSpyMissionManager = new MultiSignalSpy();
    Q_CHECK_PTR(_multiSpyMissionManager);
    QCOMPARE(_multiSpyMissionManager->init(_missionManager, _rgMissionManagerSignals, _cMissionManagerSignals), true);

    if (_missionManager->inProgress()) {
        _multiSpyMissionManager->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, _missionManagerSignalWaitTime);
        _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
        QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
        QCOMPARE(_multiSpyMissionManager->checkNoSignalByMask(canEditChangedSignalIndex), true);
    }

    QVERIFY(!_missionManager->inProgress());
    QCOMPARE(_missionManager->missionItems()->count(), 0);
    _multiSpyMissionManager->clearAllSignals();
}

/// Checks the state of the inProgress value and signal to match the specified value
void MissionControllerManagerTest::_checkInProgressValues(bool inProgress)
{
    QCOMPARE(_missionManager->inProgress(), inProgress);
    QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(inProgressChangedSignalIndex);
    QList<QVariant> signalArgs = spy->takeFirst();
    QCOMPARE(signalArgs.count(), 1);
    QCOMPARE(signalArgs[0].toBool(), inProgress);
}
<|endoftext|>


<commit_before>#include "settingswidget.h"
#include "ui_settingswidget.h"
#include "settings.h"

SettingsWidget::SettingsWidget(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::SettingsWidget)
{
    ui->setupUi(this);
    connect(ui->okButton, SIGNAL(clicked()), this, SLOT(okClicked()));
    connect(ui->closeButton, SIGNAL(clicked()), this, SLOT(cancelClicked()));

    // Setup values
    ui->colorVarSpin->setValue(N_COLOR_VAR);
    ui->nPointsSpin->setValue(N_POLY_POINTS);
    ui->focusLeft->setValue(FOCUS_LEFT);
    ui->focusRight->setValue(FOCUS_RIGHT);
    ui->focusTop->setValue(FOCUS_TOP);
    ui->focusBottom->setValue(FOCUS_BOTTOM);
}

SettingsWidget::~SettingsWidget()
{
    delete ui;
}

void SettingsWidget::cancelClicked()
{
    close();
}

void SettingsWidget::okClicked()
{
    N_POLY_POINTS = ui->nPointsSpin->value();
    N_COLOR_VAR = ui->colorVarSpin->value();
    FOCUS_LEFT = ui->focusLeft->value();
    FOCUS_RIGHT = ui->focusRight->value();
    FOCUS_TOP = ui->focusTop->value();
    FOCUS_BOTTOM = ui->focusBottom->value();
    close();
}
<commit_msg>Pevent bad settings for Focus<commit_after>#include "settingswidget.h"
#include "ui_settingswidget.h"
#include "settings.h"
#include <QMessageBox>

SettingsWidget::SettingsWidget(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::SettingsWidget)
{
    ui->setupUi(this);
    connect(ui->okButton, SIGNAL(clicked()), this, SLOT(okClicked()));
    connect(ui->closeButton, SIGNAL(clicked()), this, SLOT(cancelClicked()));

    // Setup values
    ui->colorVarSpin->setValue(N_COLOR_VAR);
    ui->nPointsSpin->setValue(N_POLY_POINTS);
    ui->focusLeft->setValue(FOCUS_LEFT);
    ui->focusRight->setValue(FOCUS_RIGHT);
    ui->focusTop->setValue(FOCUS_TOP);
    ui->focusBottom->setValue(FOCUS_BOTTOM);
}

SettingsWidget::~SettingsWidget()
{
    delete ui;
}

void SettingsWidget::cancelClicked()
{
    close();
}

void SettingsWidget::okClicked()
{
    if (ui->focusLeft->value() >= ui->focusRight->value()
        || ui->focusTop->value() >= ui->focusBottom->value())
    {
        QMessageBox::critical(this, "Error", "Invalid Focus settings");
        close();
        return;
    }

    N_POLY_POINTS = ui->nPointsSpin->value();
    N_COLOR_VAR = ui->colorVarSpin->value();
    FOCUS_LEFT = ui->focusLeft->value();
    FOCUS_RIGHT = ui->focusRight->value();
    FOCUS_TOP = ui->focusTop->value();
    FOCUS_BOTTOM = ui->focusBottom->value();
    close();
}
<|endoftext|>


<commit_before>//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.

#include <cassert>
#include <vector>
#include <set>

#include "llvm/IR/DataLayout.h"
#include "llvm/IR/BasicBlock.h"
#include "llvm/IR/Constants.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/GlobalVariable.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/IntrinsicInst.h"
#include "llvm/IR/Module.h"
#include "llvm/Pass.h"
#include "llvm/IR/Type.h"
#include "llvm/IR/TypeBuilder.h"
#if LLVM_VERSION_MAJOR >= 4 || (LLVM_VERSION_MAJOR == 3 && LLVM_VERSION_MINOR >= 5)
  #include "llvm/IR/InstIterator.h"
#else
  #include "llvm/Support/InstIterator.h"
#endif
#include "llvm/Support/raw_ostream.h"
#include "llvm/Transforms/Utils/BasicBlockUtils.h"
#include <llvm/IR/DebugInfoMetadata.h>

using namespace llvm;

namespace {

class MarkVolatile : public FunctionPass {
  public:
    static char ID;

    MarkVolatile() : FunctionPass(ID) {}

    virtual bool runOnFunction(Function &F);
};

bool MarkVolatile::runOnFunction(Function &F)
{
  bool modified = false;
  LLVMContext& ctx = F.getParent()->getContext();

  for (inst_iterator I = inst_begin(F), E = inst_end(F); I != E; ++I) {
    Instruction *ins = &*I;
    if (CallInst *CI = dyn_cast<CallInst>(ins)) {
      if (CI->isInlineAsm())
        continue;

      const Value *val = CI->getCalledValue()->stripPointerCasts();
      const Function *callee = dyn_cast<Function>(val);
      if (!callee || callee->isIntrinsic())
        continue;

      assert(callee->hasName());
      StringRef name = callee->getName();

      if (!name.startswith("__INSTR_mark_pointer"))
        continue;

      // we found a marked instruction, make it volatile
      // if it is store or load
      auto nextIt = I;
      ++nextIt;
      if (StoreInst *SI = dyn_cast<StoreInst>(&*nextIt)) {
          SI->setVolatile(true);
          modified = true;
      } else if (LoadInst *LI = dyn_cast<LoadInst>(&*nextIt)) {
          LI->setVolatile(true);
          modified = true;
      } else if (MemIntrinsic *MI = dyn_cast<MemIntrinsic>(&*nextIt)) {
          MI->setVolatile(ConstantInt::getTrue(ctx));
          modified = true;
      } else {
          errs() << "[Warning]: this marked instruction was not made volatile:\n";
          errs() << *nextIt << "\n";
      }
    }
  }
  return modified;
}

} // namespace

static RegisterPass<MarkVolatile> MVLTL("mark-volatile",
                                        "Make marked instructions as volatile");
char MarkVolatile::ID;

<commit_msg>mark-volatile: match all marker functions<commit_after>//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.

#include <cassert>
#include <vector>
#include <set>

#include "llvm/IR/DataLayout.h"
#include "llvm/IR/BasicBlock.h"
#include "llvm/IR/Constants.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/GlobalVariable.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/IntrinsicInst.h"
#include "llvm/IR/Module.h"
#include "llvm/Pass.h"
#include "llvm/IR/Type.h"
#include "llvm/IR/TypeBuilder.h"
#if LLVM_VERSION_MAJOR >= 4 || (LLVM_VERSION_MAJOR == 3 && LLVM_VERSION_MINOR >= 5)
  #include "llvm/IR/InstIterator.h"
#else
  #include "llvm/Support/InstIterator.h"
#endif
#include "llvm/Support/raw_ostream.h"
#include "llvm/Transforms/Utils/BasicBlockUtils.h"
#include <llvm/IR/DebugInfoMetadata.h>

using namespace llvm;

namespace {

class MarkVolatile : public FunctionPass {
  public:
    static char ID;

    MarkVolatile() : FunctionPass(ID) {}

    virtual bool runOnFunction(Function &F);
};

bool MarkVolatile::runOnFunction(Function &F)
{
  bool modified = false;
  LLVMContext& ctx = F.getParent()->getContext();

  for (inst_iterator I = inst_begin(F), E = inst_end(F); I != E; ++I) {
    Instruction *ins = &*I;
    if (CallInst *CI = dyn_cast<CallInst>(ins)) {
      if (CI->isInlineAsm())
        continue;

      const Value *val = CI->getCalledValue()->stripPointerCasts();
      const Function *callee = dyn_cast<Function>(val);
      if (!callee || callee->isIntrinsic())
        continue;

      assert(callee->hasName());
      StringRef name = callee->getName();

      if (!name.startswith("__INSTR_mark_"))
        continue;

      // we found a marked instruction, make it volatile
      // if it is store or load
      auto nextIt = I;
      ++nextIt;
      if (StoreInst *SI = dyn_cast<StoreInst>(&*nextIt)) {
          SI->setVolatile(true);
          modified = true;
      } else if (LoadInst *LI = dyn_cast<LoadInst>(&*nextIt)) {
          LI->setVolatile(true);
          modified = true;
      } else if (MemIntrinsic *MI = dyn_cast<MemIntrinsic>(&*nextIt)) {
          MI->setVolatile(ConstantInt::getTrue(ctx));
          modified = true;
      } else {
          errs() << "[Warning]: this marked instruction was not made volatile:\n";
          errs() << *nextIt << "\n";
      }
    }
  }
  return modified;
}

} // namespace

static RegisterPass<MarkVolatile> MVLTL("mark-volatile",
                                        "Make marked instructions as volatile");
char MarkVolatile::ID;

<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2015, Daiki Maekawa and Chiba Institute of Technology.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <ros/ros.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/MarkerArray.h>
#include <fulanghua_srvs/Pose.h>

#include <yaml-cpp/yaml.h>

#include <vector>
#include <fstream>
#include <string>
#include <exception>
#include <math.h>
#include <limits>

#ifdef NEW_YAMLCPP
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
    i = node.as<T>();
}
#endif

class SwitchRunningStatus : public std::exception {
public:
    SwitchRunningStatus() : std::exception() { }
};

class WaypointsNavigation{
public:
    WaypointsNavigation() :
        has_activate_(false),
        move_base_action_("move_base", true),
        rate_(10),
        last_moved_time_(0),
        dist_err_(0.8)
    {
        while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
        {
            ROS_INFO("Waiting...");
        }

        ros::NodeHandle private_nh("~");
        private_nh.param("robot_frame", robot_frame_, std::string("/base_link"));
        private_nh.param("world_frame", world_frame_, std::string("/map"));

        double max_update_rate;
        private_nh.param("max_update_rate", max_update_rate, 10.0);
        rate_ = ros::Rate(max_update_rate);
        std::string filename = "";
        private_nh.param("filename", filename, filename);
        if(filename != ""){
            ROS_INFO_STREAM("Read waypoints data from " << filename);
            if(!readFile(filename)) {
                ROS_ERROR("Failed loading waypoints file");
            }
            current_waypoint_ = waypoints_.begin();
        } else {
            ROS_ERROR("waypoints file doesn't have name");
        }

        private_nh.param("dist_err", dist_err_, dist_err_);

        ros::NodeHandle nh;
        start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this);
        suspend_server_ = nh.advertiseService("suspend_wp_pose", &WaypointsNavigation::suspendPoseCallback, this);
        resume_server_ = nh.advertiseService("resume_wp_pose", &WaypointsNavigation::resumePoseCallback, this);
        cmd_vel_sub_ = nh.subscribe("icart_mini/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
        marker_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 10);
        clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
    }

    bool startNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) {
        if(has_activate_) {
            response.success = false;
            return false;
        }

        std_srvs::Empty empty;
        while(!clear_costmaps_srv_.call(empty)) {
            ROS_WARN("Resend clear costmap service");
            sleep();
        }

        current_waypoint_ = waypoints_.begin();
        has_activate_ = true;
        response.success = true;
        return true;
    }

    bool resumePoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) {
        if(has_activate_) {
            response.status = false;
            return false;
        }

        std_srvs::Empty empty;
        clear_costmaps_srv_.call(empty);
        //move_base_action_.cancelAllGoals();

        ///< @todo calculating metric with request orientation
        double min_dist = std::numeric_limits<double>::max();
        for(std::vector<geometry_msgs::PointStamped>::iterator it = waypoints_.begin(); it != waypoints_.end(); it++) {
            double dist = hypot(it->point.x - request.pose.position.x, it->point.y - request.pose.position.y);
            if(dist < min_dist) {
                min_dist = dist;
                current_waypoint_ = it;
            }
        }

        response.status = true;
        has_activate_ = true;

        return true;
    }

    bool suspendPoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) {
        if(!has_activate_) {
            response.status = false;
            return false;
        }

        //move_base_action_.cancelAllGoals();
        startNavigationGL(request.pose);
        while(!navigationFinished() && ros::ok()) {
            sleep();
        }
        response.status = true;
        has_activate_ = false;

        return true;
    }

    void cmdVelCallback(const geometry_msgs::Twist &msg){
        if(msg.linear.x > -0.001 && msg.linear.x < 0.001   &&
           msg.linear.y > -0.001 && msg.linear.y < 0.001   &&
           msg.linear.z > -0.001 && msg.linear.z < 0.001   &&
           msg.angular.x > -0.001 && msg.angular.x < 0.001 &&
           msg.angular.y > -0.001 && msg.angular.y < 0.001 &&
           msg.angular.z > -0.001 && msg.angular.z < 0.001){

            ROS_INFO("command velocity all zero");
        }else{
            last_moved_time_ = ros::Time::now().toSec();
        }
    }

    bool readFile(const std::string &filename){
        waypoints_.clear();
        try{
            std::ifstream ifs(filename.c_str(), std::ifstream::in);
            if(ifs.good() == false){
                return false;
            }

            YAML::Node node;

            #ifdef NEW_YAMLCPP
                node = YAML::Load(ifs);
            #else
                YAML::Parser parser(ifs);
                parser.GetNextDocument(node);
            #endif

            #ifdef NEW_YAMLCPP
                const YAML::Node &wp_node_tmp = node["waypoints"];
                const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
            #else
                const YAML::Node *wp_node = node.FindValue("waypoints");
            #endif

            if(wp_node != NULL){
                for(int i=0; i < wp_node->size(); i++){
                    geometry_msgs::PointStamped point;

                    (*wp_node)[i]["point"]["x"] >> point.point.x;
                    (*wp_node)[i]["point"]["y"] >> point.point.y;
                    (*wp_node)[i]["point"]["z"] >> point.point.z;

                    waypoints_.push_back(point);

                }
            }else{
                return false;
            }

            #ifdef NEW_YAMLCPP
                const YAML::Node &fp_node_tmp = node["finish_pose"];
                const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL;
            #else
                const YAML::Node *fp_node = node.FindValue("finish_pose");
            #endif

            if(fp_node != NULL){
                (*fp_node)["pose"]["position"]["x"] >> finish_pose_.position.x;
                (*fp_node)["pose"]["position"]["y"] >> finish_pose_.position.y;
                (*fp_node)["pose"]["position"]["z"] >> finish_pose_.position.z;

                (*fp_node)["pose"]["orientation"]["x"] >> finish_pose_.orientation.x;
                (*fp_node)["pose"]["orientation"]["y"] >> finish_pose_.orientation.y;
                (*fp_node)["pose"]["orientation"]["z"] >> finish_pose_.orientation.z;
                (*fp_node)["pose"]["orientation"]["w"] >> finish_pose_.orientation.w;
            }else{
                return false;
            }

        }catch(YAML::ParserException &e){
            return false;

        }catch(YAML::RepresentationException &e){
            return false;
        }

        return true;
    }

    bool shouldSendGoal(){
        bool ret = true;
        actionlib::SimpleClientGoalState state = move_base_action_.getState();
        if((state != actionlib::SimpleClientGoalState::ACTIVE) &&
           (state != actionlib::SimpleClientGoalState::PENDING) &&
           (state != actionlib::SimpleClientGoalState::RECALLED) &&
           (state != actionlib::SimpleClientGoalState::PREEMPTED))
        {
            ret = false;
        }

        if(waypoints_.empty()){
            ret = false;
        }

        return ret;
    }

    bool navigationFinished(){
        return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
    }

    bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8){
        tf::StampedTransform robot_gl = getRobotPosGL();

        const double wx = dest.x;
        const double wy = dest.y;
        const double rx = robot_gl.getOrigin().x();
        const double ry = robot_gl.getOrigin().y();
        const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2));

        return dist < dist_err;
    }

    tf::StampedTransform getRobotPosGL(){
        tf::StampedTransform robot_gl;
        try{
            tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
        }catch(tf::TransformException &e){
            ROS_WARN_STREAM("tf::TransformException: " << e.what());
        }

        return robot_gl;
    }

    void sleep(){
        rate_.sleep();
        ros::spinOnce();
        publishMarkers();
    }

    void startNavigationGL(const geometry_msgs::Point &dest){
        geometry_msgs::Pose pose;
        pose.position = dest;
        pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
        startNavigationGL(pose);
    }

    void startNavigationGL(const geometry_msgs::Pose &dest){
        move_base_msgs::MoveBaseGoal move_base_goal;
        move_base_goal.target_pose.header.stamp = ros::Time::now();
        move_base_goal.target_pose.header.frame_id = world_frame_;
        move_base_goal.target_pose.pose.position = dest.position;
        move_base_goal.target_pose.pose.orientation = dest.orientation;

        move_base_action_.sendGoal(move_base_goal);
    }

    void publishMarkers(){
        visualization_msgs::MarkerArray markers_array;
        for(int i=0; i < waypoints_.size(); i++){
            visualization_msgs::Marker marker, label;
            marker.header.frame_id = world_frame_;
            marker.header.stamp = ros::Time::now();
            marker.scale.x = 0.2;
            marker.scale.y = 0.2;
            marker.scale.z = 0.2;
            marker.pose.position.z = marker.scale.z / 2.0;
            marker.color.r = 0.8f;
            marker.color.g = 0.2f;
            marker.color.b = 0.2f;

            std::stringstream name;
            name << "waypoint " << i;
            marker.ns = name.str();
            marker.id = i;
            marker.pose.position.x = waypoints_[i].point.x;
            marker.pose.position.y = waypoints_[i].point.y;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            marker.color.a = 1.0f;
            markers_array.markers.push_back(marker);

            //ROS_INFO_STREAM("waypoints \n" << waypoints_[i]);
        }
        marker_pub_.publish(markers_array);
    }

    void run(){
        while(ros::ok()){
            try {
                if(has_activate_) {
                    geometry_msgs::Pose goal_pose;
                    if(current_waypoint_ == waypoints_.end()-1) {
                        ROS_INFO("prepare finish pose");
                        double goal_direction = atan2(finish_pose_.position.y - current_waypoint_->point.y,
                                                      finish_pose_.position.x - current_waypoint_->point.x);

                        goal_pose.position = current_waypoint_->point;
                        goal_pose.orientation = tf::createQuaternionMsgFromYaw(goal_direction);
                    } else {
                        ROS_INFO("calculate waypoint direction");
                        double goal_direction = atan2((current_waypoint_+1)->point.y - current_waypoint_->point.y,
                                                      (current_waypoint_+1)->point.x - current_waypoint_->point.x);

                        ROS_INFO_STREAM("goal_direction = " << goal_direction);
                        ROS_INFO_STREAM("current_waypoint_+1 " << (current_waypoint_+1)->point.y);
                        ROS_INFO_STREAM("current_waypoint_" << current_waypoint_->point.y);

                        goal_pose.position = current_waypoint_->point;
                        goal_pose.orientation = tf::createQuaternionMsgFromYaw(goal_direction);
                    }

                    startNavigationGL(goal_pose);
                    double start_nav_time = ros::Time::now().toSec();
                    while(!onNavigationPoint(goal_pose.position, dist_err_)) {
                        if(!has_activate_)
                            throw SwitchRunningStatus();

                        double time = ros::Time::now().toSec();
                        if(time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
                            ROS_WARN("Resend the navigation goal.");
                            std_srvs::Empty empty;
                            clear_costmaps_srv_.call(empty);
                            startNavigationGL(goal_pose);
                            start_nav_time = time;
                        }
                        sleep();
                    }

                    if(current_waypoint_ != waypoints_.end()-1) {
                        current_waypoint_++;
                    } else {
                        startNavigationGL(finish_pose_);
                        while(!navigationFinished() && ros::ok()) sleep();
                        has_activate_ = false;
                    }
                }
            } catch(const SwitchRunningStatus &e) {
                ROS_INFO_STREAM("running status switched");
            }

            sleep();
        }
    }

private:
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
    std::vector<geometry_msgs::PointStamped> waypoints_;
    std::vector<geometry_msgs::PointStamped>::iterator current_waypoint_;
    geometry_msgs::Pose finish_pose_;
    bool has_activate_;
    std::string robot_frame_, world_frame_;
    tf::TransformListener tf_listener_;
    ros::Rate rate_;
    ros::ServiceServer start_server_, suspend_server_, resume_server_;
    ros::Subscriber cmd_vel_sub_;
    ros::Publisher marker_pub_;
    ros::ServiceClient clear_costmaps_srv_;
    double last_moved_time_, dist_err_;

};

int main(int argc, char *argv[]){
    ros::init(argc, argv, ROS_PACKAGE_NAME);
    WaypointsNavigation w_nav;
    w_nav.run();

    return 0;
}

<commit_msg>fix #63<commit_after>/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2015, Daiki Maekawa and Chiba Institute of Technology.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <ros/ros.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/MarkerArray.h>
#include <fulanghua_srvs/Pose.h>

#include <yaml-cpp/yaml.h>

#include <vector>
#include <fstream>
#include <string>
#include <exception>
#include <math.h>
#include <limits>

#ifdef NEW_YAMLCPP
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
    i = node.as<T>();
}
#endif

class SwitchRunningStatus : public std::exception {
public:
    SwitchRunningStatus() : std::exception() { }
};

class WaypointsNavigation{
public:
    WaypointsNavigation() :
        has_activate_(false),
        move_base_action_("move_base", true),
        rate_(10),
        last_moved_time_(0),
        dist_err_(0.8)
    {
        while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
        {
            ROS_INFO("Waiting...");
        }

        ros::NodeHandle private_nh("~");
        private_nh.param("robot_frame", robot_frame_, std::string("/base_link"));
        private_nh.param("world_frame", world_frame_, std::string("/map"));

        double max_update_rate;
        private_nh.param("max_update_rate", max_update_rate, 10.0);
        rate_ = ros::Rate(max_update_rate);
        std::string filename = "";
        private_nh.param("filename", filename, filename);
        if(filename != ""){
            ROS_INFO_STREAM("Read waypoints data from " << filename);
            if(!readFile(filename)) {
                ROS_ERROR("Failed loading waypoints file");
            }
            current_waypoint_ = waypoints_.begin();
        } else {
            ROS_ERROR("waypoints file doesn't have name");
        }

        private_nh.param("dist_err", dist_err_, dist_err_);

        ros::NodeHandle nh;
        start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this);
        suspend_server_ = nh.advertiseService("suspend_wp_pose", &WaypointsNavigation::suspendPoseCallback, this);
        resume_server_ = nh.advertiseService("resume_wp_pose", &WaypointsNavigation::resumePoseCallback, this);
        cmd_vel_sub_ = nh.subscribe("icart_mini/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
        marker_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 10);
        clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
    }

    bool startNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) {
        if(has_activate_) {
            response.success = false;
            return false;
        }

        std_srvs::Empty empty;
        while(!clear_costmaps_srv_.call(empty)) {
            ROS_WARN("Resend clear costmap service");
            sleep();
        }

        current_waypoint_ = waypoints_.begin();
        has_activate_ = true;
        response.success = true;
        return true;
    }

    bool resumePoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) {
        if(has_activate_) {
            response.status = false;
            return false;
        }

        std_srvs::Empty empty;
        clear_costmaps_srv_.call(empty);
        //move_base_action_.cancelAllGoals();

        ///< @todo calculating metric with request orientation
        double min_dist = std::numeric_limits<double>::max();
        for(std::vector<geometry_msgs::PointStamped>::iterator it = current_waypoint_; it != waypoints_.end(); it++) {
            double dist = hypot(it->point.x - request.pose.position.x, it->point.y - request.pose.position.y);
            if(dist < min_dist) {
                min_dist = dist;
                current_waypoint_ = it;
            }
        }

        response.status = true;
        has_activate_ = true;

        return true;
    }

    bool suspendPoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) {
        if(!has_activate_) {
            response.status = false;
            return false;
        }

        //move_base_action_.cancelAllGoals();
        startNavigationGL(request.pose);
        while(!navigationFinished() && ros::ok()) {
            sleep();
        }
        response.status = true;
        has_activate_ = false;

        return true;
    }

    void cmdVelCallback(const geometry_msgs::Twist &msg){
        if(msg.linear.x > -0.001 && msg.linear.x < 0.001   &&
           msg.linear.y > -0.001 && msg.linear.y < 0.001   &&
           msg.linear.z > -0.001 && msg.linear.z < 0.001   &&
           msg.angular.x > -0.001 && msg.angular.x < 0.001 &&
           msg.angular.y > -0.001 && msg.angular.y < 0.001 &&
           msg.angular.z > -0.001 && msg.angular.z < 0.001){

            ROS_INFO("command velocity all zero");
        }else{
            last_moved_time_ = ros::Time::now().toSec();
        }
    }

    bool readFile(const std::string &filename){
        waypoints_.clear();
        try{
            std::ifstream ifs(filename.c_str(), std::ifstream::in);
            if(ifs.good() == false){
                return false;
            }

            YAML::Node node;

            #ifdef NEW_YAMLCPP
                node = YAML::Load(ifs);
            #else
                YAML::Parser parser(ifs);
                parser.GetNextDocument(node);
            #endif

            #ifdef NEW_YAMLCPP
                const YAML::Node &wp_node_tmp = node["waypoints"];
                const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
            #else
                const YAML::Node *wp_node = node.FindValue("waypoints");
            #endif

            if(wp_node != NULL){
                for(int i=0; i < wp_node->size(); i++){
                    geometry_msgs::PointStamped point;

                    (*wp_node)[i]["point"]["x"] >> point.point.x;
                    (*wp_node)[i]["point"]["y"] >> point.point.y;
                    (*wp_node)[i]["point"]["z"] >> point.point.z;

                    waypoints_.push_back(point);

                }
            }else{
                return false;
            }

            #ifdef NEW_YAMLCPP
                const YAML::Node &fp_node_tmp = node["finish_pose"];
                const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL;
            #else
                const YAML::Node *fp_node = node.FindValue("finish_pose");
            #endif

            if(fp_node != NULL){
                (*fp_node)["pose"]["position"]["x"] >> finish_pose_.position.x;
                (*fp_node)["pose"]["position"]["y"] >> finish_pose_.position.y;
                (*fp_node)["pose"]["position"]["z"] >> finish_pose_.position.z;

                (*fp_node)["pose"]["orientation"]["x"] >> finish_pose_.orientation.x;
                (*fp_node)["pose"]["orientation"]["y"] >> finish_pose_.orientation.y;
                (*fp_node)["pose"]["orientation"]["z"] >> finish_pose_.orientation.z;
                (*fp_node)["pose"]["orientation"]["w"] >> finish_pose_.orientation.w;
            }else{
                return false;
            }

        }catch(YAML::ParserException &e){
            return false;

        }catch(YAML::RepresentationException &e){
            return false;
        }

        return true;
    }

    bool shouldSendGoal(){
        bool ret = true;
        actionlib::SimpleClientGoalState state = move_base_action_.getState();
        if((state != actionlib::SimpleClientGoalState::ACTIVE) &&
           (state != actionlib::SimpleClientGoalState::PENDING) &&
           (state != actionlib::SimpleClientGoalState::RECALLED) &&
           (state != actionlib::SimpleClientGoalState::PREEMPTED))
        {
            ret = false;
        }

        if(waypoints_.empty()){
            ret = false;
        }

        return ret;
    }

    bool navigationFinished(){
        return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
    }

    bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8){
        tf::StampedTransform robot_gl = getRobotPosGL();

        const double wx = dest.x;
        const double wy = dest.y;
        const double rx = robot_gl.getOrigin().x();
        const double ry = robot_gl.getOrigin().y();
        const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2));

        return dist < dist_err;
    }

    tf::StampedTransform getRobotPosGL(){
        tf::StampedTransform robot_gl;
        try{
            tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
        }catch(tf::TransformException &e){
            ROS_WARN_STREAM("tf::TransformException: " << e.what());
        }

        return robot_gl;
    }

    void sleep(){
        rate_.sleep();
        ros::spinOnce();
        publishMarkers();
    }

    void startNavigationGL(const geometry_msgs::Point &dest){
        geometry_msgs::Pose pose;
        pose.position = dest;
        pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
        startNavigationGL(pose);
    }

    void startNavigationGL(const geometry_msgs::Pose &dest){
        move_base_msgs::MoveBaseGoal move_base_goal;
        move_base_goal.target_pose.header.stamp = ros::Time::now();
        move_base_goal.target_pose.header.frame_id = world_frame_;
        move_base_goal.target_pose.pose.position = dest.position;
        move_base_goal.target_pose.pose.orientation = dest.orientation;

        move_base_action_.sendGoal(move_base_goal);
    }

    void publishMarkers(){
        visualization_msgs::MarkerArray markers_array;
        for(int i=0; i < waypoints_.size(); i++){
            visualization_msgs::Marker marker, label;
            marker.header.frame_id = world_frame_;
            marker.header.stamp = ros::Time::now();
            marker.scale.x = 0.2;
            marker.scale.y = 0.2;
            marker.scale.z = 0.2;
            marker.pose.position.z = marker.scale.z / 2.0;
            marker.color.r = 0.8f;
            marker.color.g = 0.2f;
            marker.color.b = 0.2f;

            std::stringstream name;
            name << "waypoint " << i;
            marker.ns = name.str();
            marker.id = i;
            marker.pose.position.x = waypoints_[i].point.x;
            marker.pose.position.y = waypoints_[i].point.y;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            marker.color.a = 1.0f;
            markers_array.markers.push_back(marker);

            //ROS_INFO_STREAM("waypoints \n" << waypoints_[i]);
        }
        marker_pub_.publish(markers_array);
    }

    void run(){
        while(ros::ok()){
            try {
                if(has_activate_) {
                    geometry_msgs::Pose goal_pose;
                    if(current_waypoint_ == waypoints_.end()-1) {
                        ROS_INFO("prepare finish pose");
                        double goal_direction = atan2(finish_pose_.position.y - current_waypoint_->point.y,
                                                      finish_pose_.position.x - current_waypoint_->point.x);

                        goal_pose.position = current_waypoint_->point;
                        goal_pose.orientation = tf::createQuaternionMsgFromYaw(goal_direction);
                    } else {
                        ROS_INFO("calculate waypoint direction");
                        double goal_direction = atan2((current_waypoint_+1)->point.y - current_waypoint_->point.y,
                                                      (current_waypoint_+1)->point.x - current_waypoint_->point.x);

                        ROS_INFO_STREAM("goal_direction = " << goal_direction);
                        ROS_INFO_STREAM("current_waypoint_+1 " << (current_waypoint_+1)->point.y);
                        ROS_INFO_STREAM("current_waypoint_" << current_waypoint_->point.y);

                        goal_pose.position = current_waypoint_->point;
                        goal_pose.orientation = tf::createQuaternionMsgFromYaw(goal_direction);
                    }

                    startNavigationGL(goal_pose);
                    double start_nav_time = ros::Time::now().toSec();
                    while(!onNavigationPoint(goal_pose.position, dist_err_)) {
                        if(!has_activate_)
                            throw SwitchRunningStatus();

                        double time = ros::Time::now().toSec();
                        if(time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
                            ROS_WARN("Resend the navigation goal.");
                            std_srvs::Empty empty;
                            clear_costmaps_srv_.call(empty);
                            startNavigationGL(goal_pose);
                            start_nav_time = time;
                        }
                        sleep();
                    }

                    if(current_waypoint_ != waypoints_.end()-1) {
                        current_waypoint_++;
                    } else {
                        startNavigationGL(finish_pose_);
                        while(!navigationFinished() && ros::ok()) sleep();
                        has_activate_ = false;
                    }
                }
            } catch(const SwitchRunningStatus &e) {
                ROS_INFO_STREAM("running status switched");
            }

            sleep();
        }
    }

private:
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
    std::vector<geometry_msgs::PointStamped> waypoints_;
    std::vector<geometry_msgs::PointStamped>::iterator current_waypoint_;
    geometry_msgs::Pose finish_pose_;
    bool has_activate_;
    std::string robot_frame_, world_frame_;
    tf::TransformListener tf_listener_;
    ros::Rate rate_;
    ros::ServiceServer start_server_, suspend_server_, resume_server_;
    ros::Subscriber cmd_vel_sub_;
    ros::Publisher marker_pub_;
    ros::ServiceClient clear_costmaps_srv_;
    double last_moved_time_, dist_err_;

};

int main(int argc, char *argv[]){
    ros::init(argc, argv, ROS_PACKAGE_NAME);
    WaypointsNavigation w_nav;
    w_nav.run();

    return 0;
}

<|endoftext|>


<commit_before>#include "Dialog.h"

Dialog::Dialog(LPCWSTR className, LPCWSTR dlgTemplate) :
Window(className) {
    _dlgHwnd = CreateDialogParam(
        Window::InstanceHandle(),
        dlgTemplate,
        Window::Handle(),
        StaticDialogProc,
        (LPARAM) this);
}

void Dialog::AddControl(Control *control) {
    if (control == nullptr) {
        return;
    }

    CLOG(L"Adding control id: %d", control->ID());
    int id = control->ID();
    _controlMap[id] = control;
}

HWND Dialog::DialogHandle() {
    return _dlgHwnd;
}

INT_PTR Dialog::StaticDialogProc(HWND hwndDlg, UINT uMsg,
        WPARAM wParam, LPARAM lParam) {
    Dialog *dlg;

    if (uMsg == WM_INITDIALOG) {
        dlg = (Dialog *) lParam;
        SetWindowLongPtr(hwndDlg, DWLP_USER, (LONG_PTR) dlg);
    } else {
        dlg = (Dialog *) GetWindowLongPtr(hwndDlg, DWLP_USER);
        if (!dlg) {
            return FALSE;
        }
    }

    return dlg->DialogProc(hwndDlg, uMsg, wParam, lParam);
}

INT_PTR Dialog::DialogProc(HWND hwndDlg, UINT uMsg,
        WPARAM wParam, LPARAM lParam) {

    unsigned short nCode, ctrlId;

    switch (uMsg) {
    case WM_INITDIALOG:
        return FALSE;

    case WM_COMMAND:
        nCode = HIWORD(wParam);
        ctrlId = LOWORD(wParam);
        if (_controlMap.count(ctrlId) > 0) {
            CLOG(L"yes");
            return _controlMap[ctrlId]->Command(nCode);
        } else {
            return FALSE;
        }

    case WM_NOTIFY:
        NMHDR *nHdr = (NMHDR *) lParam;
        ctrlId = nHdr->idFrom;
        if (_controlMap.count(ctrlId) > 0) {
            return _controlMap[ctrlId]->Notification(nHdr);
        } else {
            return FALSE;
        }
    }

    return FALSE;
}
<commit_msg>Remove debug log entry<commit_after>#include "Dialog.h"

Dialog::Dialog(LPCWSTR className, LPCWSTR dlgTemplate) :
Window(className) {
    _dlgHwnd = CreateDialogParam(
        Window::InstanceHandle(),
        dlgTemplate,
        Window::Handle(),
        StaticDialogProc,
        (LPARAM) this);
}

void Dialog::AddControl(Control *control) {
    if (control == nullptr) {
        return;
    }

    CLOG(L"Adding control id: %d", control->ID());
    int id = control->ID();
    _controlMap[id] = control;
}

HWND Dialog::DialogHandle() {
    return _dlgHwnd;
}

INT_PTR Dialog::StaticDialogProc(HWND hwndDlg, UINT uMsg,
        WPARAM wParam, LPARAM lParam) {
    Dialog *dlg;

    if (uMsg == WM_INITDIALOG) {
        dlg = (Dialog *) lParam;
        SetWindowLongPtr(hwndDlg, DWLP_USER, (LONG_PTR) dlg);
    } else {
        dlg = (Dialog *) GetWindowLongPtr(hwndDlg, DWLP_USER);
        if (!dlg) {
            return FALSE;
        }
    }

    return dlg->DialogProc(hwndDlg, uMsg, wParam, lParam);
}

INT_PTR Dialog::DialogProc(HWND hwndDlg, UINT uMsg,
        WPARAM wParam, LPARAM lParam) {

    unsigned short nCode, ctrlId;

    switch (uMsg) {
    case WM_INITDIALOG:
        return FALSE;

    case WM_COMMAND:
        nCode = HIWORD(wParam);
        ctrlId = LOWORD(wParam);
        if (_controlMap.count(ctrlId) > 0) {
            return _controlMap[ctrlId]->Command(nCode);
        } else {
            return FALSE;
        }

    case WM_NOTIFY:
        NMHDR *nHdr = (NMHDR *) lParam;
        ctrlId = nHdr->idFrom;
        if (_controlMap.count(ctrlId) > 0) {
            return _controlMap[ctrlId]->Notification(nHdr);
        } else {
            return FALSE;
        }
    }

    return FALSE;
}
<|endoftext|>