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

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

// Gnuradio includes ...
// ---------------------
#include <gnuradio/io_signature.h>
#include <gnuradio/blocks/pdu.h>
#include <gnuradio/thread/thread.h>
#include <gnuradio/digital/crc32.h>
#include <gnuradio/logger.h>

// Boost includes ...
// ------------------
#include <boost/beast/core.hpp>
#include <boost/beast/http.hpp>
#include <boost/beast/version.hpp>
#include <boost/asio/connect.hpp>
#include <boost/asio/ip/tcp.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <boost/date_time/posix_time/posix_time.hpp>

// System includes ...
// -------------------
#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <regex>
#include <vector>
#include <iomanip>
#include <fstream>

#include <sys/time.h>

// Gnuradio modules include ...
// ----------------------------
#include "http_transfer_sink_impl.h"

// Define constants ...
// --------------------
#define CONST_WRITE_BUFFER_SIZE 10
#define CONST_SLEEP_INTERVAL_MILI_SECONDS 1000

#define CONST_DEBUG
#define CONST_DEBUG_HTTP

/*
 https://github.com/boostorg/boost_install/issues/12
 Stops looking for BoostConfig.cmake e.g. in /opt/boost_1_76_0/lib/cmake/Boost-1.76.0/BoostConfig.cmake
 cmake -DBoost_NO_BOOST_CMAKE=ON ../

 export TARGET_BOOST=/opt/boost_1_76_0
 export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/boost_1_76_0/lib
 cmake -DBoost_NO_BOOST_CMAKE=TRUE -DBoost_NO_SYSTEM_PATHS=TRUE -DBOOST_ROOT:PATHNAME=$TARGET_BOOST -DBoost_LIBRARY_DIRS:FILEPATH=${TARGET_BOOST}/lib ../

 ldd /home/pavelf/gr3.8/lib/libgnuradio-gsSDR.so
*/

// #define CONST_BOOST_1_7

namespace gr {
  namespace gsSDR {
    // Short alias for this namespace ...
    // ----------------------------------
    namespace beast = boost::beast;     // from <boost/beast.hpp>
    namespace http = beast::http;       // from <boost/beast/http.hpp>
    namespace net = boost::asio;        // from <boost/asio.hpp>
#ifdef CONST_BOOST_1_7
    using tcp = net::ip::tcp;           // from <boost/asio/ip/tcp.hpp>
#else
    using tcp = boost::asio::ip::tcp;
#endif

    namespace pt = boost::property_tree;
    using namespace boost::posix_time;

    using namespace std;

    // std long to string conversion ...
    static std::string long_to_string(long value) {
        std::ostringstream os;
        os << value;
        return os.str();
    }

    // create_json_string function ...
    static std::string create_json_string(pt::ptree oroot_){

        std::ostringstream buf;
        write_json (buf, oroot_, false);
        std::string json = buf.str();

        std::regex reg("\\\"((?:0[xX])?[0-9a-fA-F]+)\\\"");                             // std::regex reg("\\\"([0-9]+\\.{0,1}[0-9]*)\\\"");
        std::string json_corection_1 = std::regex_replace(json, reg, "$1");
        std::regex reg_2("\\\"(true|false)\\\"");
        std::string json_corection_2 = std::regex_replace(json_corection_1, reg_2, "$1");

        // Erase redundant double quotes ...
        boost::erase_all(json_corection_2, "\\\"");

#ifdef CONST_DEBUG
        std::cout<<json_corection_2<<std::endl;
#endif
        return json_corection_2;
    }

    // Define static variables ...
    // ---------------------------
    fifo_buffer http_transfer_sink_impl::m_fifo;
    bool http_transfer_sink_impl::m_exit_requested;

    http_transfer_sink::sptr
    http_transfer_sink::make(const std::string &ServerName, const std::string &ServerPort,const std::string &ServerTarget, int stationId, int spacecraftId, const std::string &UserName, const std::string &UserPass)
    {
      return gnuradio::get_initial_sptr
        (new http_transfer_sink_impl(ServerName, ServerPort, ServerTarget, stationId, spacecraftId, UserName, UserPass));
    }

    /*
     * The private constructor ...
     * ---------------------------
     */
    http_transfer_sink_impl::http_transfer_sink_impl(const std::string &ServerName, const std::string &ServerPort,const std::string &ServerTarget, int stationId, int spacecraftId, const std::string &UserName, const std::string &UserPass)
      : gr::block("http_transfer_sink",
              gr::io_signature::make(0,0,0),    // <+MIN_IN+>, <+MAX_IN+>, sizeof(<+ITYPE+>)
              gr::io_signature::make(0,0,0)),   // <+MIN_OUT+>, <+MAX_OUT+>, sizeof(<+OTYPE+>)
              m_ServerName(ServerName), m_ServerPort(ServerPort),m_ServerTarget(ServerTarget), m_stationId(stationId), m_spacecraftId(spacecraftId), m_UserName(UserName), m_UserPass(UserPass)
    {

       m_packetCounter = 0; m_exit_requested = false;
       m_fifo.fifo_changeSize(32);

       // Register PDU input port ...
       // ---------------------------
       message_port_register_in(pmt::mp("in"));
       set_msg_handler(pmt::mp("in"), boost::bind(&http_transfer_sink_impl::packet_handler, this, boost::placeholders::_1)); // _1

       /* Create read stream THREAD [http://antonym.org/2009/05/threading-with-boost---part-i-creating-threads.html]
          and [http://antonym.org/2010/01/threading-with-boost---part-ii-threading-challenges.html]
       */
#ifdef CONST_DEBUG
     cout << "Threading - using up to CPUs/Cores: "<< boost::thread::hardware_concurrency() <<endl;
#endif
     _thread = gr::thread::thread(&http_transfer_sink_impl::http_transfer_sink_wait,this);

    }

    /*
     * Our virtual destructor ...
     * --------------------------
     */
    http_transfer_sink_impl::~http_transfer_sink_impl(){

#ifdef CONST_DEBUG
      cout<< "Calling destructor - http_transfer_sink_impl ... "<< endl;
#endif
      threadTransferDeInit(); // De-init  ...

#ifdef CONST_DEBUG
      cout<< "Calling destructor - http_transfer_sink_impl - EnDL... "<< endl;
#endif

    }

    bool http_transfer_sink_impl::stop(){

        // Only for debug  ...
        // -------------------
        cout<< "Calling STOP - http_transfer_sink_impl ... "<< endl;

        threadTransferDeInit(); // De-init  ...

        return true;
    }

    // threadTransferDeInit function [private] ...
    void http_transfer_sink_impl::threadTransferDeInit(){

        gr::thread::scoped_lock lock(fp_mutex);   // shared resources ...
        m_exit_requested = true;
        lock.unlock();

        if(_thread.joinable()) _thread.join();
    }

    void http_transfer_sink_impl::forecast (int noutput_items, gr_vector_int &ninput_items_required){
         /* <+forecast+> e.g. ninput_items_required[0] = noutput_items */
    }

    int http_transfer_sink_impl::general_work (int noutput_items,
                       gr_vector_int &ninput_items,
                       gr_vector_const_void_star &input_items,
                       gr_vector_void_star &output_items){
      /*
      const <+ITYPE+> *in = (const <+ITYPE+> *) input_items[0];
      <+OTYPE+> *out = (<+OTYPE+> *) output_items[0];
      */

      // Do <+signal processing+>
      // ------------------------
      // Tell runtime system how many input items we consumed on
      // each input stream.
      consume_each (noutput_items);

      // Tell runtime system how many output items we produced.
      return noutput_items;
    }

    // http_transfer_sink_impl function [provides new THREAD] - [private /* static */] ...
    void http_transfer_sink_impl::http_transfer_sink_wait(){

            int niutput_items_real = 0;
            bool data_sent_success = true;
            gr_storage_web buffer_write_storage[CONST_WRITE_BUFFER_SIZE];

#ifdef CONST_DEBUG
            cout << "http_transfer_sink_wait SDR processing THREAD was started ..." << endl;
#endif
            if(m_exit_requested){
#ifdef CONST_DEBUG
            cout<< "http_transfer_sink_wait processing THREAD ends - clearing resources ... "<< endl;
#endif
            return;
            }

            // ---------------------
            // HTTP post request ...
            // ---------------------
            http::request<http::string_body> req;
            req.version(11);    // Default 11 ..
            req.method(http::verb::post);
            req.target(m_ServerTarget);
            req.set(http::field::host, m_ServerName);
            req.set(http::field::content_type, "application/json");

            // The io_context is required for all I/O ...
            // ------------------------------------------
            net::io_context ioc;

            // These objects perform our I/O ...
            // ---------------------------------
#ifdef CONST_BOOST_1_7
            tcp::resolver resolver(ioc);
            beast::tcp_stream stream(ioc);
#else
            tcp::resolver resolver{ioc};
            tcp::socket stream{ioc};
#endif

            // This buffer is used for reading and must be persisted ...
            beast::flat_buffer buffer;

            // Declare a container to hold the response ...
            http::response<http::dynamic_body> res;

            // While TRUE loop ...
            // -------------------
            while(true){
             if(data_sent_success) niutput_items_real = m_fifo.fifo_read_storage(buffer_write_storage, CONST_WRITE_BUFFER_SIZE);

             if(niutput_items_real > 0){

                try{
                    // Look up the domain name ...
                    auto const results = resolver.resolve(m_ServerName, m_ServerPort);

                    // Make the connection on the IP address we get from a lookup ...
#ifdef CONST_BOOST_1_7
                    stream.connect(results);
#else
                    boost::asio::connect(stream, results.begin(), results.end());
#endif

                    // Send JSON data ...
                    // ------------------
                    for(int i=0;i<niutput_items_real;i++){
                        // Set HTTP body ...
                        req.body() = buffer_write_storage[i].getString();
                        req.prepare_payload();

                        // Send the HTTP request to the remote host
                        http::write(stream, req);

                        // Receive the HTTP response
                        http::read(stream, buffer, res);

                        if(res.result_int() == 200){
                           data_sent_success = true;
#ifdef CONST_DEBUG_HTTP
                           std::cout<<"Packet was sent succesfully: "<<res.result()<<std::endl;
#endif
                        }else{
                          data_sent_success = false;
#ifdef CONST_DEBUG_HTTP
                           std::cout<<"Packet was not sent succesfully - error: "<<res.result()<<std::endl;
#endif

                        }
                    }

                    // Gracefully close the socket ...
                    beast::error_code ec;

#ifdef CONST_BOOST_1_7
                    stream.socket().shutdown(tcp::socket::shutdown_both, ec);
#else
                    stream.shutdown(tcp::socket::shutdown_both, ec);
#endif
                    // Not_connected happens sometimes, so don't bother reporting it ...
                    if(ec && ec != beast::errc::not_connected) throw beast::system_error{ec};

                }

                catch (boost::system::system_error const& e){
                    std::cout << "Warning: could not connect : " << e.what() << std::endl;
                }

                catch(std::exception const& e){
                    std::cerr << "Error: " << e.what() << std::endl; // return EXIT_FAILURE;
                }
            }

            // Waiting for data ...
            // --------------------
            boost::this_thread::sleep(boost::posix_time::milliseconds(CONST_SLEEP_INTERVAL_MILI_SECONDS));

            gr::thread::scoped_lock lock(fp_mutex);   // shared resources ...
            if(m_exit_requested) break;               // shared global variable - exit requested if m_exit_requested == TRUE --> break ...
        }
    }

    // packet_handler - public function / callback ...
    // -----------------------------------------------
    void http_transfer_sink_impl::packet_handler(pmt::pmt_t msg){

        int packetSize = 0;

        // Extract payload ...
        // -------------------
        vector<unsigned char> data_uint8(pmt::u8vector_elements(pmt::cdr(msg)));
        packetSize = data_uint8.size();

#ifdef CONST_DEBUG
        cout<<"Packet size: "<<packetSize<<endl;
#endif

        // Extract meta information / verbose mode ...
        // -------------------------------------------
        pmt::pmt_t meta = pmt::car(msg);

        if(pmt::dict_has_key(meta, pmt::string_to_symbol("syncword"))) {
           pmt::pmt_t r = pmt::dict_ref(meta, pmt::string_to_symbol("syncword"), pmt::PMT_NIL);
#ifdef CONST_DEBUG
           cout<<"Syncword: "<<pmt::to_long(r)<<endl;
#endif
        }

        // Create a ptree (json) root ...
        // ------------------------------
        pt::ptree oroot;

        // Create JSON ...
        // ---------------
        oroot.put("groundStation", 0); struct timeval tv; gettimeofday(&tv, NULL);       // ptime t = microsec_clock::universal_time();
        oroot.put("timestamp", long_to_string(tv.tv_sec * 1000 + tv.tv_usec / 1000));    // to_iso_extended_string(t)+"Z";
        oroot.put("crcOK",true);
        oroot.put("packetNumber",1);
        oroot.put("packetLength", packetSize);

        std::ostringstream ss;
        ss << std::hex << std::uppercase << std::setfill('0');
        std::for_each(data_uint8.cbegin(), data_uint8.cend(), [&](int c) {ss << std::setw(2) << c; });
        std::string dataResult; dataResult.append("\"");/* dataResult.append("0x");*/  dataResult.append(ss.str()); dataResult.append("\"");

#ifdef CONST_DEBUG
        cout<<"DataResult: "<<dataResult<<endl;

        // Timestamp --> to String conversion ...
        // --------------------------------------
        boost::posix_time::ptime my_posix_time = boost::posix_time::second_clock::local_time();
        std::string iso_time_string = boost::posix_time::to_iso_extended_string(my_posix_time);

        std::ofstream sinkDebugFile("/tmp/http_sink_packet_debug.txt",std::ios::out | std::ios::app);

        if(sinkDebugFile.is_open()){
           sinkDebugFile <<"Timestamp: "<< iso_time_string << std::endl;
           sinkDebugFile << dataResult << std::endl;
           sinkDebugFile.close();
        }
#endif

        oroot.put("payload", dataResult);
        oroot.put("spacecraftId", m_spacecraftId);

        // Add to circular buffer ...
        // --------------------------
        gr_storage_web storage = gr_storage_web(0,create_json_string(oroot));
        m_fifo.fifo_write_storage(&storage, 1);

    }

  } /* namespace gsSDR */
} /* namespace gr */

max_stars_repo_name: pavelfpl/gr-gsSDR

max_issues_repo_name: pavelfpl/gr-gsSDR

max_forks_repo_name: pavelfpl/gr-gsSDR


text:
/*  LOOT

    A load order optimisation tool for
    Morrowind, Oblivion, Skyrim, Skyrim Special Edition, Skyrim VR,
    Fallout 3, Fallout: New Vegas, Fallout 4 and Fallout 4 VR.

    Copyright (C) 2012 WrinklyNinja

    This file is part of LOOT.

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

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

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

#include "gui/state/game/game.h"

#include <algorithm>
#include <cmath>
#include <fstream>
#include <thread>

#ifdef _WIN32
#ifndef UNICODE
#define UNICODE
#endif
#ifndef _UNICODE
#define _UNICODE
#endif
#ifndef NOMINMAX
#define NOMINMAX
#endif
#include <shlobj.h>
#include <shlwapi.h>
#include <windows.h>
#endif

#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/locale.hpp>

#include "gui/helpers.h"
#include "gui/state/game/game_detection_error.h"
#include "gui/state/game/helpers.h"
#include "gui/state/logging.h"
#include "loot/exception/file_access_error.h"
#include "loot/exception/undefined_group_error.h"

using std::list;
using std::lock_guard;
using std::mutex;
using std::string;
using std::thread;
using std::vector;
using std::filesystem::u8path;

namespace fs = std::filesystem;

namespace loot {
namespace gui {
bool hasPluginFileExtension(const std::string& filename) {
  return boost::iends_with(filename, ".esp") ||
         boost::iends_with(filename, ".esm") ||
         boost::iends_with(filename, ".esl");
}

Game::Game(const GameSettings& gameSettings,
           const std::filesystem::path& lootDataPath) :
    GameSettings(gameSettings),
    lootDataPath_(lootDataPath),
    pluginsFullyLoaded_(false),
    loadOrderSortCount_(0) {}

Game::Game(const Game& game) :
    GameSettings(game),
    lootDataPath_(game.lootDataPath_),
    gameHandle_(game.gameHandle_),
    pluginsFullyLoaded_(game.pluginsFullyLoaded_),
    messages_(game.messages_),
    loadOrderSortCount_(0) {}

Game& Game::operator=(const Game& game) {
  if (&game != this) {
    GameSettings::operator=(game);

    lootDataPath_ = game.lootDataPath_;
    gameHandle_ = game.gameHandle_;
    pluginsFullyLoaded_ = game.pluginsFullyLoaded_;
    messages_ = game.messages_;
    loadOrderSortCount_ = game.loadOrderSortCount_;
  }

  return *this;
}

void Game::Init() {
  auto logger = getLogger();
  if (logger) {
    logger->info("Initialising filesystem-related data for game: {}", Name());
  }

  // Reset data that is dependent on the libloot game handle.
  messages_.clear();
  loadOrderSortCount_ = 0;
  pluginsFullyLoaded_ = false;

  gameHandle_ = CreateGameHandle(Type(), GamePath(), GameLocalPath());
  gameHandle_->IdentifyMainMasterFile(Master());

  if (!lootDataPath_.empty()) {
    // Make sure that the LOOT game path exists.
    auto lootGamePath = lootDataPath_ / u8path(FolderName());
    if (!fs::is_directory(lootGamePath)) {
      if (fs::exists(lootGamePath)) {
        throw FileAccessError(
            "Could not create LOOT folder for game, the path exists but is not "
            "a directory");
      }
      fs::create_directories(lootGamePath);
    }
  }
}

std::shared_ptr<const PluginInterface> Game::GetPlugin(
    const std::string& name) const {
  return gameHandle_->GetPlugin(name);
}

std::vector<std::shared_ptr<const PluginInterface>> Game::GetPlugins() const {
  return gameHandle_->GetLoadedPlugins();
}

std::vector<Message> Game::CheckInstallValidity(
    const std::shared_ptr<const PluginInterface>& plugin,
    const PluginMetadata& metadata) {
  auto logger = getLogger();

  if (logger) {
    logger->trace(
        "Checking that the current install is valid according to {}'s data.",
        plugin->GetName());
  }
  std::vector<Message> messages;
  if (IsPluginActive(plugin->GetName())) {
    auto fileExists = [&](const std::string& file) {
      return std::filesystem::exists(DataPath() / u8path(file)) ||
             (hasPluginFileExtension(file) &&
              std::filesystem::exists(DataPath() / u8path(file + ".ghost")));
    };

    auto tags = metadata.GetTags();
    auto hasFilterTag =
        std::any_of(tags.cbegin(), tags.cend(), [&](const Tag& tag) {
          return tag.GetName() == "Filter";
        });

    if (!hasFilterTag) {
      for (const auto& master : plugin->GetMasters()) {
        if (!fileExists(master)) {
          if (logger) {
            logger->error("\"{}\" requires \"{}\", but it is missing.",
                          plugin->GetName(),
                          master);
          }
          messages.push_back(
              PlainTextMessage(MessageType::error,
                               (boost::format(boost::locale::translate(
                                    "This plugin requires \"%1%\" to be "
                                    "installed, but it is missing.")) %
                                master)
                                   .str()));
        } else if (!IsPluginActive(master)) {
          if (logger) {
            logger->error("\"{}\" requires \"{}\", but it is inactive.",
                          plugin->GetName(),
                          master);
          }
          messages.push_back(
              PlainTextMessage(MessageType::error,
                               (boost::format(boost::locale::translate(
                                    "This plugin requires \"%1%\" to be "
                                    "active, but it is inactive.")) %
                                master)
                                   .str()));
        }
      }
    }

    std::unordered_set<std::string> displayNamesWithMessages;

    for (const auto& req : metadata.GetRequirements()) {
      auto file = std::string(req.GetName());
      if (!fileExists(file)) {
        if (logger) {
          logger->error("\"{}\" requires \"{}\", but it is missing.",
                        plugin->GetName(),
                        file);
        }
        if (displayNamesWithMessages.count(req.GetDisplayName()) > 0) {
          continue;
        }
        messages.push_back(Message(MessageType::error,
                                   (boost::format(boost::locale::translate(
                                        "This plugin requires \"%1%\" to be "
                                        "installed, but it is missing.")) %
                                    req.GetDisplayName())
                                       .str()));
        displayNamesWithMessages.insert(req.GetDisplayName());
      }
    }

    displayNamesWithMessages.clear();

    for (const auto& inc : metadata.GetIncompatibilities()) {
      auto file = std::string(inc.GetName());
      if (fileExists(file) &&
          (!hasPluginFileExtension(file) || IsPluginActive(file))) {
        if (logger) {
          logger->error(
              "\"{}\" is incompatible with \"{}\", but both are present.",
              plugin->GetName(),
              file);
        }
        if (displayNamesWithMessages.count(inc.GetDisplayName()) > 0) {
          continue;
        }
        messages.push_back(
            Message(MessageType::error,
                    (boost::format(boost::locale::translate(
                         "This plugin is incompatible with \"%1%\", but both "
                         "are present.")) %
                     inc.GetDisplayName())
                        .str()));
        displayNamesWithMessages.insert(inc.GetDisplayName());
      }
    }
  }

  if (plugin->IsLightMaster() &&
      !boost::iends_with(plugin->GetName(), ".esp")) {
    for (const auto& masterName : plugin->GetMasters()) {
      auto master = GetPlugin(masterName);
      if (!master) {
        if (logger) {
          logger->info(
              "Tried to get plugin object for master \"{}\" of \"{}\" but it "
              "was not loaded.",
              masterName,
              plugin->GetName());
        }
        continue;
      }

      if (!master->IsLightMaster() && !master->IsMaster()) {
        if (logger) {
          logger->error(
              "\"{}\" is a light master and requires the non-master plugin "
              "\"{}\". This can cause issues in-game, and sorting will fail "
              "while this plugin is installed.",
              plugin->GetName(),
              masterName);
        }
        messages.push_back(PlainTextMessage(
            MessageType::error,
            (boost::format(boost::locale::translate(
                 "This plugin is a light master and requires the non-master "
                 "plugin \"%1%\". This can cause issues in-game, and sorting "
                 "will fail while this plugin is installed.")) %
             masterName)
                .str()));
      }
    }
  }

  if (plugin->IsLightMaster() && !plugin->IsValidAsLightMaster()) {
    if (logger) {
      logger->error(
          "\"{}\" contains records that have FormIDs outside the valid range "
          "for an ESL plugin. Using this plugin will cause irreversible damage "
          "to your game saves.",
          plugin->GetName());
    }
    messages.push_back(PlainTextMessage(
        MessageType::error,
        boost::locale::translate(
            "This plugin contains records that have FormIDs outside "
            "the valid range for an ESL plugin. Using this plugin "
            "will cause irreversible damage to your game saves.")));
  }

  if (plugin->GetHeaderVersion() < MinimumHeaderVersion()) {
    if (logger) {
      logger->warn(
          "\"{}\" has a header version of {}, which is less than the game's "
          "minimum supported header version of {}.",
          plugin->GetName(),
          plugin->GetHeaderVersion(),
          MinimumHeaderVersion());
    }
    messages.push_back(PlainTextMessage(
        MessageType::warn,
        (boost::format(boost::locale::translate(
             "This plugin has a header version of %1%, which is less than the "
             "game's minimum supported header version of %2%.")) %
         plugin->GetHeaderVersion() % MinimumHeaderVersion())
            .str()));
  }

  if (metadata.GetGroup().has_value()) {
    auto groupName = metadata.GetGroup().value();
    auto groups = gameHandle_->GetDatabase()->GetGroups();
    auto groupIsUndefined =
        std::none_of(groups.cbegin(), groups.cend(), [&](const Group& group) {
          return group.GetName() == groupName;
        });

    if (groupIsUndefined) {
      messages.push_back(PlainTextMessage(
          MessageType::error,
          (boost::format(
               boost::locale::translate("This plugin belongs to the group "
                                        "\"%1%\", which does not exist.")) %
           groupName)
              .str()));
    }
  }

  // Also generate dirty messages.
  for (const auto& element : metadata.GetDirtyInfo()) {
    messages.push_back(ToMessage(element));
  }

  return messages;
}

void Game::RedatePlugins() {
  auto logger = getLogger();

  if (Type() != GameType::tes5 && Type() != GameType::tes5se) {
    if (logger) {
      logger->warn("Cannot redate plugins for game {}.", Name());
    }
    return;
  }

  vector<string> loadorder = gameHandle_->GetLoadOrder();
  if (!loadorder.empty()) {
    std::filesystem::file_time_type lastTime =
        std::filesystem::file_time_type::clock::time_point::min();
    for (const auto& pluginName : loadorder) {
      fs::path filepath = DataPath() / u8path(pluginName);
      if (!fs::exists(filepath)) {
        filepath += ".ghost";
        if (!fs::exists(filepath)) {
          continue;
        }
      }

      auto thisTime = fs::last_write_time(filepath);
      if (thisTime >= lastTime) {
        lastTime = thisTime;

        if (logger) {
          logger->trace("No need to redate \"{}\".",
                        filepath.filename().u8string());
        }
      } else {
        lastTime += std::chrono::seconds(60);
        fs::last_write_time(filepath,
                            lastTime);  // Space timestamps by a minute.

        if (logger) {
          logger->info("Redated \"{}\"", filepath.filename().u8string());
        }
      }
    }
  }
}

void Game::LoadAllInstalledPlugins(bool headersOnly) {
  try {
    gameHandle_->LoadCurrentLoadOrderState();
  } catch (std::exception& e) {
    auto logger = getLogger();
    if (logger) {
      logger->error("Failed to load current load order. Details: {}", e.what());
    }
    AppendMessage(PlainTextMessage(
        MessageType::error,
        boost::locale::translate("Failed to load the current load order, "
                                 "information displayed may be incorrect.")
            .str()));
  }

  auto installedPluginNames = GetInstalledPluginNames();
  gameHandle_->LoadPlugins(installedPluginNames, headersOnly);

  // Check if any plugins have been removed.
  std::vector<std::string> loadedPluginNames;
  for (auto plugin : gameHandle_->GetLoadedPlugins()) {
    loadedPluginNames.push_back(plugin->GetName());
  }

  AppendMessages(
      CheckForRemovedPlugins(installedPluginNames, loadedPluginNames));

  pluginsFullyLoaded_ = !headersOnly;
}

bool Game::ArePluginsFullyLoaded() const { return pluginsFullyLoaded_; }

fs::path Game::MasterlistPath() const {
  return lootDataPath_ / u8path(FolderName()) / "masterlist.yaml";
}

fs::path Game::UserlistPath() const {
  return lootDataPath_ / u8path(FolderName()) / "userlist.yaml";
}

fs::path Game::PluginsTxtPath() const {
  return lootDataPath_.parent_path() / u8path(FolderName()) / "plugins.txt";
}

std::vector<std::string> Game::GetLoadOrder() const {
  return gameHandle_->GetLoadOrder();
}

void Game::SetLoadOrder(const std::vector<std::string>& loadOrder) {
  BackupLoadOrder(GetLoadOrder(), lootDataPath_ / u8path(FolderName()));
  gameHandle_->SetLoadOrder(loadOrder);
}

bool Game::IsPluginActive(const std::string& pluginName) const {
  return gameHandle_->IsPluginActive(pluginName);
}

std::optional<short> Game::GetActiveLoadOrderIndex(
    const std::shared_ptr<const PluginInterface>& plugin,
    const std::vector<std::string>& loadOrder) const {
  // Get the full load order, then count the number of active plugins until the
  // given plugin is encountered. If the plugin isn't active or in the load
  // order, return nullopt.

  if (!IsPluginActive(plugin->GetName()))
    return std::nullopt;

  short numberOfActivePlugins = 0;
  for (const std::string& otherPluginName : loadOrder) {
    if (CompareFilenames(plugin->GetName(), otherPluginName) == 0) {
      return numberOfActivePlugins;
    }

    auto otherPlugin = GetPlugin(otherPluginName);
    if (otherPlugin &&
        plugin->IsLightMaster() == otherPlugin->IsLightMaster() &&
        IsPluginActive(otherPluginName)) {
      ++numberOfActivePlugins;
    }
  }

  return std::nullopt;
}

std::vector<std::string> Game::SortPlugins() {
  auto logger = getLogger();

  try {
    gameHandle_->LoadCurrentLoadOrderState();
  } catch (std::exception& e) {
    if (logger) {
      logger->error("Failed to load current load order. Details: {}", e.what());
    }
    AppendMessage(PlainTextMessage(
        MessageType::error,
        boost::locale::translate("Failed to load the current load order, "
                                 "information displayed may be incorrect.")
            .str()));
  }

  std::vector<std::string> sortedPlugins;
  try {
    // Clear any existing game-specific messages, as these only relate to
    // state that has been changed by sorting.
    ClearMessages();

    auto currentLoadOrder = gameHandle_->GetLoadOrder();

    sortedPlugins = gameHandle_->SortPlugins(currentLoadOrder);

    AppendMessages(CheckForRemovedPlugins(currentLoadOrder, sortedPlugins));

    IncrementLoadOrderSortCount();
  } catch (CyclicInteractionError& e) {
    if (logger) {
      logger->error("Failed to sort plugins. Details: {}", e.what());
    }
    AppendMessage(Message(
        MessageType::error,
        (boost::format(boost::locale::translate(
             "Cyclic interaction detected between \"%1%\" and \"%2%\": %3%")) %
         EscapeMarkdownSpecialChars(e.GetCycle().front().GetName()) %
         EscapeMarkdownSpecialChars(e.GetCycle().back().GetName()) %
         DescribeCycle(e.GetCycle()))
            .str()));
    sortedPlugins.clear();
  } catch (UndefinedGroupError& e) {
    if (logger) {
      logger->error("Failed to sort plugins. Details: {}", e.what());
    }
    AppendMessage(PlainTextMessage(MessageType::error,
                                   (boost::format(boost::locale::translate(
                                        "The group \"%1%\" does not exist.")) %
                                    e.GetGroupName())
                                       .str()));
    sortedPlugins.clear();
  } catch (std::exception& e) {
    if (logger) {
      logger->error("Failed to sort plugins. Details: {}", e.what());
    }
    sortedPlugins.clear();
  }

  return sortedPlugins;
}

void Game::IncrementLoadOrderSortCount() {
  lock_guard<mutex> guard(mutex_);

  ++loadOrderSortCount_;
}

void Game::DecrementLoadOrderSortCount() {
  lock_guard<mutex> guard(mutex_);

  if (loadOrderSortCount_ > 0)
    --loadOrderSortCount_;
}

std::vector<Message> Game::GetMessages() const {
  std::vector<Message> output(
      gameHandle_->GetDatabase()->GetGeneralMessages(true));
  output.insert(end(output), begin(messages_), end(messages_));

  if (loadOrderSortCount_ == 0)
    output.push_back(PlainTextMessage(
        MessageType::warn,
        boost::locale::translate(
            "You have not sorted your load order this session.")));

  size_t activeNormalPluginsCount = 0;
  bool hasActiveEsl = false;
  for (const auto& plugin : GetPlugins()) {
    if (IsPluginActive(plugin->GetName())) {
      if (plugin->IsLightMaster()) {
        hasActiveEsl = true;
      } else {
        ++activeNormalPluginsCount;
      }
    }
  }

  if (activeNormalPluginsCount > 254 && hasActiveEsl) {
    auto logger = getLogger();
    if (logger) {
      logger->warn(
          "255 normal plugins and at least one light plugin are active at the "
          "same time.");
    }
    output.push_back(PlainTextMessage(
        MessageType::warn,
        boost::locale::translate(
            "You have a normal plugin and at least one light plugin sharing "
            "the FE load order index. Deactivate a normal plugin or all your "
            "light plugins to avoid potential issues.")));
  }

  return output;
}

void Game::AppendMessage(const Message& message) {
  lock_guard<mutex> guard(mutex_);

  messages_.push_back(message);
}

void Game::ClearMessages() {
  lock_guard<mutex> guard(mutex_);

  messages_.clear();
}

bool Game::UpdateMasterlist() {
  bool wasUpdated = gameHandle_->GetDatabase()->UpdateMasterlist(
      MasterlistPath(), RepoURL(), RepoBranch());
  if (wasUpdated && !gameHandle_->GetDatabase()->IsLatestMasterlist(
                        MasterlistPath(), RepoBranch())) {
    AppendMessage(PlainTextMessage(
        MessageType::error,
        boost::locale::translate(
            "The latest masterlist revision contains a syntax error, LOOT is "
            "using the most recent valid revision instead. Syntax errors are "
            "usually minor and fixed within hours.")));
  }

  return wasUpdated;
}

MasterlistInfo Game::GetMasterlistInfo() const {
  return gameHandle_->GetDatabase()->GetMasterlistRevision(MasterlistPath(),
                                                           true);
}

void Game::LoadMetadata() {
  auto logger = getLogger();

  std::filesystem::path masterlistPath;
  std::filesystem::path userlistPath;
  if (std::filesystem::exists(MasterlistPath())) {
    if (logger) {
      logger->debug("Preparing to parse masterlist.");
    }
    masterlistPath = MasterlistPath();
  }

  if (std::filesystem::exists(UserlistPath())) {
    if (logger) {
      logger->debug("Preparing to parse userlist.");
    }
    userlistPath = UserlistPath();
  }

  if (logger) {
    logger->debug("Parsing metadata list(s).");
  }
  try {
    gameHandle_->GetDatabase()->LoadLists(masterlistPath, userlistPath);
  } catch (std::exception& e) {
    if (logger) {
      logger->error("An error occurred while parsing the metadata list(s): {}",
                    e.what());
    }
    AppendMessage(Message(
        MessageType::error,
        (boost::format(boost::locale::translate(
             "An error occurred while parsing the metadata list(s): "
             "%1%.\n\nTry updating your masterlist to resolve the error. If "
             "the error is with your user metadata, this probably happened "
             "because an update to LOOT changed its metadata syntax support. "
             "Your user metadata will have to be updated manually.\n\nTo do "
             "so, use the 'Open Debug Log Location' in LOOT's main menu to "
             "open its data folder, then open your 'userlist.yaml' file in the "
             "relevant game folder. You can then edit the metadata it contains "
             "with reference to the documentation, which is accessible through "
             "LOOT's main menu.\n\nYou can also seek support on LOOT's forum "
             "thread, which is linked to on [LOOT's "
             "website](https://loot.github.io/).")) %
         EscapeMarkdownSpecialChars(e.what()))
            .str()));
  }
}

std::vector<std::string> Game::GetKnownBashTags() const {
  return gameHandle_->GetDatabase()->GetKnownBashTags();
}

std::vector<Group> Game::GetMasterlistGroups() const {
  return gameHandle_->GetDatabase()->GetGroups(false);
}

std::vector<Group> Game::GetUserGroups() const {
  return gameHandle_->GetDatabase()->GetUserGroups();
}

std::optional<PluginMetadata> Game::GetMasterlistMetadata(
    const std::string& pluginName,
    bool evaluateConditions) const {
  return gameHandle_->GetDatabase()->GetPluginMetadata(
      pluginName, false, evaluateConditions);
}

std::optional<PluginMetadata> Game::GetUserMetadata(
    const std::string& pluginName,
    bool evaluateConditions) const {
  return gameHandle_->GetDatabase()->GetPluginUserMetadata(pluginName,
                                                           evaluateConditions);
}

void Game::SetUserGroups(const std::vector<Group>& groups) {
  return gameHandle_->GetDatabase()->SetUserGroups(groups);
}

void Game::AddUserMetadata(const PluginMetadata& metadata) {
  gameHandle_->GetDatabase()->SetPluginUserMetadata(metadata);
}

void Game::ClearUserMetadata(const std::string& pluginName) {
  gameHandle_->GetDatabase()->DiscardPluginUserMetadata(pluginName);
}

void Game::ClearAllUserMetadata() {
  gameHandle_->GetDatabase()->DiscardAllUserMetadata();
}

void Game::SaveUserMetadata() {
  gameHandle_->GetDatabase()->WriteUserMetadata(UserlistPath(), true);
}

std::vector<std::string> Game::GetInstalledPluginNames() {
  std::vector<std::string> plugins;

  auto logger = getLogger();
  if (logger) {
    logger->trace("Scanning for plugins in {}", this->DataPath().u8string());
  }

  for (fs::directory_iterator it(this->DataPath());
       it != fs::directory_iterator();
       ++it) {
    if (fs::is_regular_file(it->status()) &&
        gameHandle_->IsValidPlugin(it->path().filename().u8string())) {
      string name = it->path().filename().u8string();

      if (logger) {
        logger->info("Found plugin: {}", name);
      }

      plugins.push_back(name);
    }
  }

  return plugins;
}

void Game::AppendMessages(std::vector<Message> messages) {
  for (auto message : messages) {
    AppendMessage(message);
  }
}
}
}

ext: cpp

max_stars_repo_name: haifengkao/HugeLoot

max_issues_repo_name: haifengkao/HugeLoot

max_forks_repo_name: haifengkao/HugeLoot


text:
#include "nailtool.h"
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/case_conv.hpp>
#include <list>
class CDataModel{
  std::stringstream out;
  std::ostream &outs;
  std::list<std::string> global_enum;
  std::string int_type(const intp &intp ){
    int length = boost::lexical_cast<int>(mk_str(intp.sign));
    if(length<=8) length = 8;
    else if(length<=16) length=16;
    else if(length<=32) length=32;
    else if(length<=64) length=64;
    else throw std::range_error("Integer longer than 64 bits");
    return (boost::format("%s%d_t") % (intp.N_type == UNSIGN ? "uint":"int" )% length).str();
  }

  void emit_array(const parser &inner, const std::string name = ""){

    out << "struct "<< name <<"{\n";
    emit_type(inner);
    out << "*elem;\n size_t count;\n";
    out << "}" ;

  }
  void emit_type( const parser &outer,const std::string name =""){
    const parserinner &p = outer.pr;
    switch(p.N_type){
    case INTEGER:
      out << int_type(p.integer.parser);
      break;
    case STRUCTURE:
      out<<"struct " << name << "{\n";
      FOREACH(field,p.structure){
        if(field->N_type != FIELD)
          continue;
        emit_type(*field->field.parser);
        out << " " << mk_str(field->field.name) << ";\n";
      }
      out << "}" << std::endl;
      break;
    case WRAP:
      emit_type(*p.wrap.parser);
      break;
    case SELECTP:{
      out << "struct " << name << "{\n";
      FOREACH(option, p.selectp.options){
        global_enum.push_front(mk_str(option->tag));
      }
      out<< "enum N_types N_type;\n";
      out << "union {\n";
      FOREACH(option, p.selectp.options){
        std::string tag = mk_str(option->tag);
        boost::algorithm::to_lower(tag);
        emit_type(*option->parser);
        out << " "<< tag << ";\n";
      }
      out<< "};\n}";
      break;
    }
    case CHOICE:{
      out << "struct " << name<< "{\n";
      if(0){//TODO: global enum option
        out<< " enum  {";
        int idx=0;
        FOREACH(option, p.choice){
          std::string tag = mk_str(option->tag);
          if(idx++ >0)
            out << ',';
          out << tag;
        }
        out << "} N_type; \n";
      }  else{
        FOREACH(option, p.choice){
          global_enum.push_front(mk_str(option->tag));
        }
        out<< "enum N_types N_type;\n";
      }
      out << "union {\n";
      FOREACH(option, p.choice){
        std::string tag = mk_str(option->tag);
        boost::algorithm::to_lower(tag);
        emit_type(*option->parser);
        out << " "<< tag << ";\n";
      }
      out<< "};\n}";
    }
      break;

    case ARRAY:
      switch(p.array.N_type){
      case MANY:
      case MANYONE:
        emit_array(*p.array.many,name);
        break;
      case SEPBY:
      case SEPBYONE:
        emit_array(*p.array.sepby.inner,name);
        break;
      }
      break;
    case FIXEDARRAY:
      emit_type(*p.fixedarray.inner);
      out << "["<< intconstant_value(p.fixedarray.length) << "]";
      break;
    case OPTIONAL:
      emit_type(*p.optional);
      out << "*";
      break;
    case LENGTH:
      emit_array(*p.length.parser,name);
      break;

    case NUNION:
      {
        std::string first;
        int idx=0;
        FOREACH(elem, p.nunion){
          //  std::stringstream tmp;
          //emit_type(tmp,**elem);
          if(idx++==0)
            emit_type(**elem);
          //            first = tmp.str();
          else{
            //TODO: Perform in-depth checking
            //         if(first!= tmp.str()){
            //        throw std::runtime_error(boost::str(boost::format("UNION elements do not induce the same type\n %s \n %s") % tmp.str() % first)); //Be more
            //descriptive
            //    }
          }
        }
        out << first;
      }
      break;
    case REF:
      out << mk_str(p.ref.name)<<"* "; // TODO: Make ref more powerful, featuring ALIAS
      break;
    case NAME:
      out << mk_str(p.name.name);
      break;
    case APPLY:
      emit_type(*p.apply.inner);
      break;
    }
  }
  void emit_type_definition( const parser &p,const std::string name)
  {
    switch(p.pr.N_type){
    case STRUCTURE:
    case ARRAY:
    case CHOICE:
    case SELECTP:
      emit_type(p,name);
      out << ";" << std::endl;
      break;
    case WRAP:
      emit_type_definition(*p.pr.wrap.parser,name);
      break;
    default:
      break; //Other definitions don't need a type
    }
  }
  void emit_type( const definition &def){
    if(def.N_type != PARSER)
      return;
    std::string name(mk_str(def.parser.name));
    emit_type_definition(def.parser.definition,name);
  }
public:
  void emit_parser( grammar *grammar){
    try{
      assert(outs.good());
      outs << "#include <stdint.h>\n #include <string.h>\n#include <assert.h>\n";
      //Emit forward declarations
      FOREACH(definition,*grammar){
        if(definition->N_type!=PARSER)
          continue;
        std::string name(mk_str(definition->parser.name));
        std::string post;
        std::string typedef_t( typedef_type(definition->parser.definition,name,&post));
        out << "typedef "<<typedef_t<< " " << name << post <<";" << std::endl;
      }
      FOREACH(definition, *grammar){
        emit_type(*definition);
      }
      out<< std::endl;

      outs << "enum N_types {_NAIL_NULL";
      for(std::string e: global_enum){
        outs << "," << e;
      }
      outs << "};\n";
      outs << out.str()<< std::endl;
    } catch(const std::exception &e){
      std::cerr << "Exception while generating parser" << e.what()<<std::endl;
      exit(-1);
    }
  }

  CDataModel(std::ostream *os) : outs(*os){}

};
void emit_header(std::ostream *out, grammar *grammar){
  CDataModel data(out);
  data.emit_parser(grammar);
  *out  <<   std::string((char *)parser_template_h,parser_template_h_len);
  FOREACH(def, *grammar){
    if(def->N_type == PARSER){
      std::string name= mk_str(def->parser.name);
      if(option::cpp)
        *out << name << "*parse_" << name << "(NailArena *arena, NailStream *data);\n";
      else
        *out << name << "*parse_" << name << "(NailArena *arena, const uint8_t *data, size_t size);\n";
    }
  }

}

ext: cc

max_stars_repo_name: russetrob/nail

max_issues_repo_name: russetrob/nail

max_forks_repo_name: russetrob/nail


text:
//----------------------------------------------------------------------------
/// @file rearrange.hpp
/// @brief Indirect algorithm
///
/// @author Copyright (c) 2016 Francisco Jose Tapia (fjtapia@gmail.com )\n
///         Distributed under the Boost Software License, Version 1.0.\n
///         ( See accompanying file LICENSE_1_0.txt or copy at
///           http://www.boost.org/LICENSE_1_0.txt  )
/// @version 0.1
///
/// @remarks
//-----------------------------------------------------------------------------
#ifndef __BOOST_SORT_COMMON_REARRANGE_HPP
#define __BOOST_SORT_COMMON_REARRANGE_HPP

//#include <boost/sort/common/atomic.hpp>
#include <boost/sort/common/util/traits.hpp>
#include <cassert>
#include <functional>
#include <iterator>
#include <type_traits>
#include <vector>

namespace boost {
namespace sort {
namespace common {

template <class Iter_data> struct filter_iterator {
  //-----------------------------------------------------------------------
  //                   Variables
  //-----------------------------------------------------------------------
  Iter_data origin;

  //-----------------------------------------------------------------------
  //                   Functions
  //-----------------------------------------------------------------------
  filter_iterator(Iter_data global_first) : origin(global_first){};
  size_t operator()(Iter_data itx) const { return size_t(itx - origin); }
};

struct filter_pos {
  size_t operator()(size_t pos) const { return pos; };
};

//
//-----------------------------------------------------------------------------
//  function : rearrange
/// @brief This function transform a logical sort of the elements in the index
///        of iterators in a physical sort.
//
/// @param global_first : iterator to the first element of the data
/// @param [in] index : vector of the iterators
//-----------------------------------------------------------------------------
template <class Iter_data, class Iter_index, class Filter_pos>
void rearrange(Iter_data global_first, Iter_index itx_first,
               Iter_index itx_last, Filter_pos pos) {
  //-----------------------------------------------------------------------
  //                    Metaprogramming
  //-----------------------------------------------------------------------
  typedef util::value_iter<Iter_data> value_data;
  typedef util::value_iter<Iter_index> value_index;

  //-------------------------------------------------------------------------
  //                     Code
  //-------------------------------------------------------------------------
  assert((itx_last - itx_first) >= 0);
  size_t pos_dest, pos_src, pos_ini;
  size_t nelem = size_t(itx_last - itx_first);
  Iter_data data = global_first;
  Iter_index index = itx_first;

  pos_ini = 0;
  while (pos_ini < nelem) {
    while (pos_ini < nelem and pos(index[pos_ini]) == pos_ini)
      ++pos_ini;
    if (pos_ini == nelem)
      return;
    pos_dest = pos_src = pos_ini;
    value_data aux = std::move(data[pos_ini]);
    value_index itx_src = std::move(index[pos_ini]);

    while ((pos_src = pos(itx_src)) != pos_ini) {
      data[pos_dest] = std::move(data[pos_src]);
      std::swap(itx_src, index[pos_src]);
      pos_dest = pos_src;
    };

    data[pos_dest] = std::move(aux);
    index[pos_ini] = std::move(itx_src);
    ++pos_ini;
  };
};

/*
 //
 //-----------------------------------------------------------------------------
 //  function : rearrange_pos
 /// @brief This function transform a logical sort of the elements in the index
 ///        of iterators in a physical sort.
 //
 /// @param global_first : iterator to the first element of the data
 /// @param [in] index : vector of the iterators
 //-----------------------------------------------------------------------------
 template < class Iter_t, class Number >
 void rearrange_pos (Iter_t global_first, std::vector< Number> &index)
 {
 //-------------------------------------------------------------------------
 //          METAPROGRAMMING AND DEFINITIONS
 //-------------------------------------------------------------------------
 static_assert ( std::is_integral<Number>::value, "Incompatible Types");
 typedef iter_value< Iter_t > value_t;

 //-------------------------------------------------------------------------
 //                     CODE
 //-------------------------------------------------------------------------
 size_t pos_dest = 0;
 size_t pos_src = 0;
 size_t pos_ini = 0;
 size_t nelem = index.size ( );
 Iter_t it_dest (global_first), it_src(global_first);

 while (pos_ini < nelem)
 {
 while (pos_ini < nelem and
 index[pos_ini] == pos_ini)
 {
 ++pos_ini;
 };

 if (pos_ini == nelem) return;
 pos_dest = pos_src = pos_ini;
 it_dest = global_first + pos_dest;
 value_t Aux = std::move (*it_dest);

 while ((pos_src = index[pos_dest]) != pos_ini)
 {
 index[pos_dest] = it_dest - global_first;
 it_src = global_first + pos_src;
 *it_dest = std::move (*it_src);
 it_dest = it_src;
 pos_dest = pos_src;
 };

 *it_dest = std::move (Aux);
 index[pos_dest] = it_dest - global_first;
 ++pos_ini;
 };
 };
 */
//
//****************************************************************************
}; //    End namespace common
}; //    End namespace sort
}; //    End namespace boost
//****************************************************************************
//
#endif

ext: hpp

max_stars_repo_name: henrywarhurst/matrix

max_issues_repo_name: henrywarhurst/matrix

max_forks_repo_name: henrywarhurst/matrix


text:
#include <boost/smart_ptr.hpp>
#include <boost/iostreams/device/mapped_file.hpp>
#include <stdexcept>
#include <stdio.h>
#include <string.h>

#define LOG_HEADER  "OpenCLBlock"
#include <glog/logging.h>

#include "blaze/nv_opencl/OpenCLEnv.h"
#include "blaze/nv_opencl/OpenCLBlock.h"

namespace blaze {

OpenCLBlock::OpenCLBlock(
    std::vector<OpenCLEnv*> &_env_list,
    int _num_items,
    int _item_length,
    int _item_size,
    int _align_width,
    int _flag):
  env_list(_env_list),
  location(-1),   // uninitialized location
  DataBlock(_num_items, _item_length, _item_size, _align_width, _flag)
{
  num_devices = _env_list.size();
}

OpenCLBlock::OpenCLBlock(const OpenCLBlock& block):
  DataBlock(block),
  env_list(block.env_list),
  data(block.data),
  location(block.location),
  num_devices(block.num_devices)
{
}

OpenCLBlock::~OpenCLBlock() {
  if (allocated && !copied) {
    if (flag == BLAZE_INPUT_BLOCK ||
        flag == BLAZE_OUTPUT_BLOCK)
    {
      clReleaseMemObject(data[0]);
      delete data;
    } else {
      for (int d=0; d<num_devices; d++) {
        clReleaseMemObject(data[d]);
      }
      delete [] data;
    }
  }
}

void OpenCLBlock::alloc() {

  if (!allocated) {
    int num_devices = env_list.size();
    cl_int err = 0;

    // figure out the device assignment
    if (flag == BLAZE_INPUT_BLOCK) {
      // for input block distribute the assignment as much as possible
      // here use thread id since each alloc() is called by comm threads
      location = getTid() % num_devices;

      cl_context context = env_list[location]->getContext();

      data = new cl_mem;
      data[0] = clCreateBuffer(
          context, CL_MEM_READ_ONLY,
          size, NULL, &err);

      if (err != CL_SUCCESS) {
        LOG(ERROR) << "Cannot allocate input block, clCreateBuffer returns: "
          << err;

        throw std::runtime_error("Failed to allocate OpenCL block");
      }
      DLOG(INFO) << "Allocate input block on GPU_" << location;
    }
    else if (flag == BLAZE_SHARED_BLOCK) {
      // allocate on all devices
      data = new cl_mem[num_devices];

      for (int d=0; d<num_devices; d++) {
        cl_context context = env_list[d]->getContext();

        data[d] = clCreateBuffer(
            context, CL_MEM_READ_ONLY,
            size, NULL, &err);

        if (err != CL_SUCCESS) {
          LOG(ERROR) << "Cannot allocate shared block, clCreateBuffer returns: "
            << err;

          throw std::runtime_error("Failed to allocate OpenCL block");
        }
        DLOG(INFO) << "Allocate shared block on GPU_" << d;
      }
    }
    else {
      // output block should be allocated on the same device as task execution
      // location should be assigned by dispatcher in queue manager
      if (location < 0) {
        DLOG(ERROR) << "Location should be assigned by now";
        throw std::runtime_error("Unexpected error in allocation");
      }
      cl_context context = env_list[location]->getContext();

      data = new cl_mem;
      data[0] = clCreateBuffer(
          context, CL_MEM_WRITE_ONLY,
          size, NULL, &err);

      if (err != CL_SUCCESS) {
        LOG(ERROR) << "Cannot allocate output block, clCreateBuffer returns: "
          << err;

        throw std::runtime_error("Failed to allocate OpenCL block");
      }
      DLOG(INFO) << "Allocate output block on GPU_" << location;
    }
    allocated = true;
  }
}

void OpenCLBlock::readFromMem(std::string path) {

  boost::iostreams::mapped_file_source fin;

  fin.open(path, size);

  if (fin.is_open()) {

    uint64_t start_t = getUs();

    // first copy data from shared memory to a temp buffer
    char* temp_data = new char[size];
    char* mem_ptr = (char*)fin.data();

    if (aligned) {
      for (int k=0; k<num_items; k++) {

        // element size in memory
        int data_size = item_length*data_width;

        // memcpy is parallel among all tasks
        memcpy((void*)(temp_data+k*item_size),
            (void*)(mem_ptr+k*data_size), data_size);
      }
    }
    else {
      memcpy((void*)temp_data, (void*)mem_ptr, size);
    }

    uint64_t elapse_t = getUs() - start_t;
    DLOG(INFO) << "Read " <<
      (double)size /1024/1024 << "MB of data from mmap file takes " <<
      elapse_t << "us.";

    // then write temp buffer to FPGA, will be serialized among all tasks
    writeData(temp_data, size);

    delete [] temp_data;
    fin.close();
  }
  else {
    throw std::runtime_error(std::string("Cannot find file: ") + path);
  }
}

void OpenCLBlock::writeToMem(std::string path) {

  int data_size = size;

  boost::iostreams::mapped_file_params param(path);
  param.flags = boost::iostreams::mapped_file::mapmode::readwrite;
  param.new_file_size = data_size;
  param.length = data_size;
  boost::iostreams::mapped_file_sink fout(param);

  if (!fout.is_open()) {
    throw fileError(std::string("Cannot write file: ") + path);
  }

  // first copy data from FPGA to a temp buffer, will be serialized among all tasks
  char* temp_data = new char[data_size];
  readData(temp_data, data_size);

  // then copy data from temp buffer to shared memory, in parallel among all tasks
  memcpy((void*)fout.data(), temp_data, data_size);

  delete [] temp_data;
  fout.close();

  // change permission
  boost::filesystem::wpath wpath(path);
  boost::filesystem::permissions(wpath, boost::filesystem::add_perms |
                                        boost::filesystem::group_read |
                                        boost::filesystem::others_read);
}

void OpenCLBlock::writeData(void* src, size_t _size) {
  if (_size > size) {
    throw std::runtime_error("Not enough space left in Block");
  }

  // lazy allocation
  alloc();

  uint64_t start_t = getUs();
  writeData(src, _size, 0);
  ready = true;

  uint64_t elapse_t = getUs() - start_t;
  DLOG(INFO) << "Writting OpenCLBlock of size "
             << (double)size /1024/1024 << "MB takes "
             << elapse_t << "us.";
}

void OpenCLBlock::writeData(void* src, size_t _size, size_t offset) {

  if (offset+_size > size) {
    throw std::runtime_error("Exists block size");
  }
  cl_event event;

  // lazy allocation
  alloc();

  if (flag == BLAZE_INPUT_BLOCK) {
    // get the command queue handler
    cl_command_queue command = env_list[location]->getCmdQueue();

    int err = clEnqueueWriteBuffer(
        command, data[0], CL_TRUE, offset,
        _size, src, 0, NULL, &event);

    if (err != CL_SUCCESS) {
      DLOG(ERROR) << "clEnqueueWriteBuffer error: " << err;
      throw std::runtime_error("writeData() error");
    }
  }
  else { // must be BLAZE_SHARED_BLOCK
    for (int d=0; d<num_devices; d++) {
      // get the command queue handler
      cl_command_queue command = env_list[d]->getCmdQueue();

      int err = clEnqueueWriteBuffer(
          command, data[d], CL_TRUE, offset,
          _size, src, 0, NULL, &event);

      if (err != CL_SUCCESS) {
        DLOG(ERROR) << "clEnqueueWriteBuffer error: " << err;
        throw std::runtime_error("writeData() error");
      }
    }
  }

  if (offset + _size == size) {
    ready = true;
  }
}

// write data to an array
void OpenCLBlock::readData(void* dst, size_t size) {
  if (allocated) {

    // NOTE: must be BLAZE_OUTPUT_BLOCK

    // get the command queue handler
    cl_command_queue command = env_list[location]->getCmdQueue();
    cl_event event;

    // use a lock on TaskEnv to guarantee single-thread access to command queues
    // NOTE: this is unnecessary if the OpenCL runtime is thread-safe
    //boost::lock_guard<OpenCLEnv> guard(*env);

    int err = clEnqueueReadBuffer(
      command, data[0], CL_TRUE, 0,
      size, dst, 0, NULL, &event);

    if (err != CL_SUCCESS) {
      DLOG(ERROR) << "clEnqueueReadBuffer error: " << err;
      throw std::runtime_error("Failed to read an OpenCL block");
    }
  }
  else {
    throw std::runtime_error("Block memory not allocated");
  }
}

DataBlock_ptr OpenCLBlock::sample(char* mask) {

  // count the total number of
  int masked_items = 0;
  for (int i=0; i<num_items; i++) {
    if (mask[i]!=0) {
      masked_items ++;
    }
  }

  OpenCLBlock* ocl_block = new OpenCLBlock(env_list,
        item_length,
        item_size,
        aligned ? align_width : item_size);

  DataBlock_ptr block(ocl_block);

  cl_mem masked_data = *((cl_mem*)(ocl_block->getData()));

  // get the command queue handler
  cl_command_queue command = env_list[location]->getCmdQueue();
  cl_int err = 0;

  // start copy the masked data items to the new block,
  // since the current block is read-only, do not need to enforce lock
  int k=0;

  // array of cl_event to wait until all buffer copy is finished
  cl_event *events = new cl_event[num_items];

  for (int i=0; i<num_items; i++) {
    if (mask[i] != 0) {
      err = clEnqueueCopyBuffer(command,
          data[0], masked_data,
          i*item_size, k*item_size,
          item_size,
          0, NULL, events+k);

      if (err != CL_SUCCESS) {
        throw std::runtime_error(LOG_HEADER +
            std::string("error in clEnqueueCopyBuffer()"));
      }

      k++;
    }
  }
  err = clWaitForEvents(num_items, events);

  if (err != CL_SUCCESS) {
    throw std::runtime_error(LOG_HEADER +
        std::string("error during sampling"));
  }
  ocl_block->ready = true;

  delete [] events;

  return block;
}

char* OpenCLBlock::getData() {
  alloc();

  if (flag == BLAZE_INPUT_BLOCK ||
      flag == BLAZE_OUTPUT_BLOCK)
  {
    return (char*)data;
  }
  else {
    // location should be assigned already
    if (location < 0) {
      DLOG(ERROR) << "Location for shared block is not assigned";
      return NULL;
    }
    return (char*)&data[location];
  }
}

void OpenCLBlock::relocate(int loc) {
  if (flag == BLAZE_INPUT_BLOCK) {
    LOG(WARNING) << "relocation of input block is unsupported" ;
  }
  else {
    location = loc;
    if (flag == BLAZE_SHARED_BLOCK) {
      DLOG(INFO) << "Assigned shared block to GPU_" << location;
    }
    else {
      DLOG(INFO) << "Assigned output block to GPU_" << location;
    }
  }
}

int OpenCLBlock::getDeviceId() {
  return location;
}

} // namespace

max_stars_repo_name: FCS-holding/blaze

max_issues_repo_name: FCS-holding/blaze

max_forks_repo_name: FCS-holding/blaze


text:
//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/make_cref.hpp>
#include <fcppt/make_ref.hpp>
#include <fcppt/reference.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <unordered_set>
#include <fcppt/config/external_end.hpp>

namespace
{

typedef
fcppt::reference<
	int
>
int_ref;

typedef
fcppt::reference<
	int const
>
const_int_ref;

}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	reference
)
{

FCPPT_PP_POP_WARNING

	int x(
		42
	);

	int_ref ref(
		x
	);

	BOOST_CHECK_EQUAL(
		&ref.get(),
		&x
	);

	const_int_ref cref(
		x
	);

	BOOST_CHECK_EQUAL(
		&cref.get(),
		&x
	);

	BOOST_CHECK_EQUAL(
		&fcppt::make_ref(
			x
		).get(),
		&x
	);

	BOOST_CHECK_EQUAL(
		&fcppt::make_cref(
			x
		).get(),
		&x
	);

	int_ref other_ref(
		ref
	);

	BOOST_CHECK_EQUAL(
		&other_ref.get(),
		&ref.get()
	);

	ref = other_ref;

	BOOST_CHECK_EQUAL(
		&other_ref.get(),
		&ref.get()
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	reference_hash
)
{

FCPPT_PP_POP_WARNING

	typedef
	std::unordered_set<
		int_ref
	>
	int_ref_set;

	int x(
		42
	);

	int_ref_set set;

	set.insert(
		int_ref(
			x
		)
	);

	{
		int_ref_set::const_iterator const it(
			set.find(
				int_ref(
					x
				)
			)
		);

		BOOST_REQUIRE(
			it
			!=
			set.end()
		);

		BOOST_CHECK_EQUAL(
			&it->get(),
			&x
		);
	}

	typedef
	std::unordered_set<
		const_int_ref
	>
	const_int_ref_set;

	int const y(
		42
	);

	const_int_ref_set const_set;

	const_set.insert(
		const_int_ref(
			y
		)
	);

	{
		const_int_ref_set::const_iterator const it(
			const_set.find(
				const_int_ref(
					y
				)
			)
		);

		BOOST_REQUIRE(
			it
			!=
			const_set.end()
		);

		BOOST_CHECK_EQUAL(
			&it->get(),
			&y
		);
	}
}

max_stars_repo_name: vinzenz/fcppt

max_issues_repo_name: vinzenz/fcppt

max_forks_repo_name: vinzenz/fcppt


text:
#pragma once

#include <boost/format.hpp>

struct Escaper
{
    template<class FindResultT>
    std::string operator()(const FindResultT &match) const
    {
        std::string result;
        for (
            typename FindResultT::const_iterator iter = match.begin();
            iter != match.end();
            ++iter
        ) {
            result +=
                (boost::format("\\x%02x") % static_cast<int>(*iter)).str();
        }
        return result;
    }
};

max_stars_repo_name: tailix/taivirtfs

max_issues_repo_name: tailix/taivirtfs

max_forks_repo_name: tailix/taivirtfs


text:
/*
  This ROS node creates an instance of the libdeepdive driver, which it uses
  to pull data from all available trackers, as well as lighthouse/tracker info.
*/

// ROS includes
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>

// General messages
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Imu.h>
#include <deepdive_ros/Trackers.h>
#include <deepdive_ros/Light.h>
#include <deepdive_ros/Lighthouses.h>

// Eigen includes
#include <Eigen/Core>
#include <Eigen/Geometry>

// UKF includes
#include <UKF/Types.h>
#include <UKF/Integrator.h>
#include <UKF/StateVector.h>
#include <UKF/MeasurementVector.h>
#include <UKF/Core.h>

// C++ includes
#include <vector>
#include <functional>

// Deepdive internal
#include "deepdive.hh"

// FILTER KEYS

// State indexes
enum Keys : uint8_t {
  // STATE
  Position,             // Position (world frame, m)
  Attitude,             // Attitude quaternion (rotates vec from body to world)
  Velocity,             // Velocity (body frame, m/s)
  Omega,                // Angular velocity (body frame, rads/s)
  Acceleration,         // Acceleration (body frame, m/s^2)
  Alpha,                // Angular acceleration (body frame, rads/s^2)
  // ERRORS
  GyroscopeBias,        // Gyroscope bias offset (body frame, rad/s)
  GyroscopeScale,       // Gyroscope scale factor (body frame, multiplier)
  AccelerometerBias,    // Accelerometer bias offset (body frame, m/s^2)
  AccelerometerScale,   // Accelerometer scale factor (body frame, mutliplier)
  // MEASUREMENTS
  Accelerometer,        // Acceleration (body frame, m/s^2)
  Gyroscope,            // Gyroscope (body frame, rads/s)
  Angle
};

// OBSERVATION

// Observation vector
using Observation = UKF::DynamicMeasurementVector<
  UKF::Field<Accelerometer, UKF::Vector<3>>,
  UKF::Field<Gyroscope, UKF::Vector<3>>,
  UKF::Field<Angle, real_t>
>;

// TRACKING FILTER

// State vector
using State = UKF::StateVector<
  UKF::Field<Position, UKF::Vector<3>>,
  UKF::Field<Attitude, UKF::Quaternion>,
  UKF::Field<Velocity, UKF::Vector<3>>,
  UKF::Field<Omega, UKF::Vector<3>>,
  UKF::Field<Acceleration, UKF::Vector<3>>,
  UKF::Field<Alpha, UKF::Vector<3>>
>;

// For tracking
using TrackingFilter = UKF::Core<
  State, Observation, UKF::IntegratorRK4
>;

// ERROR FILTER

// Parameters
using Error = UKF::StateVector<
  UKF::Field<AccelerometerBias, UKF::Vector<3>>,
  UKF::Field<AccelerometerScale, UKF::Vector<3>>,
  UKF::Field<GyroscopeBias, UKF::Vector<3>>,
  UKF::Field<GyroscopeScale, UKF::Vector<3>>
>;

// For parameter estimation
using ErrorFilter = UKF::Core<
  Error, Observation, UKF::IntegratorEuler
>;

// For IMU parameter estimation

typedef std::map<std::string, Eigen::Affine3d> TransformMap;
typedef std::map<std::string, ErrorFilter> ErrorMap;

// Default IMU errors
Eigen::Vector3d imu_cov_ab_;         // Initial covariance: Accel bias
Eigen::Vector3d imu_cov_as_;         // Initial covariance: Accel scale
Eigen::Vector3d imu_cov_gb_;         // Initial covariance: Gyro bias
Eigen::Vector3d imu_cov_gs_;         // Initial covariance: Gyro scale
Eigen::Vector3d imu_proc_ab_;        // Process noise: Accel bias
Eigen::Vector3d imu_proc_as_;        // Process noise: Accel scale
Eigen::Vector3d imu_proc_gb_;        // Process noise: Gyro bias
Eigen::Vector3d imu_proc_gs_;        // Process noise: Gyro scale

// GLOBAL DATA STRUCTURES

std::string calfile_ = "deepdive.tf2";
std::string frame_world_ = "world";     // World frame
std::string frame_vive_ = "vive";       // Vive frame
std::string frame_body_ = "body";       // EKF / external solution
std::string frame_truth_ = "truth";     // Vive solution

LighthouseMap lighthouses_;          // List of lighthouses
TrackerMap trackers_;                // List of trackers
ErrorMap errors_;                    // List of error filters
TrackingFilter filter_;              // Tracking filter
std::string frame_parent_;           // Parent frame, eg "world"
std::string frame_child_;            // Child frame, eg "truth"
double rate_ = 10.0;                 // Desired tracking rate in Hz
int thresh_count_ = 4;               // Min num measurements required per bundle
double thresh_angle_ = 60.0;         // Angle threshold in degrees
double thresh_duration_ = 1.0;       // Duration threshold in micorseconds
bool use_gyroscope_ = true;          // Input measurements from gyroscope
bool use_accelerometer_ = true;      // Input measurements from accelerometer
bool use_light_ = true;              // Input measurements from light
double registration_[6];             // World -> vive

// ROS publishers
ros::Publisher pub_pose_;
ros::Publisher pub_twist_;

// Default measurement errors
bool correct_ = false;               // Whether to correct light parameters
Eigen::Vector3d gravity_;            // Gravity
Eigen::Vector3d obs_cov_acc_;        // Measurement covariance: Accelerometer
Eigen::Vector3d obs_cov_gyr_;        // Measurement covariance: Gyroscope
double obs_cov_ang_;                 // Measurement covariance: Angle

// Intermediary data
Eigen::Affine3d wTv_;                // ALL: world -> vive
TransformMap vTl_;                   // ALL: vive -> lighthouse
TransformMap bTh_;                   // ALL: head -> body
TransformMap tTh_;                   // ALL: head -> tracking
TransformMap tTi_;                   // ALL: imu -> tracking

// Context data
struct Context {
  std::string lighthouse;            // Active lighthouse
  std::string tracker;               // Active tracker
  Eigen::Vector3d sensor;            // Active sensor
  uint8_t axis;                      // Active axis
};

// Are we initialized and ready to track
bool initialized_ = false;

// TRACKING FILTER

namespace UKF {

  // MEASURMENT

  template <>
  Observation::CovarianceVector Observation::measurement_covariance(
    (Observation::CovarianceVector() <<
      1.0e-4, 1.0e-4, 1.0e-4,   // Accel
      1.0e-6, 1.0e-6, 1.0e-6,   // Gyro
      1.0e-8).finished());

  // TRACKING FILTER

  // Standard 6DoF kinematics with constant Acceleration assumption
  template <> template <> State
  State::derivative<>() const {
    UKF::Quaternion omega_q;
    omega_q.vec() = get_field<Omega>() * 0.5;
    omega_q.w() = 0;
    State output;
    output.set_field<Position>(get_field<Velocity>());
    output.set_field<Velocity>(get_field<Attitude>() * get_field<Acceleration>());
    output.set_field<Acceleration>(UKF::Vector<3>(0, 0, 0));
    output.set_field<Attitude>(omega_q * get_field<Attitude>());
    output.set_field<Omega>(get_field<Alpha>());
    output.set_field<Alpha>(UKF::Vector<3>(0, 0, 0));
    return output;
  }

  // See http://www.mdpi.com/1424-8220/11/7/6771/htm
  template <> template <> UKF::Vector<3>
  Observation::expected_measurement<State, Accelerometer, Error, Context>(
    State const& state, Error const& error, Context const& context) {
    Eigen::Affine3d iTb = tTi_[context.tracker].inverse()    // light -> imu
                        * tTh_[context.tracker]              // head -> light
                        * bTh_[context.tracker].inverse();   // body -> head
    Eigen::Vector3d r = iTb.translation();
    Eigen::Vector3d w = state.get_field<Omega>();
    return error.get_field<AccelerometerScale>().cwiseInverse().cwiseProduct(
        iTb.linear() * state.get_field<Acceleration>()
      + iTb.linear() * w.cross(w.cross(r))
      + iTb.linear() * (state.get_field<Attitude>().conjugate() * gravity_)
      - error.get_field<AccelerometerBias>());
  }

  // See http://www.mdpi.com/1424-8220/11/7/6771/htm
  template <> template <> UKF::Vector<3>
  Observation::expected_measurement<State, Gyroscope, Error, Context>(
    State const& state, Error const& error, Context const& context) {
    Eigen::Affine3d iTb = tTi_[context.tracker].inverse()    // light -> imu
                        * tTh_[context.tracker]              // head -> light
                        * bTh_[context.tracker].inverse();   // body -> head
    return error.get_field<GyroscopeScale>().cwiseInverse().cwiseProduct(
        iTb.linear() * state.get_field<Omega>()
      - error.get_field<GyroscopeBias>());
  }

  // Lighthouse angle prediction
  template <> template <> real_t
  Observation::expected_measurement<State, Angle, Error, Context>(
    State const& state, Error const& error, Context const& context) {
    Eigen::Affine3d wTb;
    wTb.translation() = state.get_field<Position>();
    wTb.linear() = state.get_field<Attitude>().toRotationMatrix();
    UKF::Vector<3> x = wTv_.inverse()                      // world -> vive
                     * vTl_[context.lighthouse].inverse()  // vive -> lh
                     * wTb                                 // body -> world
                     * bTh_[context.tracker]               // head -> body
                     * tTh_[context.tracker].inverse()     // tracker -> head
                     * context.sensor;
    double xyz[3], ang[2];
    xyz[0] = x[0];
    xyz[1] = x[1];
    xyz[2] = x[2];
    Predict(lighthouses_[context.lighthouse].params, xyz, ang, correct_);
    return ang[context.axis];
  }

  // ERROR FILTER

  template <> template <> Error
  Error::derivative<>() const {
    return Error::Zero();
  }

  template <> template <> UKF::Vector<3>
  Observation::expected_measurement<Error, Accelerometer, State, Context>(
    Error const& errors, State const& state, Context const& context) {
    return expected_measurement<State, Accelerometer, Error, Context>(
      state, errors, context);
  }

  template <> template <> UKF::Vector<3>
  Observation::expected_measurement<Error, Gyroscope, State, Context>(
    Error const& errors, State const& state, Context const& context) {
    return expected_measurement<State, Gyroscope, Error, Context>(
      state, errors, context);
  }

  template <> template <> real_t
  Observation::expected_measurement<Error, Angle, State, Context>(
    Error const& errors, State const& state, Context const& context) {
    return expected_measurement<State, Angle, Error, Context>(
      state, errors, context);
  }
}

// UTILITY FUNCTIONS

bool Delta(double & dt) {
  static ros::Time last = ros::Time::now(), now;
  now = ros::Time::now();
  dt = (now - last).toSec();
  if (dt > 0)
    last = now;
  return (dt > 0 && dt < 1.0);
}

// CALLBACKS

// This will be called at approximately 120Hz
// - Single lighthouse in 'A' mode : 120Hz (60Hz per axis)
// - Dual lighthouses in b/A or b/c modes : 120Hz (30Hz per axis)
void LightCallback(deepdive_ros::Light::ConstPtr const& msg) {
  static double dt;
  if (!use_light_ || !initialized_ || !Delta(dt))
    return;

  // Check that we are recording and that the tracker/lighthouse is ready
  TrackerMap::iterator tracker = trackers_.find(msg->header.frame_id);
  if (tracker == trackers_.end() || !tracker->second.ready) {
    ROS_INFO_STREAM_THROTTLE(1, "Tracker not found or ready");
    return;
  }

  // Check that we are recording and that the tracker/lighthouse is ready
  LighthouseMap::iterator lighthouse = lighthouses_.find(msg->lighthouse);
  if (lighthouse == lighthouses_.end() || !lighthouse->second.ready) {
    ROS_INFO_STREAM_THROTTLE(1, "Lighthouse not found or ready");
    return;
  }

  // Make sure we have a filter setup for this
  ErrorMap::iterator error = errors_.find(msg->header.frame_id);
  if (error == errors_.end()) {
    ROS_INFO_STREAM_THROTTLE(1, "Tracker error filter not initialized");
    return;
  }

  // Clean up the measurments
  std::vector<deepdive_ros::Pulse> data;
  for (size_t i = 0; i < msg->pulses.size(); i++) {
    // Basic sanity checks on the data
    if (fabs(msg->pulses[i].angle) > thresh_angle_ / 57.2958) {
      ROS_INFO_STREAM_THROTTLE(1.0, "Rejected based on angle");
      continue;
    }
    if (fabs(msg->pulses[i].duration) < thresh_duration_ / 1e6) {
      ROS_INFO_STREAM_THROTTLE(1.0, "Rejected based on duration");
      continue;
    }
    if (msg->pulses[i].sensor >= NUM_SENSORS) {
      ROS_INFO_STREAM_THROTTLE(1.0, "Rejected based on invalid sensor id");
      continue;
    }
    data.push_back(msg->pulses[i]);
  }
  if (thresh_count_ > 0 && data.size() < thresh_count_) {
    ROS_INFO_STREAM_THROTTLE(1, "Not enough data so skipping bundle.");
    return;
  }

  // Set the context correctly
  Context context;
  context.tracker = msg->header.frame_id;
  context.lighthouse = msg->lighthouse;
  context.axis = msg->axis;

  // Correct the error filter
  error->second.a_priori_step(dt);
  for (size_t i = 0; i < data.size(); i++) {
    // Set the context correctly
    context.sensor[0] = tracker->second.sensors[6 * data[i].sensor + 0];
    context.sensor[1] = tracker->second.sensors[6 * data[i].sensor + 1];
    context.sensor[2] = tracker->second.sensors[6 * data[i].sensor + 2];
    // Create the observation
    Observation obs;
    obs.set_field<Angle>(data[i].angle);
    error->second.innovation_step(obs, filter_.state, context);
  }
  error->second.a_posteriori_step();

  // Correct the tracking filter
  filter_.a_priori_step(dt);
  for (size_t i = 0; i < data.size(); i++) {
    // Set the context correctly
    context.sensor[0] = tracker->second.sensors[6 * data[i].sensor + 0];
    context.sensor[1] = tracker->second.sensors[6 * data[i].sensor + 1];
    context.sensor[2] = tracker->second.sensors[6 * data[i].sensor + 2];
    // Create the observation
    Observation obs;
    obs.set_field<Angle>(data[i].angle);
    filter_.innovation_step(obs, error->second.state, context);
  }
  filter_.a_posteriori_step();
}
// This will be called at approximately 250Hz
void ImuCallback(sensor_msgs::Imu::ConstPtr const& msg) {
  static double dt;
  if ((!use_accelerometer_ && !use_gyroscope_) || !initialized_ || !Delta(dt))
    return;

  // Check that we are recording and that the tracker/lighthouse is ready
  TrackerMap::iterator tracker = trackers_.find(msg->header.frame_id);
  if (tracker == trackers_.end() || !tracker->second.ready) {
    ROS_INFO_STREAM_THROTTLE(1, "Tracker not found or ready");
    return;
  }

  // Make sure we have a filter setup for this
  ErrorMap::iterator error = errors_.find(msg->header.frame_id);
  if (error == errors_.end()) {
    ROS_INFO_STREAM_THROTTLE(1, "Tracker error filter not initialized");
    return;
  }

  // Get the measurements
  Eigen::Vector3d acc(
    msg->linear_acceleration.x,
    msg->linear_acceleration.y,
    msg->linear_acceleration.z);
  Eigen::Vector3d gyr(
    msg->angular_velocity.x,
    msg->angular_velocity.y,
    msg->angular_velocity.z);

  // Set the context correctly
  Context context;
  context.tracker = msg->header.frame_id;

  // Create a measurement
  Observation obs;
  if (use_accelerometer_)
    obs.set_field<Accelerometer>(acc);
  if (use_gyroscope_)
    obs.set_field<Gyroscope>(gyr);

  // Step the parameter filter
  error->second.a_priori_step(dt);
  error->second.innovation_step(obs, filter_.state, context);
  error->second.a_posteriori_step();

  // Propagate the filter
  filter_.a_priori_step(dt);
  filter_.innovation_step(obs, error->second.state, context);
  filter_.a_posteriori_step();
}

// This will be called back at the desired tracking rate
void TimerCallback(ros::TimerEvent const& info) {
  static double dt;
  if (!initialized_ || !Delta(dt))
    return;

  // Propagate the filter forward
  filter_.a_priori_step(dt);

  // Debug
  /*
  ErrorMap::iterator it;
  for (it = errors_.begin(); it != errors_.end(); it++) {
    ROS_INFO_STREAM(it->first << ":");
    ROS_INFO_STREAM(it->second.state);
  }
  ROS_INFO_STREAM(filter_.state);
  ROS_INFO_STREAM("Filter:");
  ROS_INFO_STREAM(filter_.state);
  */

  // The filter relates WORLD and IMU frames
  ros::Time now = ros::Time::now();

  // Broadcast the tracker pose on TF2
  static tf2_ros::TransformBroadcaster br;
  geometry_msgs::TransformStamped tfs;
  tfs.header.stamp = now;
  tfs.header.frame_id = frame_world_;
  tfs.child_frame_id = frame_truth_;
  tfs.transform.translation.x = filter_.state.get_field<Position>()[0];
  tfs.transform.translation.y = filter_.state.get_field<Position>()[1];
  tfs.transform.translation.z = filter_.state.get_field<Position>()[2];
  tfs.transform.rotation.w = filter_.state.get_field<Attitude>().w();
  tfs.transform.rotation.x = filter_.state.get_field<Attitude>().x();
  tfs.transform.rotation.y = filter_.state.get_field<Attitude>().y();
  tfs.transform.rotation.z = filter_.state.get_field<Attitude>().z();
  SendDynamicTransform(tfs);

  // Broadcast the pose with covariance
  geometry_msgs::PoseWithCovarianceStamped pwcs;
  pwcs.header.stamp = now;
  pwcs.header.frame_id = frame_world_;
  pwcs.pose.pose.position.x = filter_.state.get_field<Position>()[0];
  pwcs.pose.pose.position.y = filter_.state.get_field<Position>()[1];
  pwcs.pose.pose.position.z = filter_.state.get_field<Position>()[2];
  pwcs.pose.pose.orientation.w = filter_.state.get_field<Attitude>().w();
  pwcs.pose.pose.orientation.x = filter_.state.get_field<Attitude>().x();
  pwcs.pose.pose.orientation.y = filter_.state.get_field<Attitude>().y();
  pwcs.pose.pose.orientation.z = filter_.state.get_field<Attitude>().z();
  for (size_t i = 0; i < 6; i++)
    for (size_t j = 0; j < 6; j++)
      pwcs.pose.covariance[i*6 + j] = filter_.covariance(i, j);
  pub_pose_.publish(pwcs);

  // Broadcast the twist with covariance
  geometry_msgs::TwistWithCovarianceStamped twcs;
  twcs.header.stamp = now;
  twcs.header.frame_id = frame_world_;
  twcs.twist.twist.linear.x = filter_.state.get_field<Velocity>()[0];
  twcs.twist.twist.linear.y = filter_.state.get_field<Velocity>()[1];
  twcs.twist.twist.linear.z = filter_.state.get_field<Velocity>()[2];
  twcs.twist.twist.angular.x = filter_.state.get_field<Omega>()[0];
  twcs.twist.twist.angular.y = filter_.state.get_field<Omega>()[1];
  twcs.twist.twist.angular.z = filter_.state.get_field<Omega>()[2];
  for (size_t i = 0; i < 6; i++)
    for (size_t j = 0; j < 6; j++)
      twcs.twist.covariance[i*6 + j] = filter_.covariance(6+i, 6+j);
  pub_twist_.publish(twcs);
}

void CheckIfReadyToTrack() {
  if (!initialized_) {
    TrackerMap::const_iterator it;
    for (it = trackers_.begin(); it != trackers_.end(); it++)
      if (!it->second.ready) return;
    LighthouseMap::const_iterator jt;
    for (jt = lighthouses_.begin(); jt != lighthouses_.end(); jt++)
      if (!jt->second.ready) return;
    ROS_INFO_STREAM("All trackers and lighthouses found. Tracking started.");
    initialized_ = true;
  }
}

// Convert an angle axis to an eigen transform
Eigen::Affine3d AngleAxisToTransform(double data[6]) {
  Eigen::Affine3d tf = Eigen::Affine3d::Identity();
  tf.translation() = Eigen::Vector3d(data[0], data[1], data[2]);
  Eigen::Vector3d v(data[3], data[4], data[5]);
  if (v.norm() > 0) {
    Eigen::AngleAxisd aa = Eigen::AngleAxisd::Identity();
    aa.angle() = v.norm();
    aa.axis() = v.normalized();
    tf.linear() = aa.toRotationMatrix();
  }
  return tf;
}

// Called when a new lighthouse appears
void NewLighthouseCallback(LighthouseMap::iterator lighthouse) {
  ROS_INFO_STREAM("Found lighthouse " << lighthouse->first);
  // Initialize
  vTl_[lighthouse->first] = AngleAxisToTransform(lighthouse->second.vTl);
  // Check if we have got all info from lighthouses and trackers
  CheckIfReadyToTrack();
}

// Called when a new tracker appears
void NewTrackerCallback(TrackerMap::iterator tracker) {
  ROS_INFO_STREAM("Found tracker " << tracker->first);
  // Initialize the error filter
  ErrorFilter & error = errors_[tracker->first];
  error.state.set_field<AccelerometerBias>(UKF::Vector<3>(
    tracker->second.errors[ERROR_ACC_BIAS][0],
    tracker->second.errors[ERROR_ACC_BIAS][1],
    tracker->second.errors[ERROR_ACC_BIAS][2]));
  error.state.set_field<AccelerometerScale>(UKF::Vector<3>(
    tracker->second.errors[ERROR_ACC_SCALE][0],
    tracker->second.errors[ERROR_ACC_SCALE][1],
    tracker->second.errors[ERROR_ACC_SCALE][2]));
  error.state.set_field<GyroscopeBias>(UKF::Vector<3>(
    tracker->second.errors[ERROR_GYR_BIAS][0],
    tracker->second.errors[ERROR_GYR_BIAS][1],
    tracker->second.errors[ERROR_GYR_BIAS][2]));
  error.state.set_field<GyroscopeScale>(UKF::Vector<3>(
    tracker->second.errors[ERROR_GYR_SCALE][0],
    tracker->second.errors[ERROR_GYR_SCALE][1],
    tracker->second.errors[ERROR_GYR_SCALE][2]));
  error.covariance = Error::CovarianceMatrix::Zero();
  error.covariance.diagonal() <<
    imu_cov_ab_, imu_cov_as_, imu_cov_gb_, imu_cov_gs_;
  error.process_noise_covariance = Error::CovarianceMatrix::Zero();
  error.process_noise_covariance.diagonal() <<
    imu_proc_ab_, imu_proc_as_, imu_proc_gb_, imu_proc_gs_;
  // Global cache
  bTh_[tracker->first] = AngleAxisToTransform(tracker->second.bTh);
  tTh_[tracker->first] = AngleAxisToTransform(tracker->second.tTh);
  tTi_[tracker->first] = AngleAxisToTransform(tracker->second.tTi);
  // Check if we have got all info from lighthouses and trackers
  CheckIfReadyToTrack();
}

// MAIN ENTRY POINT OF APPLICATION

bool GetPairParam(ros::NodeHandle &nh,
  std::string const& name, UKF::Vector<2> & data) {
  std::vector<double> tmp;
  if (!nh.getParam(name, tmp) || tmp.size() != 2)
    return false;
  data[0] = tmp[0];
  data[1] = tmp[1];
  return true;
}

bool GetVectorParam(ros::NodeHandle &nh,
  std::string const& name, UKF::Vector<3> & data) {
  std::vector<double> tmp;
  if (!nh.getParam(name, tmp) || tmp.size() != 3)
    return false;
  data[0] = tmp[0];
  data[1] = tmp[1];
  data[2] = tmp[2];
  return true;
}

bool GetQuaternionParam(ros::NodeHandle &nh,
  std::string const& name, UKF::Quaternion & data) {
  std::vector<double> tmp;
  if (!nh.getParam(name, tmp) || tmp.size() != 4)
    return false;
  data.x() = tmp[0];
  data.y() = tmp[1];
  data.z() = tmp[2];
  data.w() = tmp[3];
  return true;
}

int main(int argc, char **argv) {
  // Initialize ROS and create node handle
  ros::init(argc, argv, "deepdive_tracker");
  ros::NodeHandle nh("~");

  // Get the parent information
  if (!nh.getParam("calfile", calfile_))
    ROS_FATAL("Failed to get the calfile file.");

  // Get some global information
  if (!nh.getParam("frames/world", frame_world_))
    ROS_FATAL("Failed to get frames/world parameter.");
  if (!nh.getParam("frames/vive", frame_vive_))
    ROS_FATAL("Failed to get frames/vive parameter.");
  if (!nh.getParam("frames/body", frame_body_))
    ROS_FATAL("Failed to get frames/body parameter.");
  if (!nh.getParam("frames/truth", frame_truth_))
    ROS_FATAL("Failed to get frames/truth parameter.");

  // Get the parent information
  std::vector<std::string> lighthouses;
  if (!nh.getParam("lighthouses", lighthouses))
    ROS_FATAL("Failed to get the lighthouse list.");
  std::vector<std::string>::iterator it;
  for (it = lighthouses.begin(); it != lighthouses.end(); it++) {
    std::string serial;
    if (!nh.getParam(*it + "/serial", serial))
      ROS_FATAL("Failed to get the lighthouse serial.");
    std::vector<double> transform;
    if (!nh.getParam(*it + "/transform", transform))
      ROS_FATAL("Failed to get the lighthouse transform.");
    if (transform.size() != 7) {
      ROS_FATAL("Failed to parse lighthouse transform.");
      continue;
    }
    Eigen::Quaterniond q(
      transform[6],  // qw
      transform[3],  // qx
      transform[4],  // qy
      transform[5]); // qz
    Eigen::AngleAxisd aa(q);
    lighthouses_[serial].vTl[0] = transform[0];
    lighthouses_[serial].vTl[1] = transform[1];
    lighthouses_[serial].vTl[2] = transform[2];
    lighthouses_[serial].vTl[3] = aa.angle() * aa.axis()[0];
    lighthouses_[serial].vTl[4] = aa.angle() * aa.axis()[1];
    lighthouses_[serial].vTl[5] = aa.angle() * aa.axis()[2];
    lighthouses_[serial].ready = false;
  }

  // Get the parent information
  std::vector<std::string> trackers;
  if (!nh.getParam("trackers", trackers))
    ROS_FATAL("Failed to get the tracker list.");
  std::vector<std::string>::iterator jt;
  for (jt = trackers.begin(); jt != trackers.end(); jt++) {
    std::string serial;
    if (!nh.getParam(*jt + "/serial", serial))
      ROS_FATAL("Failed to get the tracker serial.");
    std::vector<double> extrinsics;
    if (!nh.getParam(*jt + "/extrinsics", extrinsics))
      ROS_FATAL("Failed to get the tracker extrinsics.");
    if (extrinsics.size() != 7) {
      ROS_FATAL("Failed to parse tracker extrinsics.");
      continue;
    }
    Eigen::Quaterniond q(
      extrinsics[6],  // qw
      extrinsics[3],  // qx
      extrinsics[4],  // qy
      extrinsics[5]); // qz
    Eigen::AngleAxisd aa(q);
    trackers_[serial].bTh[0] = extrinsics[0];
    trackers_[serial].bTh[1] = extrinsics[1];
    trackers_[serial].bTh[2] = extrinsics[2];
    trackers_[serial].bTh[3] = aa.angle() * aa.axis()[0];
    trackers_[serial].bTh[4] = aa.angle() * aa.axis()[1];
    trackers_[serial].bTh[5] = aa.angle() * aa.axis()[2];
    trackers_[serial].ready = false;
  }

  // Get the topics for data topics
  std::string topic_pose, topic_twist;
  if (!nh.getParam("topics/pose", topic_pose))
    ROS_FATAL("Failed to get topics/pose parameter.");
  if (!nh.getParam("topics/twist", topic_twist))
    ROS_FATAL("Failed to get topics/twist parameter.");

  // Get the thresholds
  if (!nh.getParam("thresholds/angle", thresh_angle_))
    ROS_FATAL("Failed to get thresholds/angle parameter.");
  if (!nh.getParam("thresholds/duration", thresh_duration_))
    ROS_FATAL("Failed to get thresholds/duration parameter.");
  if (!nh.getParam("thresholds/count", thresh_count_))
    ROS_FATAL("Failed to get thresholds/count parameter.");

  // Whether to apply light corrections
  if (!nh.getParam("correct", correct_))
    ROS_FATAL("Failed to get correct parameter.");

  // Get the tracker update rate.
  if (!nh.getParam("rate", rate_))
    ROS_FATAL("Failed to get rate parameter.");

  // Get the tracker update rate.
  if (!nh.getParam("use/gyroscope", use_gyroscope_))
    ROS_FATAL("Failed to get use/gyroscope  parameter.");
  if (!nh.getParam("use/accelerometer", use_accelerometer_))
    ROS_FATAL("Failed to get use/accelerometer  parameter.");
  if (!nh.getParam("use/light", use_light_))
    ROS_FATAL("Failed to get use/light parameter.");

  // Get gravity
  if (!GetVectorParam(nh, "gravity", gravity_))
    ROS_FATAL("Failed to get gravity parameter.");

  // Tracking filter: Initial estimates
  UKF::Vector<3> est_position(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/position", est_position))
    ROS_FATAL("Failed to get position parameter.");
  UKF::Quaternion est_attitude(1, 0, 0, 0);
  if (!GetQuaternionParam(nh, "initial_estimate/attitude", est_attitude))
    ROS_FATAL("Failed to get attitude parameter.");
  UKF::Vector<3> est_velocity(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/velocity", est_velocity))
    ROS_FATAL("Failed to get velocity parameter.");
  UKF::Vector<3> est_omega(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/omega", est_omega))
    ROS_FATAL("Failed to get angular_velocity parameter.");
  UKF::Vector<3> est_acceleration(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/acceleration", est_acceleration))
    ROS_FATAL("Failed to get acceleration parameter.");
  UKF::Vector<3> est_alpha(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/alpha", est_alpha))
    ROS_FATAL("Failed to get alpha parameter.");

  // Tracking filter:  Initial covariances
  UKF::Vector<3> cov_position(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/position", cov_position))
    ROS_FATAL("Failed to get position parameter.");
  UKF::Vector<3> cov_attitude(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/attitude", cov_attitude))
    ROS_FATAL("Failed to get attitude parameter.");
  UKF::Vector<3> cov_velocity(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/velocity", cov_velocity))
    ROS_FATAL("Failed to get velocity parameter.");
  UKF::Vector<3> cov_omega(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/omega", cov_omega))
    ROS_FATAL("Failed to get omega parameter.");
  UKF::Vector<3> cov_accel(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/acceleration", cov_accel))
    ROS_FATAL("Failed to get acceleration parameter.");
  UKF::Vector<3> cov_alpha(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/alpha", cov_alpha))
    ROS_FATAL("Failed to get acceleration parameter.");

  // Tracking filter:  Noise
  UKF::Vector<3> noise_position(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/position", noise_position))
    ROS_FATAL("Failed to get position parameter.");
  UKF::Vector<3> noise_attitude(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/attitude", noise_attitude))
    ROS_FATAL("Failed to get attitude parameter.");
  UKF::Vector<3> noise_velocity(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/velocity", noise_velocity))
    ROS_FATAL("Failed to get velocity parameter.");
  UKF::Vector<3> noise_omega(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/omega", noise_omega))
    ROS_FATAL("Failed to get omega parameter.");
  UKF::Vector<3> noise_accel(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/acceleration", noise_accel))
    ROS_FATAL("Failed to get acceleration parameter.");
  UKF::Vector<3> noise_alpha(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/alpha", noise_alpha))
    ROS_FATAL("Failed to get acceleration parameter.");

  // Setup the filter
  filter_.state.set_field<Position>(est_position);
  filter_.state.set_field<Attitude>(est_attitude);
  filter_.state.set_field<Velocity>(est_velocity);
  filter_.state.set_field<Omega>(est_omega);
  filter_.state.set_field<Acceleration>(est_acceleration);
  filter_.state.set_field<Alpha>(est_alpha);
  filter_.covariance = State::CovarianceMatrix::Zero();
  filter_.covariance.diagonal() <<
    cov_position, cov_attitude,
    cov_velocity, cov_omega,
    cov_accel, cov_alpha;
  filter_.process_noise_covariance = State::CovarianceMatrix::Zero();
  filter_.process_noise_covariance.diagonal() <<
    noise_position, noise_attitude,
    noise_velocity, noise_omega,
    noise_accel, noise_alpha;

  // IMU error : initial estimate
  if (!GetVectorParam(nh, "measurement_cov/accelerometer", obs_cov_acc_))
    ROS_FATAL("Failed to get accelerometer parameter.");
  if (!GetVectorParam(nh, "measurement_cov/gyroscope", obs_cov_gyr_))
    ROS_FATAL("Failed to get gyroscope parameter.");
  if (!nh.getParam("measurement_cov/angle", obs_cov_ang_))
    ROS_FATAL("Failed to get angle parameter.");

  // IMU error : initial estimate
  if (!GetVectorParam(nh, "imu_initial_cov/acc_bias", imu_cov_ab_))
    ROS_FATAL("Failed to get acc_bias parameter.");
  if (!GetVectorParam(nh, "imu_initial_cov/acc_scale", imu_cov_as_))
    ROS_FATAL("Failed to get acc_scale parameter.");
  if (!GetVectorParam(nh, "imu_initial_cov/gyr_bias", imu_cov_gb_))
    ROS_FATAL("Failed to get gyr_bias parameter.");
  if (!GetVectorParam(nh, "imu_initial_cov/gyr_scale", imu_cov_gs_))
    ROS_FATAL("Failed to get gyr_scale parameter.");

  // IMU error : initial estimate
  if (!GetVectorParam(nh, "imu_process_noise_cov/acc_bias", imu_proc_ab_))
    ROS_FATAL("Failed to get acc_bias parameter.");
  if (!GetVectorParam(nh, "imu_process_noise_cov/acc_scale", imu_proc_as_))
    ROS_FATAL("Failed to get acc_scale parameter.");
  if (!GetVectorParam(nh, "imu_process_noise_cov/gyr_bias", imu_proc_gb_))
    ROS_FATAL("Failed to get gyr_bias parameter.");
  if (!GetVectorParam(nh, "imu_process_noise_cov/gyr_scale", imu_proc_gs_))
    ROS_FATAL("Failed to get gyr_scale parameter.");

  // If reading the configuration file results in inserting the correct
  // number of static transforms into the problem, then we can publish
  // the solution for use by other entities in the system.
  if (ReadConfig(calfile_, frame_world_, frame_vive_, frame_body_,
    registration_, lighthouses_, trackers_)) {
    ROS_INFO("Read transforms from calibration");
  } else {
    ROS_INFO("Could not read calibration file");
  }
  SendTransforms(frame_world_, frame_vive_, frame_body_,
    registration_, lighthouses_, trackers_);

  // Convert the registration info to a world transform
  wTv_ = AngleAxisToTransform(registration_);

  // Markers showing sensor positions
  pub_pose_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>
    (topic_pose, 0);
  pub_twist_ = nh.advertise<geometry_msgs::TwistWithCovarianceStamped>
    (topic_twist, 0);

  // Subscribe to the motion and light callbacks
  std::vector<ros::Subscriber> subs;
  subs.push_back(nh.subscribe<deepdive_ros::Trackers>("/trackers", 1000,
    std::bind(TrackerCallback, std::placeholders::_1,
      std::ref(trackers_), NewTrackerCallback)));
  subs.push_back(nh.subscribe<deepdive_ros::Lighthouses>("/lighthouses", 1000,
    std::bind(LighthouseCallback, std::placeholders::_1,
      std::ref(lighthouses_), NewLighthouseCallback)));
  subs.push_back(nh.subscribe("/light", 1000, LightCallback));
  subs.push_back(nh.subscribe("/imu", 1000, ImuCallback));

  // Start a timer to callback
  ros::Timer timer = nh.createTimer(
    ros::Duration(ros::Rate(rate_)), TimerCallback, false, true);

  // Block until safe shutdown
  ros::spin();

  // Success!
  return 0;
}

max_stars_repo_name: asymingt/deepdive

max_issues_repo_name: asymingt/deepdive

max_forks_repo_name: asymingt/deepdive