<commit_before>#include "GMSAssetResourceManagementSystem.hpp"

#include "../AssetResources/TextureInfoResource.hpp"
#include "../AssetResources/GMSRoomResource.hpp"

#include <KEngine/Events/OtherGraphicsEvents.hpp>
#include <KEngine/App.hpp>
#include <KEngine/Core/EventManager.hpp>
#include <KEngine/Utility/FileSystemHelper.hpp>
#include <KEngine/Log/Log.hpp>

#include <SFML/Graphics/Image.hpp>

#include <algorithm>
#include <execution>
#include <filesystem>
#include <fstream>
#include <limits>
#include <mutex>
#include <utility>

namespace
{
    static const auto ProjectForecastExecAssetsRoot = "D:/workspace/ProjectForecastExecAssetsRoot";
    static const auto ProjectForecastExecAssetPath = "D:/workspace/ProjectForecastExecAssetsRoot/assets";
}

namespace pf
{

    bool GMSAssetResourceManagementSystem::initialise()
    {
        ke::Log::instance()->info("Scanning assets...");
        namespace fs = std::filesystem;

        ke::Log::instance()->info("Scanning texture assets...");
        fs::path texturesRootDirPath = fs::path{ ProjectForecastExecAssetPath } / "textures";
        sf::Image tempImage;
        std::hash<ke::String> hasher;
        for (const auto & texDirPath : ke::FileSystemHelper::getChildPaths(texturesRootDirPath))
        {
            ke::Log::instance()->info("Discovered texture asset: {}", texDirPath.string());

            auto textureFilePaths = ke::FileSystemHelper::getFilePaths(texDirPath);
            if (textureFilePaths.size() == 1)
            {
                auto texPath         = textureFilePaths[0];
                auto textureResource = std::make_shared<TextureInfoResource>();
                textureResource->setName(texPath.stem().string());
                textureResource->setTextureId(hasher(textureResource->getName()));
                textureResource->setSourcePath(texPath.string());

                // retrieve size
                bool ret = tempImage.loadFromFile(texPath.string());
                assert(ret);
                TextureInfoResource::DimensionType dimension;
                dimension.width  = tempImage.getSize().x;
                dimension.height = tempImage.getSize().y;
                textureResource->setTextureSize(dimension);

                ke::App::instance()->getResourceManager()->registerResource(textureResource);
            }
            else
            {
                // ignore when there're multiple texture files in a single dir for now.
            }

        }

        ke::Log::instance()->info("Scanning GM:S room assets...");
        const auto gmsRoomsRootDirPath = fs::path{ ProjectForecastExecAssetPath } / "rooms";
        const auto gmsRoomPaths = ke::FileSystemHelper::getFilePaths(gmsRoomsRootDirPath);
        std::mutex gmsRoomLoadMutex;
        std::for_each(std::execution::par, std::begin(gmsRoomPaths), std::end(gmsRoomPaths), [&](const auto & gmsRoomPath) {
            ke::Log::instance()->info("Discovered GM:S room asset: {}", gmsRoomPath.string());
            auto roomResource = std::make_shared<GMSRoomResource>();
            roomResource->setName(gmsRoomPath.stem().string());
            roomResource->setSourcePath(gmsRoomPath.string());

            std::ifstream roomFileStream{ gmsRoomPath.string() };
            ke::json roomJson;
            roomFileStream >> roomJson;

            //
            // Load general room info.
            //
            GMSRoomResource::SizeType roomSize;
            roomSize.width  = roomJson["size"]["width"].get<unsigned>();
            roomSize.height = roomJson["size"]["height"].get<unsigned>();
            roomResource->setSize(roomSize);
            roomResource->setSpeed(roomJson["speed"].get<int>());
            auto roomColourStr = roomJson["colour"].get<ke::String>();
            assert(roomColourStr.length() == 9);
            assert(roomColourStr[0] == '#');
            ke::Colour roomColour = {
                // assume colour is in hex RGBA starting with the '#' symbol.
                static_cast<uint8_t>(std::stol(roomColourStr.substr(1, 2), nullptr, 16)),
                static_cast<uint8_t>(std::stol(roomColourStr.substr(3, 2), nullptr, 16)),
                static_cast<uint8_t>(std::stol(roomColourStr.substr(5, 2), nullptr, 16)),
                static_cast<uint8_t>(std::stol(roomColourStr.substr(7, 2), nullptr, 16)) };
            roomResource->setColour(roomColour);

            //
            // load background info
            //
            auto roomBackgroundsJson = roomJson["bgs"];
            for (const auto & backgroundJson : roomBackgroundsJson)
            {
                GMSRoomBackgroundInfo backgroundInfo;
                backgroundInfo.enabled    = backgroundJson["enabled"].get<bool>();
                backgroundInfo.foreground = backgroundJson["foreground"].get<bool>();
                backgroundInfo.pos        = { backgroundJson["pos"]["x"].get<int>(), -backgroundJson["pos"]["y"].get<int>() };
                backgroundInfo.tilex      = backgroundJson["tilex"].get<bool>();
                backgroundInfo.tiley      = backgroundJson["tiley"].get<bool>();
                backgroundInfo.speed      = { backgroundJson["speed"]["x"].get<int>(), -backgroundJson["speed"]["y"].get<int>() };
                backgroundInfo.stretch    = backgroundJson["stretch"].get<bool>();
                backgroundInfo.bg         = backgroundJson.value("bg", "");
                backgroundInfo.bg_hash    = hasher(backgroundInfo.bg);
                roomResource->addBackgroundInfo(backgroundInfo);
            }

            //
            // load tile instances
            //
            auto roomTilesJson = roomJson["tiles"];
            for (const auto & tileJson : roomTilesJson)
            {
                pf::GMSRoomTileInstance newTile;
                newTile.instanceid = tileJson["instanceid"].get<unsigned>();

                // Here we make sure to convert the GM:S room coordinates to KEngine's world coordinates.
                // I.e. y-down to y-up.
                // Texture coordinates are the same at the moment at y-down. I.e. (0,0) at top left.
                newTile.pos       = { tileJson["pos"]["x"].get<int>(), -tileJson["pos"]["y"].get<int>() };
                newTile.bg        = tileJson["bg"].get<ke::String>();
                newTile.bg_hash   = hasher(newTile.bg);
                newTile.sourcepos = { tileJson["sourcepos"]["x"].get<int>(), tileJson["sourcepos"]["y"].get<int>() }; // sourcepos is y-down local image coordinates.
                newTile.size      = { tileJson["size"]["width"].get<int>(), tileJson["size"]["height"].get<int>() };
                newTile.scale     = { tileJson["scale"]["x"].get<float>(), tileJson["scale"]["y"].get<float>() };

                auto colourStr    = tileJson["colour"].get<ke::String>();
                assert(colourStr.length() == 9);
                assert(colourStr[0] == '#');
                newTile.colour    = {
                    // assume colour is in hex RGBA starting with the '#' symbol.
                    static_cast<uint8_t>(std::stol(colourStr.substr(1, 2), nullptr, 16)),
                    static_cast<uint8_t>(std::stol(colourStr.substr(3, 2), nullptr, 16)),
                    static_cast<uint8_t>(std::stol(colourStr.substr(5, 2), nullptr, 16)),
                    static_cast<uint8_t>(std::stol(colourStr.substr(7, 2), nullptr, 16)) };

                // Here convert GM:S' depth system to KEngine's depth system.
                // GM:S depth value: larger == further back.
                // KEngine depth value: larger == further in front.
                newTile.tiledepth = -tileJson["tiledepth"].get<ke::graphics::DepthType>();

                roomResource->addTile(newTile);
            }

            std::scoped_lock lock(gmsRoomLoadMutex);
            ke::App::instance()->getResourceManager()->registerResource(roomResource);
        });

        ke::Log::instance()->info("Scanning assets... DONE");
        return true;
    }

    void GMSAssetResourceManagementSystem::shutdown()
    {
    }

    void GMSAssetResourceManagementSystem::update(ke::Time elapsedTime)
    {
        KE_UNUSED(elapsedTime);
    }

}<commit_msg>use std::execution::par_unseq intead of std::execution::par when loading GMS rooms<commit_after>#include "GMSAssetResourceManagementSystem.hpp"

#include "../AssetResources/TextureInfoResource.hpp"
#include "../AssetResources/GMSRoomResource.hpp"

#include <KEngine/Events/OtherGraphicsEvents.hpp>
#include <KEngine/App.hpp>
#include <KEngine/Core/EventManager.hpp>
#include <KEngine/Utility/FileSystemHelper.hpp>
#include <KEngine/Log/Log.hpp>

#include <SFML/Graphics/Image.hpp>

#include <algorithm>
#include <execution>
#include <filesystem>
#include <fstream>
#include <limits>
#include <mutex>
#include <utility>

namespace
{
    static const auto ProjectForecastExecAssetsRoot = "D:/workspace/ProjectForecastExecAssetsRoot";
    static const auto ProjectForecastExecAssetPath = "D:/workspace/ProjectForecastExecAssetsRoot/assets";
}

namespace pf
{

    bool GMSAssetResourceManagementSystem::initialise()
    {
        ke::Log::instance()->info("Scanning assets...");
        namespace fs = std::filesystem;

        ke::Log::instance()->info("Scanning texture assets...");
        fs::path texturesRootDirPath = fs::path{ ProjectForecastExecAssetPath } / "textures";
        sf::Image tempImage;
        std::hash<ke::String> hasher;
        for (const auto & texDirPath : ke::FileSystemHelper::getChildPaths(texturesRootDirPath))
        {
            ke::Log::instance()->info("Discovered texture asset: {}", texDirPath.string());

            auto textureFilePaths = ke::FileSystemHelper::getFilePaths(texDirPath);
            if (textureFilePaths.size() == 1)
            {
                auto texPath         = textureFilePaths[0];
                auto textureResource = std::make_shared<TextureInfoResource>();
                textureResource->setName(texPath.stem().string());
                textureResource->setTextureId(hasher(textureResource->getName()));
                textureResource->setSourcePath(texPath.string());

                // retrieve size
                bool ret = tempImage.loadFromFile(texPath.string());
                assert(ret);
                TextureInfoResource::DimensionType dimension;
                dimension.width  = tempImage.getSize().x;
                dimension.height = tempImage.getSize().y;
                textureResource->setTextureSize(dimension);

                ke::App::instance()->getResourceManager()->registerResource(textureResource);
            }
            else
            {
                // ignore when there're multiple texture files in a single dir for now.
            }

        }

        ke::Log::instance()->info("Scanning GM:S room assets...");
        const auto gmsRoomsRootDirPath = fs::path{ ProjectForecastExecAssetPath } / "rooms";
        const auto gmsRoomPaths = ke::FileSystemHelper::getFilePaths(gmsRoomsRootDirPath);
        std::mutex gmsRoomLoadMutex;
        std::for_each(std::execution::par_unseq, std::begin(gmsRoomPaths), std::end(gmsRoomPaths), [&](const auto & gmsRoomPath) {
            ke::Log::instance()->info("Discovered GM:S room asset: {}", gmsRoomPath.string());
            auto roomResource = std::make_shared<GMSRoomResource>();
            roomResource->setName(gmsRoomPath.stem().string());
            roomResource->setSourcePath(gmsRoomPath.string());

            std::ifstream roomFileStream{ gmsRoomPath.string() };
            ke::json roomJson;
            roomFileStream >> roomJson;

            //
            // Load general room info.
            //
            GMSRoomResource::SizeType roomSize;
            roomSize.width  = roomJson["size"]["width"].get<unsigned>();
            roomSize.height = roomJson["size"]["height"].get<unsigned>();
            roomResource->setSize(roomSize);
            roomResource->setSpeed(roomJson["speed"].get<int>());
            auto roomColourStr = roomJson["colour"].get<ke::String>();
            assert(roomColourStr.length() == 9);
            assert(roomColourStr[0] == '#');
            ke::Colour roomColour = {
                // assume colour is in hex RGBA starting with the '#' symbol.
                static_cast<uint8_t>(std::stol(roomColourStr.substr(1, 2), nullptr, 16)),
                static_cast<uint8_t>(std::stol(roomColourStr.substr(3, 2), nullptr, 16)),
                static_cast<uint8_t>(std::stol(roomColourStr.substr(5, 2), nullptr, 16)),
                static_cast<uint8_t>(std::stol(roomColourStr.substr(7, 2), nullptr, 16)) };
            roomResource->setColour(roomColour);

            //
            // load background info
            //
            auto roomBackgroundsJson = roomJson["bgs"];
            for (const auto & backgroundJson : roomBackgroundsJson)
            {
                GMSRoomBackgroundInfo backgroundInfo;
                backgroundInfo.enabled    = backgroundJson["enabled"].get<bool>();
                backgroundInfo.foreground = backgroundJson["foreground"].get<bool>();
                backgroundInfo.pos        = { backgroundJson["pos"]["x"].get<int>(), -backgroundJson["pos"]["y"].get<int>() };
                backgroundInfo.tilex      = backgroundJson["tilex"].get<bool>();
                backgroundInfo.tiley      = backgroundJson["tiley"].get<bool>();
                backgroundInfo.speed      = { backgroundJson["speed"]["x"].get<int>(), -backgroundJson["speed"]["y"].get<int>() };
                backgroundInfo.stretch    = backgroundJson["stretch"].get<bool>();
                backgroundInfo.bg         = backgroundJson.value("bg", "");
                backgroundInfo.bg_hash    = hasher(backgroundInfo.bg);
                roomResource->addBackgroundInfo(backgroundInfo);
            }

            //
            // load tile instances
            //
            auto roomTilesJson = roomJson["tiles"];
            for (const auto & tileJson : roomTilesJson)
            {
                pf::GMSRoomTileInstance newTile;
                newTile.instanceid = tileJson["instanceid"].get<unsigned>();

                // Here we make sure to convert the GM:S room coordinates to KEngine's world coordinates.
                // I.e. y-down to y-up.
                // Texture coordinates are the same at the moment at y-down. I.e. (0,0) at top left.
                newTile.pos       = { tileJson["pos"]["x"].get<int>(), -tileJson["pos"]["y"].get<int>() };
                newTile.bg        = tileJson["bg"].get<ke::String>();
                newTile.bg_hash   = hasher(newTile.bg);
                newTile.sourcepos = { tileJson["sourcepos"]["x"].get<int>(), tileJson["sourcepos"]["y"].get<int>() }; // sourcepos is y-down local image coordinates.
                newTile.size      = { tileJson["size"]["width"].get<int>(), tileJson["size"]["height"].get<int>() };
                newTile.scale     = { tileJson["scale"]["x"].get<float>(), tileJson["scale"]["y"].get<float>() };

                auto colourStr    = tileJson["colour"].get<ke::String>();
                assert(colourStr.length() == 9);
                assert(colourStr[0] == '#');
                newTile.colour    = {
                    // assume colour is in hex RGBA starting with the '#' symbol.
                    static_cast<uint8_t>(std::stol(colourStr.substr(1, 2), nullptr, 16)),
                    static_cast<uint8_t>(std::stol(colourStr.substr(3, 2), nullptr, 16)),
                    static_cast<uint8_t>(std::stol(colourStr.substr(5, 2), nullptr, 16)),
                    static_cast<uint8_t>(std::stol(colourStr.substr(7, 2), nullptr, 16)) };

                // Here convert GM:S' depth system to KEngine's depth system.
                // GM:S depth value: larger == further back.
                // KEngine depth value: larger == further in front.
                newTile.tiledepth = -tileJson["tiledepth"].get<ke::graphics::DepthType>();

                roomResource->addTile(newTile);
            }

            std::scoped_lock lock(gmsRoomLoadMutex);
            ke::App::instance()->getResourceManager()->registerResource(roomResource);
        });

        ke::Log::instance()->info("Scanning assets... DONE");
        return true;
    }

    void GMSAssetResourceManagementSystem::shutdown()
    {
    }

    void GMSAssetResourceManagementSystem::update(ke::Time elapsedTime)
    {
        KE_UNUSED(elapsedTime);
    }

}<|endoftext|>


<commit_before>#ifdef CHEMISTRY_GPU

#include "chemistry_gpu.h"
#include "../grid3D.h"
#include "../io.h"

#ifdef DE
#include"../hydro_cuda.h"
#endif

void Grid3D::Initialize_Chemistry( struct parameters *P ){

  chprintf( "Initializing the GPU Chemistry Solver... \n");

  Chem.nx = H.nx;
  Chem.ny = H.ny;
  Chem.nz = H.nz;

  Chem.gamma = gama;

  Chem.Initialize( P );

  // Initialize the Chemistry Header
  #ifdef COSMOLOGY
  Real kpc_cgs = KPC_CGS;
  Chem.H.density_conversion = Cosmo.rho_0_gas * Cosmo.cosmo_h * Cosmo.cosmo_h / pow( kpc_cgs, 3) * MSUN_CGS ;
  Chem.H.energy_conversion  =  Cosmo.v_0_gas * Cosmo.v_0_gas * 1e10;  //km^2 -> cm^2 ;
  Chem.H.cosmological_parameters_d = Chem.cosmo_params_d;
  #else // Not COSMOLOGY
  Chem.H.density_conversion = 1.0;
  Chem.H.energy_conversion  = 1.0;
  Chem.H.cosmological_parameters_d = NULL;
  #endif
  Chem.H.n_uvb_rates_samples  = Chem.n_uvb_rates_samples;
  Chem.H.uvb_rates_redshift_d = Chem.rates_z_d;
  Chem.H.photo_ion_HI_rate_d   = Chem.Ion_rates_HI_d;
  Chem.H.photo_ion_HeI_rate_d  = Chem.Ion_rates_HeI_d;
  Chem.H.photo_ion_HeII_rate_d = Chem.Ion_rates_HeII_d;
  Chem.H.photo_heat_HI_rate_d   = Chem.Heat_rates_HI_d;
  Chem.H.photo_heat_HeI_rate_d  = Chem.Heat_rates_HeI_d;
  Chem.H.photo_heat_HeII_rate_d = Chem.Heat_rates_HeII_d;

  chprintf( "Allocating Memory. \n\n");
  int n_cells = H.nx * H.ny * H.nz;
  Chem.Fields.temperature_h = (Real *) malloc(n_cells * sizeof(Real));

  chprintf( "Chemistry Solver Successfully Initialized. \n\n");

}

void Chem_GPU::Initialize( struct parameters *P ){

  chprintf( " Initializing Cosmological Parameters... \n");
  cosmo_params_h = (float *)malloc(3*sizeof(Real));
  cosmo_params_h[0] = (float)P->H0;
  cosmo_params_h[1] = (float)P->Omega_M;
  cosmo_params_h[2] = (float)P->Omega_L;

  Allocate_Array_GPU_float( &cosmo_params_d, 3 );
  Copy_Float_Array_to_Device( 3, cosmo_params_h, cosmo_params_d );

  Initialize_UVB_Ionization_and_Heating_Rates( P );

}

void Chem_GPU::Initialize_UVB_Ionization_and_Heating_Rates( struct parameters *P ){

  Load_UVB_Ionization_and_Heating_Rates( P );

  Copy_UVB_Rates_to_GPU( );

  Bind_GPU_Textures( n_uvb_rates_samples, Heat_rates_HI_h, Heat_rates_HeI_h, Heat_rates_HeII_h, Ion_rates_HI_h, Ion_rates_HeI_h, Ion_rates_HeII_h);

}

void Chem_GPU::Copy_UVB_Rates_to_GPU( ){

  Allocate_Array_GPU_float( &rates_z_d,         n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Heat_rates_HI_d,   n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Heat_rates_HeI_d,  n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Heat_rates_HeII_d, n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Ion_rates_HI_d,    n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Ion_rates_HeI_d,   n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Ion_rates_HeII_d,  n_uvb_rates_samples );

  Copy_Float_Array_to_Device( n_uvb_rates_samples, rates_z_h, rates_z_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Heat_rates_HI_h, Heat_rates_HI_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Heat_rates_HeI_h, Heat_rates_HeI_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Heat_rates_HeII_h, Heat_rates_HeII_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Ion_rates_HI_h, Ion_rates_HI_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Ion_rates_HeI_h, Ion_rates_HeI_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Ion_rates_HeII_h, Ion_rates_HeII_d );

}

void Grid3D::Update_Chemistry_Header(){

  #ifdef COSMOLOGY
  Chem.H.current_z = Cosmo.current_z;
  #else
  Chem.H.current_z = 0;
  #endif

  Chem.H.runtime_chemistry_step = 0;

}

void Grid3D::Compute_Gas_Temperature(  Real *temperature ){

  int k, j, i, id;
  Real dens_HI, dens_HII, dens_HeI, dens_HeII, dens_HeIII, dens_e, gamma;
  Real d, vx, vy, vz, E, E_kin, GE, P, mu, temp, cell_dens, cell_n;

  gamma = gama;

  for (k=0; k<H.nz; k++) {
    for (j=0; j<H.ny; j++) {
      for (i=0; i<H.nx; i++) {
        id = i + j*H.nx + k*H.nx*H.ny;

        d  =  C.density[id];
        vx =  C.momentum_x[id] / d;
        vy =  C.momentum_y[id] / d;
        vz =  C.momentum_z[id] / d;
        E = C.Energy[id];
        E_kin = 0.5 * d * ( vx*vx + vy*vy + vz*vz );

        #ifdef DE
        GE = C.GasEnergy[id];
        P = GE * (gamma - 1.0);
        #else
        P  = (dev_conserved[4*n_cells + id] - 0.5*d*(vx*vx + vy*vy + vz*vz)) * (gamma - 1.0);
        #endif

        dens_HI    = C.HI_density[id];
        dens_HII   = C.HII_density[id];
        dens_HeI   = C.HeI_density[id];
        dens_HeII  = C.HeII_density[id];
        dens_HeIII = C.HeIII_density[id];
        dens_e     = C.e_density[id];

        cell_dens = dens_HI + dens_HII + dens_HeI + dens_HeII + dens_HeIII;
        cell_n =  dens_HI + dens_HII + ( dens_HeI + dens_HeII + dens_HeIII )/4 + dens_e;
        mu = cell_dens / cell_n;

        temp = P * MP  * mu / d / KB;

        #ifdef COSMOLOGY
        temp *= 1e10; // convert from (km/s)^2 to (cm/s)^2
        #endif

        temperature[id] = temp;

        // chprintf( "mu: %e \n", mu );
        if ( temp < 0 ) chprintf( "Negative Temperature: %e \n", temp );

      }
    }
  }

}

void Chem_GPU::Reset(){

  free( rates_z_h );
  free( Heat_rates_HI_h );
  free( Heat_rates_HeI_h );
  free( Heat_rates_HeII_h );
  free( Ion_rates_HI_h );
  free( Ion_rates_HeI_h );
  free( Ion_rates_HeII_h );

  Free_Array_GPU_float( rates_z_d );
  Free_Array_GPU_float( Heat_rates_HI_d );
  Free_Array_GPU_float( Heat_rates_HeI_d );
  Free_Array_GPU_float( Heat_rates_HeII_d );
  Free_Array_GPU_float( Ion_rates_HI_d );
  Free_Array_GPU_float( Ion_rates_HeI_d );
  Free_Array_GPU_float( Ion_rates_HeII_d );

  free( Fields.temperature_h );

}

#endif<commit_msg>fixed temperature calculation<commit_after>#ifdef CHEMISTRY_GPU

#include "chemistry_gpu.h"
#include "../grid3D.h"
#include "../io.h"

#ifdef DE
#include"../hydro_cuda.h"
#endif

void Grid3D::Initialize_Chemistry( struct parameters *P ){

  chprintf( "Initializing the GPU Chemistry Solver... \n");

  Chem.nx = H.nx;
  Chem.ny = H.ny;
  Chem.nz = H.nz;

  Chem.gamma = gama;

  Chem.Initialize( P );

  // Initialize the Chemistry Header
  #ifdef COSMOLOGY
  Real kpc_cgs = KPC_CGS;
  Chem.H.density_conversion = Cosmo.rho_0_gas * Cosmo.cosmo_h * Cosmo.cosmo_h / pow( kpc_cgs, 3) * MSUN_CGS ;
  Chem.H.energy_conversion  =  Cosmo.v_0_gas * Cosmo.v_0_gas * 1e10;  //km^2 -> cm^2 ;
  Chem.H.cosmological_parameters_d = Chem.cosmo_params_d;
  #else // Not COSMOLOGY
  Chem.H.density_conversion = 1.0;
  Chem.H.energy_conversion  = 1.0;
  Chem.H.cosmological_parameters_d = NULL;
  #endif
  Chem.H.n_uvb_rates_samples  = Chem.n_uvb_rates_samples;
  Chem.H.uvb_rates_redshift_d = Chem.rates_z_d;
  Chem.H.photo_ion_HI_rate_d   = Chem.Ion_rates_HI_d;
  Chem.H.photo_ion_HeI_rate_d  = Chem.Ion_rates_HeI_d;
  Chem.H.photo_ion_HeII_rate_d = Chem.Ion_rates_HeII_d;
  Chem.H.photo_heat_HI_rate_d   = Chem.Heat_rates_HI_d;
  Chem.H.photo_heat_HeI_rate_d  = Chem.Heat_rates_HeI_d;
  Chem.H.photo_heat_HeII_rate_d = Chem.Heat_rates_HeII_d;

  chprintf( "Allocating Memory. \n\n");
  int n_cells = H.nx * H.ny * H.nz;
  Chem.Fields.temperature_h = (Real *) malloc(n_cells * sizeof(Real));

  chprintf( "Chemistry Solver Successfully Initialized. \n\n");

}

void Chem_GPU::Initialize( struct parameters *P ){

  chprintf( " Initializing Cosmological Parameters... \n");
  cosmo_params_h = (float *)malloc(3*sizeof(Real));
  cosmo_params_h[0] = (float)P->H0;
  cosmo_params_h[1] = (float)P->Omega_M;
  cosmo_params_h[2] = (float)P->Omega_L;

  Allocate_Array_GPU_float( &cosmo_params_d, 3 );
  Copy_Float_Array_to_Device( 3, cosmo_params_h, cosmo_params_d );

  Initialize_UVB_Ionization_and_Heating_Rates( P );

}

void Chem_GPU::Initialize_UVB_Ionization_and_Heating_Rates( struct parameters *P ){

  Load_UVB_Ionization_and_Heating_Rates( P );

  Copy_UVB_Rates_to_GPU( );

  Bind_GPU_Textures( n_uvb_rates_samples, Heat_rates_HI_h, Heat_rates_HeI_h, Heat_rates_HeII_h, Ion_rates_HI_h, Ion_rates_HeI_h, Ion_rates_HeII_h);

}

void Chem_GPU::Copy_UVB_Rates_to_GPU( ){

  Allocate_Array_GPU_float( &rates_z_d,         n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Heat_rates_HI_d,   n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Heat_rates_HeI_d,  n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Heat_rates_HeII_d, n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Ion_rates_HI_d,    n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Ion_rates_HeI_d,   n_uvb_rates_samples );
  Allocate_Array_GPU_float( &Ion_rates_HeII_d,  n_uvb_rates_samples );

  Copy_Float_Array_to_Device( n_uvb_rates_samples, rates_z_h, rates_z_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Heat_rates_HI_h, Heat_rates_HI_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Heat_rates_HeI_h, Heat_rates_HeI_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Heat_rates_HeII_h, Heat_rates_HeII_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Ion_rates_HI_h, Ion_rates_HI_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Ion_rates_HeI_h, Ion_rates_HeI_d );
  Copy_Float_Array_to_Device( n_uvb_rates_samples, Ion_rates_HeII_h, Ion_rates_HeII_d );

}

void Grid3D::Update_Chemistry_Header(){

  #ifdef COSMOLOGY
  Chem.H.current_z = Cosmo.current_z;
  #else
  Chem.H.current_z = 0;
  #endif

  Chem.H.runtime_chemistry_step = 0;

}

void Grid3D::Compute_Gas_Temperature(  Real *temperature ){

  int k, j, i, id;
  Real dens_HI, dens_HII, dens_HeI, dens_HeII, dens_HeIII, dens_e, gamma;
  Real d, vx, vy, vz, E, GE, mu, temp, cell_dens, cell_n;
  Real current_a, a2;
  gamma = gama;

  for (k=0; k<H.nz; k++) {
    for (j=0; j<H.ny; j++) {
      for (i=0; i<H.nx; i++) {
        id = i + j*H.nx + k*H.nx*H.ny;

        d  =  C.density[id];
        vx =  C.momentum_x[id] / d;
        vy =  C.momentum_y[id] / d;
        vz =  C.momentum_z[id] / d;
        E = C.Energy[id];

        #ifdef DE
        GE = C.GasEnergy[id];
        #else
        GE = (dev_conserved[4*n_cells + id] - 0.5*d*(vx*vx + vy*vy + vz*vz));
        #endif

        dens_HI    = C.HI_density[id];
        dens_HII   = C.HII_density[id];
        dens_HeI   = C.HeI_density[id];
        dens_HeII  = C.HeII_density[id];
        dens_HeIII = C.HeIII_density[id];
        dens_e     = C.e_density[id];

        cell_dens = dens_HI + dens_HII + dens_HeI + dens_HeII + dens_HeIII;
        cell_n =  dens_HI + dens_HII + ( dens_HeI + dens_HeII + dens_HeIII )/4 + dens_e;
        mu = cell_dens / cell_n;

        #ifdef COSMOLOGY
        current_a = Cosmo.current_a;
        a2 = current_a * current_a;
        GE *= Chem.H.energy_conversion / a2;
        #endif

        temp = GE * MP  * mu / d / KB * (gamma - 1.0);  ;
        temperature[id] = temp;
        // chprintf( "mu: %e \n", mu );
        // if ( temp > 1e7 ) chprintf( "Temperature: %e   mu: %e \n", temp, mu );

      }
    }
  }

}

void Chem_GPU::Reset(){

  free( rates_z_h );
  free( Heat_rates_HI_h );
  free( Heat_rates_HeI_h );
  free( Heat_rates_HeII_h );
  free( Ion_rates_HI_h );
  free( Ion_rates_HeI_h );
  free( Ion_rates_HeII_h );

  Free_Array_GPU_float( rates_z_d );
  Free_Array_GPU_float( Heat_rates_HI_d );
  Free_Array_GPU_float( Heat_rates_HeI_d );
  Free_Array_GPU_float( Heat_rates_HeII_d );
  Free_Array_GPU_float( Ion_rates_HI_d );
  Free_Array_GPU_float( Ion_rates_HeI_d );
  Free_Array_GPU_float( Ion_rates_HeII_d );

  free( Fields.temperature_h );

}

#endif<|endoftext|>


<commit_before><commit_msg>Prediction: minor fixes.<commit_after><|endoftext|>


<commit_before>#ifndef CTHREADPOOLITEM_RET
#define CTHREADPOOLITEM_RET
#include<functional>	//bind, ref
#include<utility>	//forward
#include"../../lib/header/tool/CScopeGuard.hpp"
#include"../../lib/header/thread/CTask.hpp"
#include"../../lib/header/thread/CWait_bounded_queue.hpp"
#include"IThreadPoolItemBase.hpp"

namespace nThread
{
	template<class Ret>
	class CThreadPoolItem_Ret:public IThreadPoolItemBase
	{
		CTask<Ret> exec_;
		CWait_bounded_queue<CThreadPoolItem_Ret<Ret>*> *waiting_queue_;
	public:
		CThreadPoolItem_Ret(CWait_bounded_queue<CThreadPoolItem_Ret<Ret>*> *waitingQue)
			:waiting_queue_{waitingQue}{}
		CThreadPoolItem_Ret(const CThreadPoolItem_Ret &)=delete;
		CThreadPoolItem_Ret(CThreadPoolItem_Ret &&)=default;
		template<class Func,class ... Args>
		void assign(Func &&func,Args &&...args)
		{
			exec_=CTask<Ret>{std::forward<Func>(func),std::forward<Args>(args)...};
			IThreadPoolItemBase::exec_(std::bind(&CTask<Ret>::operator(),std::ref(exec_)));
		}
		inline Ret get()
		{
			const nTool::CScopeGuard sg{[this]{waiting_queue_->emplace_and_notify(this);}};
			return exec_.get();
		}
		bool is_running() const noexcept override
		{
			return exec_.valid();
		}
		void wait() const override
		{
			exec_.wait();
		}
		CThreadPoolItem_Ret& operator=(const CThreadPoolItem_Ret &)=delete;
		CThreadPoolItem_Ret& operator=(CThreadPoolItem_Ret &&)=default;
		~CThreadPoolItem_Ret()
		{
			if(is_running())
				wait();
		}
	};
}

#endif<commit_msg>reorder code<commit_after>#ifndef CTHREADPOOLITEM_RET
#define CTHREADPOOLITEM_RET
#include<functional>	//bind, ref
#include<utility>	//forward
#include"../../lib/header/thread/CTask.hpp"
#include"../../lib/header/thread/CWait_bounded_queue.hpp"
#include"../../lib/header/tool/CScopeGuard.hpp"
#include"IThreadPoolItemBase.hpp"

namespace nThread
{
	template<class Ret>
	class CThreadPoolItem_Ret:public IThreadPoolItemBase
	{
		CTask<Ret> exec_;
		CWait_bounded_queue<CThreadPoolItem_Ret<Ret>*> *waiting_queue_;
	public:
		CThreadPoolItem_Ret(CWait_bounded_queue<CThreadPoolItem_Ret<Ret>*> *waitingQue)
			:waiting_queue_{waitingQue}{}
		CThreadPoolItem_Ret(const CThreadPoolItem_Ret &)=delete;
		CThreadPoolItem_Ret(CThreadPoolItem_Ret &&)=default;
		template<class Func,class ... Args>
		void assign(Func &&func,Args &&...args)
		{
			exec_=CTask<Ret>{std::forward<Func>(func),std::forward<Args>(args)...};
			IThreadPoolItemBase::exec_(std::bind(&CTask<Ret>::operator(),std::ref(exec_)));
		}
		inline Ret get()
		{
			const nTool::CScopeGuard sg{[this]{waiting_queue_->emplace_and_notify(this);}};
			return exec_.get();
		}
		bool is_running() const noexcept override
		{
			return exec_.valid();
		}
		void wait() const override
		{
			exec_.wait();
		}
		CThreadPoolItem_Ret& operator=(const CThreadPoolItem_Ret &)=delete;
		CThreadPoolItem_Ret& operator=(CThreadPoolItem_Ret &&)=default;
		~CThreadPoolItem_Ret()
		{
			if(is_running())
				wait();
		}
	};
}

#endif<|endoftext|>


<commit_before>#include <iostream>                                         /* allows to perform standard input and output operations */
#include <fstream>
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <ctype.h>                                          /* isxxx() */
#include <ros/ros.h>                                        /* ROS */
#include <geometry_msgs/Twist.h>                            /* ROS Twist message */
#include <base_controller/encoders.h>                       /* Custom message /encoders */
#include <base_controller/md49data.h>                       /* Custom message /encoders */
#include <sqlite3.h>

int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
unsigned char speed_l=128, speed_r=128;                     /* speed to set for MD49 */
unsigned char last_speed_l=128, last_speed_r=128;           /* speed to set for MD49 */
double vr = 0.0;
double vl = 0.0;
double max_vr = 0.2;
double max_vl = 0.2;
double min_vr = 0.2;
double min_vl = 0.2;
double base_width = 0.4;                                    /* Base width in meters */

unsigned char serialBuffer[18];                             /* Serial buffer to store uart data */
void read_MD49_Data (void);
void set_md49_speed (unsigned char speed_l, unsigned char speed_r);

// sqlite globals
sqlite3 *db;
char *zErrMsg = 0;
int  rc;
const char *sql;
const char* data = "Callback function called";

using namespace std;

base_controller::encoders encoders;
base_controller::md49data md49data;

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){

        if (vel_cmd.linear.x>0){
            speed_l = 255;
            speed_r = 255;
        }
        if (vel_cmd.linear.x<0){
            speed_l = 1;
            speed_r = 1;
        }
        if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
            speed_l = 128;
            speed_r = 128;
        }
        if (vel_cmd.angular.z>0){
            speed_l = 1;
            speed_r = 255;
        }
        if (vel_cmd.angular.z<0){
            speed_l = 255;
            speed_r = 1;
        }

    /*
        //ANFANG Alternative
        if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){vl=0;vr=0;}
        else if(vel_cmd.linear.x == 0){
            // turning
            vr = vel_cmd.angular.z * base_width / 2.0;
            vl = (-1) * vr;
        }
        else if(vel_cmd.angular.z == 0){
            // forward / backward
            vl = vr = vel_cmd.linear.x;
        }
        else{
            // moving doing arcs
            vl = vel_cmd.linear.x - vel_cmd.angular.z * base_width / 2.0;
            if (vl > max_vl) {vl=max_vl;}
            if (vl < min_vl) {vl=min_vl;}
            vr = vel_cmd.linear.x + vel_cmd.angular.z * base_width / 2.0;
            if (vr > max_vr) {vr=max_vr;}
            if (vr < min_vr) {vr=min_vr;}
        }
        //ENDE Alternative
    */
}

int main( int argc, char* argv[] ){

    // Setup as ROS node
    // *****************
    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 10, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<base_controller::encoders>("encoders",10);
    ros::Publisher md49data_pub = n.advertise<base_controller::md49data>("md49data",10);

    ros::Rate loop_rate(10);
    ROS_INFO("Starting base_controller node:");
    ROS_INFO("============================================");
    ROS_INFO("subscribing to /cmd_vel");
    ROS_INFO("publishing to /encoders");
    ROS_INFO("publishing to /md49data");
    ROS_INFO("============================================");

    // Open database md49data.db and add
    // table md49commands
    // *********************************
    rc = sqlite3_open("data/md49data.db", &db);
    if( rc ){
        ROS_INFO("Can't open database: %s", sqlite3_errmsg(db));
        exit(0);
    }else{
        ROS_INFO("Opened database successfully,");
    }

    // Create table md49commands
    // *************************
    sql = "CREATE TABLE md49commands("  \
     "ID INT PRIMARY KEY     NOT NULL," \
     "SpeedL         INT DEFAULT 128," \
     "SpeedR         INT DEFAULT 128 );" \
     "INSERT INTO md49commands (ID,SpeedL,SpeedR) VALUES (1,128,128);";
    rc = sqlite3_exec(db, sql, NULL, 0, &zErrMsg);                      // Execute SQL statement
    if( rc != SQLITE_OK ){
        ROS_INFO("SQL message: %s", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        ROS_INFO("table created successfully");
    }

    // Set SpeedL and SpeedR to
    // defaults =128
    // ************************
    char sql_buffer[200];
    int cx;
    cx = snprintf (sql_buffer,200,"UPDATE md49commands SET SpeedL=%i, SpeedR=%i WHERE ID=1", 128,128);

    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        ROS_INFO("SQL message: %s", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        ROS_INFO("SpeedL & SpeedR set to defaults in Table md49commands(md49data.db)");
    }

    while(n.ok())
    {
        // Read encoder values and other data from MD49:
        // serial_controller_node reads data from AVR-Master
        // and provides that data in md49_data.txt
        // *************************************************
//        read_MD49_Data();

        // Set MD49 speed_l and speed_r:
        // serial_controller_node reads commands from
        // md49_commands.txt and writes commands to AVR-Master
        // ***************************************************
        if ((speed_l != last_speed_l) || (speed_r != last_speed_r)){
            set_md49_speed(speed_l,speed_r);
            last_speed_l=speed_l;
            last_speed_r=speed_r;
        }

        // Publish encoder values to topic /encoders (custom message)
        // **********************************************************
        encoders.encoder_l=EncoderL;
        encoders.encoder_r=EncoderR;
        encoders_pub.publish(encoders);

        // Publish MD49 data to topic /md49data (custom message)
        // *****************************************************
        md49data.speed_l = serialBuffer[8];
        md49data.speed_r = serialBuffer[9];
        md49data.volt = serialBuffer[10];
        md49data.current_l = serialBuffer[11];
        md49data.current_r = serialBuffer[12];
        md49data.error = serialBuffer[13];
        md49data.acceleration = serialBuffer[14];
        md49data.mode = serialBuffer[15];
        md49data.regulator = serialBuffer[16];
        md49data.timeout = serialBuffer[17];
        md49data_pub.publish(md49data);

        ros::spinOnce();
        loop_rate.sleep();

    }// end.mainloop

    return 1;
} // end.main

void read_MD49_Data (void){

    // Read all MD49 data from md49_data.txt
    // *************************************
    string line;
    ifstream myfile ("md49_data.txt");
    if (myfile.is_open()){
        int i=0;
        while (getline (myfile,line)){
            //cout << line << '\n';
            char data[10];
            std::copy(line.begin(), line.end(), data);
            serialBuffer[i]=atoi(data);
            i =i++;
        }
        myfile.close();

    }
    else ROS_ERROR("Unable to open file: md49_data.txt");

    // Put toghether new encodervalues
    // *******************************
    EncoderL = serialBuffer[0] << 24;                        // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                        // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

}

void set_md49_speed (unsigned char speed_l, unsigned char speed_r){

    // Set SpeedL=speed_l and
    // SpeedR=speed_r in Table md49commands
    // ************************************
    char sql_buffer[200];
    int cx;
    cx = snprintf (sql_buffer,200,"UPDATE md49commands SET SpeedL=%i, SpeedR=%i WHERE ID=1", speed_l,speed_r);

    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        ROS_WARN("SQL message: %s", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        ROS_INFO("Set SpeedL=%i and SpeedR=%i in Table md49commands(md49data.db)",speed_l, speed_r);
    }
}

<commit_msg>Update code<commit_after>#include <iostream>                                         /* allows to perform standard input and output operations */
#include <fstream>
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <ctype.h>                                          /* isxxx() */
#include <ros/ros.h>                                        /* ROS */
#include <geometry_msgs/Twist.h>                            /* ROS Twist message */
#include <base_controller/encoders.h>                       /* Custom message /encoders */
#include <base_controller/md49data.h>                       /* Custom message /encoders */
#include <sqlite3.h>

int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
unsigned char speed_l=128, speed_r=128;                     /* speed to set for MD49 */
unsigned char last_speed_l=128, last_speed_r=128;           /* speed to set for MD49 */
double vr = 0.0;
double vl = 0.0;
double max_vr = 0.2;
double max_vl = 0.2;
double min_vr = 0.2;
double min_vl = 0.2;
double base_width = 0.4;                                    /* Base width in meters */

unsigned char serialBuffer[18];                             /* Serial buffer to store uart data */
void read_MD49_Data (void);
void set_md49_speed (unsigned char speed_l, unsigned char speed_r);
void open_sql_db_md49data(void);

// sqlite globals
sqlite3 *db;
char *zErrMsg = 0;
int  rc;
const char *sql;
const char* data = "Callback function called";

using namespace std;

base_controller::encoders encoders;
base_controller::md49data md49data;

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){

        if (vel_cmd.linear.x>0){
            speed_l = 255;
            speed_r = 255;
        }
        if (vel_cmd.linear.x<0){
            speed_l = 0;
            speed_r = 0;
        }
        if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
            speed_l = 128;
            speed_r = 128;
        }
        if (vel_cmd.angular.z>0){
            speed_l = 0;
            speed_r = 255;
        }
        if (vel_cmd.angular.z<0){
            speed_l = 255;
            speed_r = 0;
        }

    /*
        //ANFANG Alternative
        if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){vl=0;vr=0;}
        else if(vel_cmd.linear.x == 0){
            // turning
            vr = vel_cmd.angular.z * base_width / 2.0;
            vl = (-1) * vr;
        }
        else if(vel_cmd.angular.z == 0){
            // forward / backward
            vl = vr = vel_cmd.linear.x;
        }
        else{
            // moving doing arcs
            vl = vel_cmd.linear.x - vel_cmd.angular.z * base_width / 2.0;
            if (vl > max_vl) {vl=max_vl;}
            if (vl < min_vl) {vl=min_vl;}
            vr = vel_cmd.linear.x + vel_cmd.angular.z * base_width / 2.0;
            if (vr > max_vr) {vr=max_vr;}
            if (vr < min_vr) {vr=min_vr;}
        }
        //ENDE Alternative
    */
}

void open_sql_db_md49data(void){
    // Open database md49data.db and add
    // table md49commands
    // *********************************
    rc = sqlite3_open("data/md49data.db", &db);
    if( rc ){
        ROS_INFO("Can't open database: %s", sqlite3_errmsg(db));
        exit(0);
    }else{
        ROS_INFO("Opened database successfully,");
    }

    // Create table md49commands
    // *************************
    sql = "CREATE TABLE md49commands("  \
     "ID INT PRIMARY KEY     NOT NULL," \
     "SpeedL         INT DEFAULT 128," \
     "SpeedR         INT DEFAULT 128 );" \
     "INSERT INTO md49commands (ID,SpeedL,SpeedR) VALUES (1,128,128);";
    rc = sqlite3_exec(db, sql, NULL, 0, &zErrMsg);                      // Execute SQL statement
    if( rc != SQLITE_OK ){
        ROS_INFO("SQL message: %s", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        ROS_INFO("table created successfully");
    }

    // Set SpeedL and SpeedR to
    // defaults =128
    // ************************
    char sql_buffer[200];
    int cx;
    cx = snprintf (sql_buffer,200,"UPDATE md49commands SET SpeedL=%i, SpeedR=%i WHERE ID=1", 128,128);

    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        ROS_INFO("SQL message: %s", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        ROS_INFO("SpeedL & SpeedR set to defaults in Table md49commands(md49data.db)");
    }

}

int main( int argc, char* argv[] ){

    // Setup as ROS node
    // *****************
    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 10, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<base_controller::encoders>("encoders",10);
    ros::Publisher md49data_pub = n.advertise<base_controller::md49data>("md49data",10);

    ros::Rate loop_rate(10);
    ROS_INFO("Starting base_controller node:");
    ROS_INFO("============================================");
    ROS_INFO("subscribing to /cmd_vel");
    ROS_INFO("publishing to /encoders");
    ROS_INFO("publishing to /md49data");
    ROS_INFO("============================================");

    open_sql_db_md49data();

    while(n.ok())
    {
        // Read encoder values and other data from MD49:
        // serial_controller_node reads data from AVR-Master
        // and provides that data in md49_data.txt
        // *************************************************
//        read_MD49_Data();

        // Set MD49 speed_l and speed_r:
        // serial_controller_node reads commands from
        // md49_commands.txt and writes commands to AVR-Master
        // ***************************************************
        if ((speed_l != last_speed_l) || (speed_r != last_speed_r)){
            set_md49_speed(speed_l,speed_r);
            last_speed_l=speed_l;
            last_speed_r=speed_r;
        }

        // Publish encoder values to topic /encoders (custom message)
        // **********************************************************
        encoders.encoder_l=EncoderL;
        encoders.encoder_r=EncoderR;
        encoders_pub.publish(encoders);

        // Publish MD49 data to topic /md49data (custom message)
        // *****************************************************
        md49data.speed_l = serialBuffer[8];
        md49data.speed_r = serialBuffer[9];
        md49data.volt = serialBuffer[10];
        md49data.current_l = serialBuffer[11];
        md49data.current_r = serialBuffer[12];
        md49data.error = serialBuffer[13];
        md49data.acceleration = serialBuffer[14];
        md49data.mode = serialBuffer[15];
        md49data.regulator = serialBuffer[16];
        md49data.timeout = serialBuffer[17];
        md49data_pub.publish(md49data);

        ros::spinOnce();
        loop_rate.sleep();

    }// end.mainloop

    return 1;
} // end.main

void read_MD49_Data (void){

    // Read MD49 data from md49data.db
    // *******************************
    // code here!

    // Read all MD49 data from md49_data.txt
    // *************************************
    string line;
    ifstream myfile ("md49_data.txt");
    if (myfile.is_open()){
        int i=0;
        while (getline (myfile,line)){
            //cout << line << '\n';
            char data[10];
            std::copy(line.begin(), line.end(), data);
            serialBuffer[i]=atoi(data);
            i =i++;
        }
        myfile.close();

    }
    else ROS_ERROR("Unable to open file: md49_data.txt");

    // Put toghether new encodervalues
    // *******************************
    EncoderL = serialBuffer[0] << 24;                        // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                        // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

}

void set_md49_speed (unsigned char speed_l, unsigned char speed_r){

    // Set SpeedL=speed_l and
    // SpeedR=speed_r in Table md49commands
    // ************************************
    char sql_buffer[200];
    int cx;
    cx = snprintf (sql_buffer,200,"UPDATE md49commands SET SpeedL=%i, SpeedR=%i WHERE ID=1", speed_l,speed_r);

    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        ROS_WARN("SQL message: %s", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        ROS_INFO("Set SpeedL=%s and SpeedR=%s in Table md49commands(md49data.db)",speed_l, speed_r);
    }
}

<|endoftext|>


<commit_before>#include <iostream>                                         /* allows to perform standard input and output operations */
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
//#include <string>                                         /* String function definitions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <errno.h>                                          /* Error number definitions */
#include <termios.h>                                        /* POSIX terminal control definitions */
//#include <ctype.h>                                        /* isxxx() */
#include <ros/ros.h>                                        /* ROS */
#include <geometry_msgs/Twist.h>                            /* ROS Twist message */
#include <base_controller/encoders.h>                       /* Custom message /encoders */

const char* serialport="/dev/ttyAMA0";                      /* defines used serialport */
int serialport_bps=B38400;                                  /* defines baudrate od serialport */
int16_t EncoderL;                                           /* stores encoder value left read from md49 */
int16_t EncoderR;                                           /* stores encoder value right read from md49 */
double vr = 0.0;
double vl = 0.0;
double max_vr = 0.2;
double max_vl = 0.2;
double min_vr = 0.2;
double min_vl = 0.2;
double base_width = 0.4;                                    /* Base width in meters */

int filedesc;                                               /* filedescriptor serialport */
int fd;                                                     /* serial port file descriptor */
unsigned char serialBuffer[16];                             /* Serial buffer to store uart data */
struct termios orig;                                        /* stores original settings */

int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void read_MD49_Data (void);

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){

    ROS_DEBUG("geometry_msgs/Twist received: linear.x= %f angular.z= %f", vel_cmd.linear.x, vel_cmd.angular.z);

    //ANFANG Alternative
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){vl=0;vr=0;}
    else if(vel_cmd.linear.x == 0){
        // turning
        vr = vel_cmd.angular.z * base_width / 2.0;
        vl = (-1) * vr;
    }
    else if(vel_cmd.angular.z == 0){
        // forward / backward
        vl = vr = vel_cmd.linear.x;
    }
    else{
        // moving doing arcs
        vl = vel_cmd.linear.x - vel_cmd.angular.z * base_width / 2.0;
        if (vl > max_vl) {vl=max_vl;}
        if (vl < min_vl) {vl=min_vl;}
        vr = vel_cmd.linear.x + vel_cmd.angular.z * base_width / 2.0;
        if (vr > max_vr) {vr=max_vr;}
        if (vr < min_vr) {vr=min_vr;}
    }
    //ENDE Alternative

    if (vel_cmd.linear.x>0){
        serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 255;					// speed1
        serialBuffer[3] = 255;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.linear.x<0){
        serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 0;					// speed1
        serialBuffer[3] = 0;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
        serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 128;					// speed1
        serialBuffer[3] = 128;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.angular.z>0){
        serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 0;					// speed1
        serialBuffer[3] = 255;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.angular.z<0){
        serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 255;					// speed1
        serialBuffer[3] = 0;					// speed2
        writeBytes(fd, 4);
    }
}

int main( int argc, char* argv[] ){

    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 100, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<base_controller::encoders>("encoders",100);

    // Open serial port
    // ****************
    filedesc = openSerialPort("/dev/ttyAMA0", B38400);
    if (filedesc == -1) exit(1);
    usleep(40000);                                      // Sleep for UART to power up and set options
    ROS_DEBUG("Serial Port opened \n");

    // Set nodes looprate 10Hz
    // ***********************
    ros::Rate loop_rate(5);

    while( n.ok() )
    {
            // Read encoder and other data from MD49
            // *************************************
            read_MD49_Data();

            // Publish encoder values to topic /encoders (custom message)
            // ********************************************************************
            base_controller::encoders encoders;
            encoders.encoder_l=EncoderL;
            encoders.encoder_r=EncoderR;
            encoders_pub.publish(encoders);

            // Loop
            // ****
            ros::spinOnce();
            loop_rate.sleep();
    }// end.mainloop

    return 1;
} // end.main

int openSerialPort(const char * device, int bps){
   struct termios neu;
   char buf[128];

   fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig); 						/* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      //fprintf(stderr, "speed=%d\n", bps);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcsetattr(fd, TCSANOW, &neu); 				/* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {	// Send data out
        perror("Error writing");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
    //write(fd,serialBuffer, count);

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {	// Read back data into buf[]
        perror("Error reading ");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
}

void read_MD49_Data (void){
    serialBuffer[0] = 82;							// 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);
    EncoderL = serialBuffer[0] << 24;                        // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                        // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

    printf("\033[2J");        /*  clear the screen  */
    printf("\033[H");         /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("====================================================== \n");
    //printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    //printf("Byte2: %i ",serialBuffer[1]);
    //printf("Byte3: % i ",serialBuffer[2]);
    //printf("Byte4: %i \n",serialBuffer[3]);
    //printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    //printf("Byte2: %i ",serialBuffer[5]);
    //printf("Byte3: %i ",serialBuffer[6]);
    //printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("====================================================== \n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);

    printf("vl= %f \n", vl);
    printf("vr= %f \n", vr);

}
<commit_msg>Update code<commit_after>#include <iostream>                                         /* allows to perform standard input and output operations */
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
//#include <string>                                         /* String function definitions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <errno.h>                                          /* Error number definitions */
#include <termios.h>                                        /* POSIX terminal control definitions */
//#include <ctype.h>                                        /* isxxx() */
#include <ros/ros.h>                                        /* ROS */
#include <geometry_msgs/Twist.h>                            /* ROS Twist message */
#include <base_controller/encoders.h>                       /* Custom message /encoders */

const char* serialport="/dev/ttyAMA0";                      /* defines used serialport */
int serialport_bps=B38400;                                  /* defines baudrate od serialport */
int16_t EncoderL;                                           /* stores encoder value left read from md49 */
int16_t EncoderR;                                           /* stores encoder value right read from md49 */
int speed_l=128, speed_r=128;                               /* speed to set for MD49 */
bool cmd_vel_received=true;
double vr = 0.0;
double vl = 0.0;
double max_vr = 0.2;
double max_vl = 0.2;
double min_vr = 0.2;
double min_vl = 0.2;
double base_width = 0.4;                                    /* Base width in meters */

int filedesc;                                               /* filedescriptor serialport */
int fd;                                                     /* serial port file descriptor */
unsigned char serialBuffer[16];                             /* Serial buffer to store uart data */
struct termios orig;                                        /* stores original settings */

int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void read_MD49_Data (void);
void set_MD49_speed (int speed_l, int speed_r);

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){

    ROS_DEBUG("geometry_msgs/Twist received: linear.x= %f angular.z= %f", vel_cmd.linear.x, vel_cmd.angular.z);

    //ANFANG Alternative
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){vl=0;vr=0;}
    else if(vel_cmd.linear.x == 0){
        // turning
        vr = vel_cmd.angular.z * base_width / 2.0;
        vl = (-1) * vr;
    }
    else if(vel_cmd.angular.z == 0){
        // forward / backward
        vl = vr = vel_cmd.linear.x;
    }
    else{
        // moving doing arcs
        vl = vel_cmd.linear.x - vel_cmd.angular.z * base_width / 2.0;
        if (vl > max_vl) {vl=max_vl;}
        if (vl < min_vl) {vl=min_vl;}
        vr = vel_cmd.linear.x + vel_cmd.angular.z * base_width / 2.0;
        if (vr > max_vr) {vr=max_vr;}
        if (vr < min_vr) {vr=min_vr;}
    }
    //ENDE Alternative

    if (vel_cmd.linear.x>0){
        speed_l = 255;
        speed_r = 255;
        //serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
        //serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
        //serialBuffer[2] = 255;					// speed1
        //serialBuffer[3] = 255;					// speed2
        //writeBytes(fd, 4);
    }
    if (vel_cmd.linear.x<0){
        speed_l = 0;
        speed_r = 0;
    }
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
        speed_l = 128;
        speed_r = 128;
    }
    if (vel_cmd.angular.z>0){
        speed_l = 0;
        speed_r = 255;
    }
    if (vel_cmd.angular.z<0){
        speed_l = 255;
        speed_r = 0;
    }

    cmd_vel_received=true;
}

int main( int argc, char* argv[] ){

    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 100, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<base_controller::encoders>("encoders",100);

    // Open serial port
    // ****************
    filedesc = openSerialPort("/dev/ttyAMA0", B38400);
    if (filedesc == -1) exit(1);
    usleep(40000);                                      // Sleep for UART to power up and set options
    ROS_DEBUG("Serial Port opened \n");

    // Set nodes looprate 10Hz
    // ***********************
    ros::Rate loop_rate(10);

    while( n.ok() )
    {
            // Read encoder and other data from MD49
            // *************************************
            read_MD49_Data();

            // Set speed left andright for MD49
            // ********************************
            //if (cmd_vel_received=true) {set_MD49_speed(speed_l,speed_r); cmd_vel_received=false;}
            set_MD49_speed(speed_l,speed_r);

            // Publish encoder values to topic /encoders (custom message)
            // ********************************************************************
            base_controller::encoders encoders;
            encoders.encoder_l=EncoderL;
            encoders.encoder_r=EncoderR;
            encoders_pub.publish(encoders);

            // Loop
            // ****
            ros::spinOnce();
            loop_rate.sleep();
    }// end.mainloop

    return 1;
} // end.main

int openSerialPort(const char * device, int bps){
   struct termios neu;
   char buf[128];

   fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig); 						/* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      //fprintf(stderr, "speed=%d\n", bps);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcsetattr(fd, TCSANOW, &neu); 				/* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {	// Send data out
        perror("Error writing");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
    //write(fd,serialBuffer, count);

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {	// Read back data into buf[]
        perror("Error reading ");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
}

void read_MD49_Data (void){
    serialBuffer[0] = 82;							// 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);
    EncoderL = serialBuffer[0] << 24;                        // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                        // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

    //printf("\033[2J");        /*  clear the screen  */
    printf("\033[H");         /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("====================================================== \n");
    //printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    //printf("Byte2: %i ",serialBuffer[1]);
    //printf("Byte3: % i ",serialBuffer[2]);
    //printf("Byte4: %i \n",serialBuffer[3]);
    //printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    //printf("Byte2: %i ",serialBuffer[5]);
    //printf("Byte3: %i ",serialBuffer[6]);
    //printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("====================================================== \n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);

    printf("vl= %f \n", vl);
    printf("vr= %f \n", vr);
}

void set_MD49_speed (int speed_l, int speed_r){
    serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
    serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
    serialBuffer[2] = speed_l;					// speed1
    serialBuffer[3] = speed_r;					// speed2
    writeBytes(fd, 4);
}
<|endoftext|>


<commit_before>/** @file
    @brief Main file for a unified video-based and inertial tracking system.

    @date 2015

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015 Sensics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Internal Includes
#include "ImageSources/ImageSource.h"
#include "ImageSources/ImageSourceFactories.h"

#include "MakeHDKTrackingSystem.h"
#include "TrackingSystem.h"
#include "ThreadsafeBodyReporting.h"

#include "HDKLedIdentifierFactory.h"
#include "CameraParameters.h"
#include "HDKData.h"
#include <osvr/AnalysisPluginKit/AnalysisPluginKitC.h>
#include <osvr/PluginKit/PluginKit.h>
#include <osvr/PluginKit/TrackerInterfaceC.h>
#include <osvr/PluginKit/AnalogInterfaceC.h>

#include <osvr/Util/EigenInterop.h>

#include "ConfigurationParser.h"

// Generated JSON header file
#include "org_osvr_unifiedvideoinertial_json.h"

// Library/third-party includes
#include <opencv2/core/core.hpp> // for basic OpenCV types
#include <opencv2/core/operations.hpp>
#include <opencv2/highgui/highgui.hpp> // for image capture
#include <opencv2/imgproc/imgproc.hpp> // for image scaling
#include <json/value.h>
#include <json/reader.h>

#include <boost/noncopyable.hpp>

#include <util/Stride.h>

// Standard includes
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <memory>
#include <thread>
#include <mutex>
#include <stdexcept>

// Anonymous namespace to avoid symbol collision
namespace {
static const auto DRIVER_NAME = "UnifiedTrackingSystem";

static const auto DEBUGGABLE_BEACONS = 34;
static const auto DATAPOINTS_PER_BEACON = 5;
using TrackingSystemPtr = std::unique_ptr<osvr::vbtracker::TrackingSystem>;
using BodyReportingVector = std::vector<osvr::vbtracker::BodyReportingPtr>;
class TrackerThread : boost::noncopyable {
  public:
    TrackerThread(osvr::vbtracker::TrackingSystem &trackingSystem,
                  osvr::vbtracker::ImageSource &imageSource,
                  BodyReportingVector &reportingVec,
                  osvr::vbtracker::CameraParameters const &camParams)
        : m_trackingSystem(trackingSystem), m_cam(imageSource),
          m_reportingVec(reportingVec), m_camParams(camParams) {
        msg() << "Tracker thread object created." << std::endl;
    }

    void operator()() {
        msg() << "Tracker thread object invoked." << std::endl;
        bool keepGoing = true;
        while (keepGoing) {
            doFrame();

            {
                /// Copy the run flag.
                std::lock_guard<std::mutex> lock(m_runMutex);
                keepGoing = m_run;
            }
            if (!keepGoing) {
                msg() << "Tracker thread object: Just checked our run flag and "
                         "noticed it turned false..."
                      << std::endl;
            }
        }
        msg() << "Tracker thread object: functor exiting." << std::endl;
    }

    /// Call from the main thread!
    void triggerStop() {
        msg() << "Tracker thread object: triggerStop() called" << std::endl;
        std::lock_guard<std::mutex> lock(m_runMutex);
        m_run = false;
    }

  private:
    std::ostream &msg() const { return std::cout << "[UnifiedTracker] "; }
    std::ostream &warn() const { return msg() << "Warning: "; }
    void doFrame() {
        // Check camera status.
        if (!m_cam.ok()) {
            // Hmm, camera seems bad. Might regain it? Skip for now...
            warn() << "Camera is reporting it is not OK." << std::endl;
            return;
        }
        // Trigger a grab.
        if (!m_cam.grab()) {
            // Again failing without quitting, in hopes we get better luck next
            // time...
            warn() << "Camera grab failed." << std::endl;
            return;
        }
        // When we triggered the grab is our current best guess of the time for
        // the image
        /// @todo backdate to account for image transfer image, exposure time,
        /// etc.
        auto tv = osvr::util::time::getNow();

        // Pull the image into an OpenCV matrix named m_frame.
        m_cam.retrieve(m_frame, m_frameGray);
        if (!m_frame.data || !m_frameGray.data) {
            warn()
                << "Camera retrieve appeared to fail: frames had null pointers!"
                << std::endl;
            return;
        }

        // Submit to the tracking system.
        auto bodyIds = m_trackingSystem.processFrame(tv, m_frame, m_frameGray,
                                                     m_camParams);

        for (auto const &bodyId : bodyIds) {
            auto &body = m_trackingSystem.getBody(bodyId);
            m_reportingVec[bodyId.value()]->updateState(
                body.getStateTime(), body.getState(), body.getProcessModel());
        }
    }
    osvr::vbtracker::TrackingSystem &m_trackingSystem;
    osvr::vbtracker::ImageSource &m_cam;
    BodyReportingVector &m_reportingVec;
    osvr::vbtracker::CameraParameters m_camParams;
    cv::Mat m_frame;
    cv::Mat m_frameGray;
    std::mutex m_runMutex;
    volatile bool m_run = true;
};

class UnifiedVideoInertialTracker : boost::noncopyable {
  public:
    using size_type = std::size_t;
    UnifiedVideoInertialTracker(OSVR_PluginRegContext ctx,
                                osvr::vbtracker::ImageSourcePtr &&source,
                                osvr::vbtracker::ConfigParams params,
                                TrackingSystemPtr &&trackingSystem)
        : m_source(std::move(source)),
          m_trackingSystem(std::move(trackingSystem)),
          m_additionalPrediction(params.additionalPrediction) {
        if (params.numThreads > 0) {
            // Set the number of threads for OpenCV to use.
            cv::setNumThreads(params.numThreads);
        }

        /// Create the initialization options
        OSVR_DeviceInitOptions opts = osvrDeviceCreateInitOptions(ctx);

        // Configure the tracker interface.
        osvrDeviceTrackerConfigure(opts, &m_tracker);
#if 0
        osvrDeviceAnalogConfigure(opts, &m_analog,
                                  DEBUGGABLE_BEACONS * DATAPOINTS_PER_BEACON);
#endif

        /// Create the analysis device token with the options
        OSVR_DeviceToken dev;
        if (OSVR_RETURN_FAILURE ==
            osvrAnalysisSyncInit(ctx, DRIVER_NAME, opts, &dev, &m_clientCtx)) {
            throw std::runtime_error("Could not initialize analysis plugin!");
        }
        m_dev = osvr::pluginkit::DeviceToken(dev);

        /// Send JSON descriptor
        m_dev.sendJsonDescriptor(org_osvr_unifiedvideoinertial_json);

        /// Set up thread communication.
        setupBodyReporting();

        /// Set up the object that will run in the other thread, and spawn the
        /// thread.
        startTrackerThread();

        /// Register update callback
        m_dev.registerUpdateCallback(this);
    }

    ~UnifiedVideoInertialTracker() { stopTrackerThread(); }

    /// Create a "BodyReporting" interchange structure for each body we track.
    void setupBodyReporting() {
        m_bodyReportingVector.clear();
        auto n = m_trackingSystem->getNumBodies();
        for (size_type i = 0; i < n; ++i) {
            m_bodyReportingVector.emplace_back(
                osvr::vbtracker::BodyReporting::make());
        }
    }

    OSVR_ReturnCode update();

    void startTrackerThread() {
        if (m_trackerThreadFunctor) {
            throw std::logic_error("Trying to start the tracker thread when "
                                   "it's already started!");
        }
        std::cout << "Starting the tracker thread..." << std::endl;
        m_trackerThreadFunctor.reset(new TrackerThread(
            *m_trackingSystem, *m_source, m_bodyReportingVector,
            osvr::vbtracker::getHDKCameraParameters()));
        m_trackerThread = std::thread([&] { (*m_trackerThreadFunctor)(); });
    }
    void stopTrackerThread() {
        if (m_trackerThreadFunctor) {
            std::cout << "Shutting down the tracker thread..." << std::endl;
            m_trackerThreadFunctor->triggerStop();
            m_trackerThread.join();
            m_trackerThreadFunctor.reset();
            m_trackerThread = std::thread();
        }
    }

  private:
    osvr::pluginkit::DeviceToken m_dev;
    OSVR_ClientContext m_clientCtx;
    OSVR_ClientInterface m_clientInterface;
    OSVR_TrackerDeviceInterface m_tracker;
#if 0
    OSVR_AnalogDeviceInterface m_analog;
#endif
    osvr::vbtracker::ImageSourcePtr m_source;
    cv::Mat m_frame;
    cv::Mat m_imageGray;
    TrackingSystemPtr m_trackingSystem;
    const double m_additionalPrediction;
    std::vector<osvr::vbtracker::BodyReportingPtr> m_bodyReportingVector;
    std::unique_ptr<TrackerThread> m_trackerThreadFunctor;
    std::thread m_trackerThread;
};

inline OSVR_ReturnCode UnifiedVideoInertialTracker::update() {
    namespace ei = osvr::util::eigen_interop;
    std::size_t numSensors = m_bodyReportingVector.size();
    for (std::size_t i = 0; i < numSensors; ++i) {
        auto report =
            m_bodyReportingVector[i]->getReport(m_additionalPrediction);
        if (!report) {
            /// couldn't get a report for this sensor for one reason or another.
            // std::cout << "Couldn't get report for " << i << std::endl;
            continue;
        }
        osvrDeviceTrackerSendPoseTimestamped(m_dev, m_tracker, &report.pose, i,
                                             &report.timestamp);
    }
    return OSVR_RETURN_SUCCESS;
}

class ConfiguredDeviceConstructor {
  public:
    /// @brief This is the required signature for a device instantiation
    /// callback.
    OSVR_ReturnCode operator()(OSVR_PluginRegContext ctx, const char *params) {
        // Read the JSON data from parameters.
        Json::Value root;
        if (params) {
            Json::Reader r;
            if (!r.parse(params, root)) {
                std::cerr << "Could not parse parameters!" << std::endl;
            }
        }

        // Read these parameters from a "params" field in the device Json
        // configuration file.

        // This is in a separate function/header for sharing and for clarity.
        auto config = osvr::vbtracker::parseConfigParams(root);

#ifdef _WIN32
        auto cam = osvr::vbtracker::openHDKCameraDirectShow();
#else // !_WIN32
        /// @todo This is rather crude, as we can't select the exact camera we
        /// want, nor set the "50Hz" high-gain mode (and only works with HDK
        /// camera firmware v7 and up). Presumably eventually use libuvc on
        /// other platforms instead, at least for the HDK IR camera.

        auto cam = osvr::vbtracker::openOpenCVCamera(0);
#endif

        if (!cam || !cam->ok()) {
            std::cerr << "Could not access the tracking camera, skipping "
                         "video-based tracking!"
                      << std::endl;
            return OSVR_RETURN_FAILURE;
        }

        auto trackingSystem = osvr::vbtracker::makeHDKTrackingSystem(config);
        // OK, now that we have our parameters, create the device.
        osvr::pluginkit::PluginContext context(ctx);
        auto newTracker = osvr::pluginkit::registerObjectForDeletion(
            ctx, new UnifiedVideoInertialTracker(ctx, std::move(cam), config,
                                                 std::move(trackingSystem)));

        return OSVR_RETURN_SUCCESS;
    }
};

} // namespace

OSVR_PLUGIN(org_osvr_unifiedvideoinertial) {
    osvr::pluginkit::PluginContext context(ctx);

    /// Tell the core we're available to create a device object.
    osvr::pluginkit::registerDriverInstantiationCallback(
        ctx, DRIVER_NAME, new ConfiguredDeviceConstructor);

    return OSVR_RETURN_SUCCESS;
}
<commit_msg>Stub in the async<commit_after>/** @file
    @brief Main file for a unified video-based and inertial tracking system.

    @date 2015

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015 Sensics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Internal Includes
#include "ImageSources/ImageSource.h"
#include "ImageSources/ImageSourceFactories.h"

#include "MakeHDKTrackingSystem.h"
#include "TrackingSystem.h"
#include "ThreadsafeBodyReporting.h"

#include "HDKLedIdentifierFactory.h"
#include "CameraParameters.h"
#include "HDKData.h"
#include <osvr/AnalysisPluginKit/AnalysisPluginKitC.h>
#include <osvr/PluginKit/PluginKit.h>
#include <osvr/PluginKit/TrackerInterfaceC.h>
#include <osvr/PluginKit/AnalogInterfaceC.h>

#include <osvr/Util/EigenInterop.h>

#include "ConfigurationParser.h"

// Generated JSON header file
#include "org_osvr_unifiedvideoinertial_json.h"

// Library/third-party includes
#include <opencv2/core/core.hpp> // for basic OpenCV types
#include <opencv2/core/operations.hpp>
#include <opencv2/highgui/highgui.hpp> // for image capture
#include <opencv2/imgproc/imgproc.hpp> // for image scaling
#include <json/value.h>
#include <json/reader.h>

#include <boost/noncopyable.hpp>

#include <util/Stride.h>

// Standard includes
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <memory>
#include <thread>
#include <mutex>
#include <stdexcept>
#include <future>

// Anonymous namespace to avoid symbol collision
namespace {
static const auto DRIVER_NAME = "UnifiedTrackingSystem";

static const auto DEBUGGABLE_BEACONS = 34;
static const auto DATAPOINTS_PER_BEACON = 5;
using TrackingSystemPtr = std::unique_ptr<osvr::vbtracker::TrackingSystem>;
using BodyReportingVector = std::vector<osvr::vbtracker::BodyReportingPtr>;
class TrackerThread : boost::noncopyable {
  public:
    TrackerThread(osvr::vbtracker::TrackingSystem &trackingSystem,
                  osvr::vbtracker::ImageSource &imageSource,
                  BodyReportingVector &reportingVec,
                  osvr::vbtracker::CameraParameters const &camParams)
        : m_trackingSystem(trackingSystem), m_cam(imageSource),
          m_reportingVec(reportingVec), m_camParams(camParams) {
        msg() << "Tracker thread object created." << std::endl;
    }

    void operator()() {
        msg() << "Tracker thread object invoked." << std::endl;
        bool keepGoing = true;
        while (keepGoing) {
            doFrame();

            {
                /// Copy the run flag.
                std::lock_guard<std::mutex> lock(m_runMutex);
                keepGoing = m_run;
            }
            if (!keepGoing) {
                msg() << "Tracker thread object: Just checked our run flag and "
                         "noticed it turned false..."
                      << std::endl;
            }
        }
        msg() << "Tracker thread object: functor exiting." << std::endl;
    }

    /// Call from the main thread!
    void triggerStop() {
        msg() << "Tracker thread object: triggerStop() called" << std::endl;
        std::lock_guard<std::mutex> lock(m_runMutex);
        m_run = false;
    }

  private:
    std::ostream &msg() const { return std::cout << "[UnifiedTracker] "; }
    std::ostream &warn() const { return msg() << "Warning: "; }
    void doFrame() {
        // Check camera status.
        if (!m_cam.ok()) {
            // Hmm, camera seems bad. Might regain it? Skip for now...
            warn() << "Camera is reporting it is not OK." << std::endl;
            return;
        }
        // Trigger a grab.
        if (!m_cam.grab()) {
            // Again failing without quitting, in hopes we get better luck next
            // time...
            warn() << "Camera grab failed." << std::endl;
            return;
        }
        // When we triggered the grab is our current best guess of the time for
        // the image
        /// @todo backdate to account for image transfer image, exposure time,
        /// etc.
        auto tv = osvr::util::time::getNow();

        // Pull the image into an OpenCV matrix named m_frame.
        m_cam.retrieve(m_frame, m_frameGray);
        if (!m_frame.data || !m_frameGray.data) {
            warn()
                << "Camera retrieve appeared to fail: frames had null pointers!"
                << std::endl;
            return;
        }

        /// Launch an asynchronous task to perform the initial image processing.
        auto imageProcFuture = std::async(
            std::launch::async, [&]() -> osvr::vbtracker::ImageOutputDataPtr {
                return m_trackingSystem.performInitialImageProcessing(
                    tv, m_frame, m_frameGray, m_camParams);
            });

        /// @todo handle IMU reports in here

        /// By calling .get() on the std::future we block on the async we
        /// launched.
        // Submit to the tracking system.
        auto bodyIds =
            m_trackingSystem.updateBodiesFromVideoData(imageProcFuture.get());

        for (auto const &bodyId : bodyIds) {
            auto &body = m_trackingSystem.getBody(bodyId);
            m_reportingVec[bodyId.value()]->updateState(
                body.getStateTime(), body.getState(), body.getProcessModel());
        }
    }
    osvr::vbtracker::TrackingSystem &m_trackingSystem;
    osvr::vbtracker::ImageSource &m_cam;
    BodyReportingVector &m_reportingVec;
    osvr::vbtracker::CameraParameters m_camParams;
    cv::Mat m_frame;
    cv::Mat m_frameGray;
    std::mutex m_runMutex;
    bool m_run = true;
};

class UnifiedVideoInertialTracker : boost::noncopyable {
  public:
    using size_type = std::size_t;
    UnifiedVideoInertialTracker(OSVR_PluginRegContext ctx,
                                osvr::vbtracker::ImageSourcePtr &&source,
                                osvr::vbtracker::ConfigParams params,
                                TrackingSystemPtr &&trackingSystem)
        : m_source(std::move(source)),
          m_trackingSystem(std::move(trackingSystem)),
          m_additionalPrediction(params.additionalPrediction) {
        if (params.numThreads > 0) {
            // Set the number of threads for OpenCV to use.
            cv::setNumThreads(params.numThreads);
        }

        /// Create the initialization options
        OSVR_DeviceInitOptions opts = osvrDeviceCreateInitOptions(ctx);

        // Configure the tracker interface.
        osvrDeviceTrackerConfigure(opts, &m_tracker);
#if 0
        osvrDeviceAnalogConfigure(opts, &m_analog,
                                  DEBUGGABLE_BEACONS * DATAPOINTS_PER_BEACON);
#endif

        /// Create the analysis device token with the options
        OSVR_DeviceToken dev;
        if (OSVR_RETURN_FAILURE ==
            osvrAnalysisSyncInit(ctx, DRIVER_NAME, opts, &dev, &m_clientCtx)) {
            throw std::runtime_error("Could not initialize analysis plugin!");
        }
        m_dev = osvr::pluginkit::DeviceToken(dev);

        /// Send JSON descriptor
        m_dev.sendJsonDescriptor(org_osvr_unifiedvideoinertial_json);

        /// Set up thread communication.
        setupBodyReporting();

        /// Set up the object that will run in the other thread, and spawn the
        /// thread.
        startTrackerThread();

        /// Register update callback
        m_dev.registerUpdateCallback(this);
    }

    ~UnifiedVideoInertialTracker() { stopTrackerThread(); }

    /// Create a "BodyReporting" interchange structure for each body we track.
    void setupBodyReporting() {
        m_bodyReportingVector.clear();
        auto n = m_trackingSystem->getNumBodies();
        for (size_type i = 0; i < n; ++i) {
            m_bodyReportingVector.emplace_back(
                osvr::vbtracker::BodyReporting::make());
        }
    }

    OSVR_ReturnCode update();

    void startTrackerThread() {
        if (m_trackerThreadFunctor) {
            throw std::logic_error("Trying to start the tracker thread when "
                                   "it's already started!");
        }
        std::cout << "Starting the tracker thread..." << std::endl;
        m_trackerThreadFunctor.reset(new TrackerThread(
            *m_trackingSystem, *m_source, m_bodyReportingVector,
            osvr::vbtracker::getHDKCameraParameters()));
        m_trackerThread = std::thread([&] { (*m_trackerThreadFunctor)(); });
    }
    void stopTrackerThread() {
        if (m_trackerThreadFunctor) {
            std::cout << "Shutting down the tracker thread..." << std::endl;
            m_trackerThreadFunctor->triggerStop();
            m_trackerThread.join();
            m_trackerThreadFunctor.reset();
            m_trackerThread = std::thread();
        }
    }

  private:
    osvr::pluginkit::DeviceToken m_dev;
    OSVR_ClientContext m_clientCtx;
    OSVR_ClientInterface m_clientInterface;
    OSVR_TrackerDeviceInterface m_tracker;
#if 0
    OSVR_AnalogDeviceInterface m_analog;
#endif
    osvr::vbtracker::ImageSourcePtr m_source;
    cv::Mat m_frame;
    cv::Mat m_imageGray;
    TrackingSystemPtr m_trackingSystem;
    const double m_additionalPrediction;
    std::vector<osvr::vbtracker::BodyReportingPtr> m_bodyReportingVector;
    std::unique_ptr<TrackerThread> m_trackerThreadFunctor;
    std::thread m_trackerThread;
};

inline OSVR_ReturnCode UnifiedVideoInertialTracker::update() {
    namespace ei = osvr::util::eigen_interop;
    std::size_t numSensors = m_bodyReportingVector.size();
    for (std::size_t i = 0; i < numSensors; ++i) {
        auto report =
            m_bodyReportingVector[i]->getReport(m_additionalPrediction);
        if (!report) {
            /// couldn't get a report for this sensor for one reason or another.
            // std::cout << "Couldn't get report for " << i << std::endl;
            continue;
        }
        osvrDeviceTrackerSendPoseTimestamped(m_dev, m_tracker, &report.pose, i,
                                             &report.timestamp);
    }
    return OSVR_RETURN_SUCCESS;
}

class ConfiguredDeviceConstructor {
  public:
    /// @brief This is the required signature for a device instantiation
    /// callback.
    OSVR_ReturnCode operator()(OSVR_PluginRegContext ctx, const char *params) {
        // Read the JSON data from parameters.
        Json::Value root;
        if (params) {
            Json::Reader r;
            if (!r.parse(params, root)) {
                std::cerr << "Could not parse parameters!" << std::endl;
            }
        }

        // Read these parameters from a "params" field in the device Json
        // configuration file.

        // This is in a separate function/header for sharing and for clarity.
        auto config = osvr::vbtracker::parseConfigParams(root);

#ifdef _WIN32
        auto cam = osvr::vbtracker::openHDKCameraDirectShow();
#else // !_WIN32
        /// @todo This is rather crude, as we can't select the exact camera we
        /// want, nor set the "50Hz" high-gain mode (and only works with HDK
        /// camera firmware v7 and up). Presumably eventually use libuvc on
        /// other platforms instead, at least for the HDK IR camera.

        auto cam = osvr::vbtracker::openOpenCVCamera(0);
#endif

        if (!cam || !cam->ok()) {
            std::cerr << "Could not access the tracking camera, skipping "
                         "video-based tracking!"
                      << std::endl;
            return OSVR_RETURN_FAILURE;
        }

        auto trackingSystem = osvr::vbtracker::makeHDKTrackingSystem(config);
        // OK, now that we have our parameters, create the device.
        osvr::pluginkit::PluginContext context(ctx);
        auto newTracker = osvr::pluginkit::registerObjectForDeletion(
            ctx, new UnifiedVideoInertialTracker(ctx, std::move(cam), config,
                                                 std::move(trackingSystem)));

        return OSVR_RETURN_SUCCESS;
    }
};

} // namespace

OSVR_PLUGIN(org_osvr_unifiedvideoinertial) {
    osvr::pluginkit::PluginContext context(ctx);

    /// Tell the core we're available to create a device object.
    osvr::pluginkit::registerDriverInstantiationCallback(
        ctx, DRIVER_NAME, new ConfiguredDeviceConstructor);

    return OSVR_RETURN_SUCCESS;
}
<|endoftext|>