text:
/***********************************************************************************
 * Copyright (c) 2018, UT-Battelle
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the xacc nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Contributors:
 *   Initial implementation - H. Charles Zhao
 *
 **********************************************************************************/

#include <boost/math/constants/constants.hpp>
#include "exprtk.hpp"
#include "QuilToXACCListener.hpp"
#include "QuilBaseListener.h"
#include "XACC.hpp"
#include "IRProvider.hpp"
#include "IR.hpp"

using namespace quil;

using symbol_table_t = exprtk::symbol_table<double>;
using expression_t = exprtk::expression<double>;
using parser_t = exprtk::parser<double>;

namespace xacc {
    namespace quantum {
        constexpr static double pi = boost::math::constants::pi<double>();

        QuilToXACCListener::QuilToXACCListener(std::shared_ptr<xacc::IR> ir) : ir(ir) {
            gateRegistry = xacc::getService<IRProvider>("gate");
        }

        double evalMathExpression(const std::string &expression) {
            symbol_table_t symbol_table;
            symbol_table.add_constant("pi", pi);
            expression_t expr;
            expr.register_symbol_table(symbol_table);
            parser_t parser;
            parser.compile(expression, expr);
            return expr.value();
        }

        InstructionParameter strToParam(const std::string &str) {
            double num = evalMathExpression(str);
            if (std::isnan(num)) {
                return InstructionParameter(str);
            } else {
                return InstructionParameter(num);
            }
        }

        void QuilToXACCListener::enterXacckernel(quil::QuilParser::XacckernelContext *ctx) {
            std::vector<InstructionParameter> params;
            for (int i = 0; i < ctx->typedparam().size(); i++) {
                params.push_back(InstructionParameter(ctx->typedparam(static_cast<size_t>(i))->IDENTIFIER()->getText()));
            }
            curFunc = gateRegistry->createFunction(ctx->kernelname->getText(), {}, params);
            functions.insert({curFunc->name(), curFunc});
        }

        void QuilToXACCListener::exitXacckernel(quil::QuilParser::XacckernelContext *ctx) {
            ir->addKernel(curFunc);
        }

        void QuilToXACCListener::exitKernelcall(quil::QuilParser::KernelcallContext *ctx) {
            std::string gateName = ctx->kernelname->getText();
            if (functions.count(gateName)) {
                curFunc->addInstruction(functions[gateName]);
            } else {
                xacc::error("Tried calling an undefined kernel.");
            }
        }

        void QuilToXACCListener::exitGate(quil::QuilParser::GateContext *ctx) {
            std::string gateName = ctx->name()->getText();
            if (gateName == "RX") gateName = "Rx";
            if (gateName == "RY") gateName = "Ry";
            if (gateName == "RZ") gateName = "Rz";

            std::vector<int> qubits;
            for (int i = 0; i < ctx->qubit().size(); i++) {
                qubits.push_back(std::stoi(ctx->qubit(i)->getText()));
            }

            std::shared_ptr<xacc::Instruction> instruction = gateRegistry->createInstruction(gateName, qubits);
            InstructionParameter param;
            for (int i = 0; i < ctx->param().size(); i++) {
                param = strToParam(ctx->param(static_cast<size_t>(i))->getText());
                instruction->setParameter(i, param);
            }

            curFunc->addInstruction(instruction);
        }

        void QuilToXACCListener::exitMeasure(quil::QuilParser::MeasureContext *ctx) {
            std::vector<int> qubits;
            qubits.push_back(std::stoi(ctx->qubit()->getText()));
            std::vector<InstructionParameter> cbits;
            if (ctx->addr() != nullptr) {
                cbits.push_back(InstructionParameter(std::stoi(ctx->addr()->classicalBit()->getText())));
            }

            std::shared_ptr<xacc::Instruction> instruction = gateRegistry->createInstruction("Measure", qubits, cbits);
            curFunc->addInstruction(instruction);
        }
    }
}


text:
#include <vld.h>
#include "flcliapi.hpp"
#include "logger/logger.hpp"
#include "logger/logger_define.hpp"
#include "utils/config_parser.hpp"
#include "utils/zmqutil.hpp"
#include "flp_constant.hpp"
#include <boost/algorithm/string.hpp>

int main(int argc, char* argv[])
{
	using namespace std::string_literals;
	using namespace sg;
	logger::get_instance();
	try {
		if (argc < 2) {
			SPDLOG_INFO("Usage: flclient config.json");
			return EXIT_FAILURE;
		}

		// Load config file
		sg::config_parser conf;
		conf.load(argv[1]);
		zmqpp::context_t ctx;

		sg::flcliapi cli(ctx);
		cli.connect(conf.get_value(server_endpoint));
		cli.connect(conf.get_value(server_endpoint2));
		cli.connect(conf.get_value(server_endpoint3));
		for (int i = 1; i < 100000; ++i) {
			zmqpp::message_t msg;
			msg << std::format("hello {}", i);
			auto reply = cli.request(msg);
			std::this_thread::sleep_for(std::chrono::milliseconds(2000));
		}
	}
	catch (const std::exception& ex) {
		SPDLOG_ERROR(ex.what());
	}
	return EXIT_SUCCESS;
}

ext: cpp

max_stars_repo_name: pvthuyet/ZeroMQ-RFC-Implementation

max_issues_repo_name: pvthuyet/ZeroMQ-RFC-Implementation

max_forks_repo_name: pvthuyet/ZeroMQ-RFC-Implementation


text:
/*
 * neonGX - Color.cpp
 *
 * Copyright (c) 2016, cynecx
 *
 * This file is part of neonGX and is distributed under a modified
 * BSD 3-Clause License. See LICENSE.txt for details.
 *
 */

#include <neonGX/Core/Graphics/Color.hpp>
#include <boost/format.hpp>

namespace neonGX {

  std::string ColorRGB::ToString() const {
    using namespace std;
    using namespace boost;

    stringstream outputStr;
    outputStr << format("ColorRGB{r: %1%, g: %2%, b: %3%}")
                 % int(r) % int(g) % int(b);

    return outputStr.str();
  }

  std::string ColorRGBA::ToString() const {
    using namespace std;
    using namespace boost;

    stringstream outputStr;
    outputStr << format("ColorRGBA{r: %1%, g: %2%, b: %3%, a: %4%}")
                 % int(r * 255.0f) % int(g * 255.0f)
                 % int(b * 255.0f) % a;

    return outputStr.str();
  }
}

max_stars_repo_name: cynecx/neonGX

max_issues_repo_name: cynecx/neonGX

max_forks_repo_name: cynecx/neonGX


text:
// Copyright (C) 2009-2012 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0
// (see accompanying file LICENSE_1_0.txt or a copy at
// http://www.boost.org/LICENSE_1_0.txt)
// Home at http://www.boost.org/libs/local_function

#ifndef BOOST_LOCAL_FUNCTION_AUX_PP_DECL_TRAITS_HPP_
#define BOOST_LOCAL_FUNCTION_AUX_PP_DECL_TRAITS_HPP_

#include <boost/local_function/aux_/preprocessor/traits/decl_sign_/sign.hpp>
#include <boost/local_function/aux_/preprocessor/traits/decl_/nil.hpp>
#include <boost/preprocessor/control/iif.hpp>
#include <boost/preprocessor/tuple/eat.hpp>
#include <boost/preprocessor/list/adt.hpp>

// PUBLIC //

// Expand: decl_traits (see DECL_TRAITS macros to inspect these traits).
#define BOOST_LOCAL_FUNCTION_AUX_PP_DECL_TRAITS(declarations) \
    BOOST_PP_IIF(BOOST_PP_LIST_IS_NIL(declarations), \
        BOOST_LOCAL_FUNCTION_AUX_PP_DECL_TRAITS_NIL \
        BOOST_PP_TUPLE_EAT(1) \
    , \
        BOOST_LOCAL_FUNCTION_AUX_PP_DECL_TRAITS_SIGN \
    )(declarations)

#endif // #include guard

ext: hpp


text:
#ifndef BOOST_MPL_AUX_BEGIN_END_IMPL_HPP_INCLUDED
#define BOOST_MPL_AUX_BEGIN_END_IMPL_HPP_INCLUDED

// Copyright Aleksey Gurtovoy 2000-2004
//
// 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)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Source: /CVSROOT/boost/boost/mpl/aux_/begin_end_impl.hpp,v $
// $Date: 2007/10/29 07:32:40 $
// $Revision: 1.1.1.1 $

#include <boost/mpl/begin_end_fwd.hpp>
#include <boost/mpl/sequence_tag_fwd.hpp>
#include <boost/mpl/void.hpp>
#include <boost/mpl/aux_/na.hpp>
#include <boost/mpl/aux_/traits_lambda_spec.hpp>
#include <boost/mpl/aux_/config/eti.hpp>

namespace boost { namespace mpl {

// default implementation; conrete sequences might override it by
// specializing either the 'begin_impl/end_impl' or the primary
// 'begin/end' templates

template< typename Tag >
struct begin_impl
{
    template< typename Sequence > struct apply
    {
        typedef typename Sequence::begin type;
    };
};

template< typename Tag >
struct end_impl
{
    template< typename Sequence > struct apply
    {
        typedef typename Sequence::end type;
    };
};

// specialize 'begin_trait/end_trait' for two pre-defined tags

#   define AUX778076_IMPL_SPEC(name, tag, result) \
template<> \
struct name##_impl<tag> \
{ \
    template< typename Sequence > struct apply \
    { \
        typedef result type; \
    }; \
}; \
/**/

// a sequence with nested 'begin/end' typedefs; just query them
AUX778076_IMPL_SPEC(begin, nested_begin_end_tag, typename Sequence::begin)
AUX778076_IMPL_SPEC(end, nested_begin_end_tag, typename Sequence::end)

// if a type 'T' does not contain 'begin/end' or 'tag' members
// and doesn't specialize either 'begin/end' or 'begin_impl/end_impl'
// templates, then we end up here
AUX778076_IMPL_SPEC(begin, non_sequence_tag, void_)
AUX778076_IMPL_SPEC(end, non_sequence_tag, void_)
AUX778076_IMPL_SPEC(begin, na, void_)
AUX778076_IMPL_SPEC(end, na, void_)

#   undef AUX778076_IMPL_SPEC

BOOST_MPL_ALGORITM_TRAITS_LAMBDA_SPEC(1,begin_impl)
BOOST_MPL_ALGORITM_TRAITS_LAMBDA_SPEC(1,end_impl)

}}

#endif // BOOST_MPL_AUX_BEGIN_END_IMPL_HPP_INCLUDED

max_stars_repo_name: hackerlank/SourceCode

max_issues_repo_name: shacojx/SourceCodeGameTLBB

max_forks_repo_name: shacojx/SourceCodeGameTLBB


text: #include <boost/spirit/include/classic_if.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
//******************************************************************************
// RCF - Remote Call Framework
//
// Copyright (c) 2005 - 2013, Delta V Software. All rights reserved.
// http://www.deltavsoft.com
//
// RCF is distributed under dual licenses - closed source or GPL.
// Consult your particular license for conditions of use.
//
// If you have not purchased a commercial license, you are using RCF
// under GPL terms.
//
// Version: 2.0
// Contact: support <at> deltavsoft.com
//
//******************************************************************************

#ifndef INCLUDE_SF_STREAM_HPP
#define INCLUDE_SF_STREAM_HPP

#include <map>
#include <string>

#include <boost/noncopyable.hpp>

#include <RCF/Export.hpp>

#include <SF/DataPtr.hpp>
#include <SF/Encoding.hpp>
#include <SF/I_Stream.hpp>

#include <iosfwd>

namespace RCF {

    class SerializationProtocolIn;
    class SerializationProtocolOut;

    class MemIstream;
    class MemOstream;

}
namespace SF {

    //**************************************************
    // Encoding of object data

    template<typename E>
    class Encoding : public I_Encoding
    {
    public:
        virtual UInt32 getCount(
            DataPtr &               data,
            const std::type_info &  type)
        {
            return countElements( (E *) 0, data, type);
        }

        virtual void toData(
            DataPtr &               data,
            void *                  pvObject,
            const std::type_info &  type,
            int                     nCount)
        {
            encodeElements( (E *) 0, data, pvObject, type, nCount);
        }

        virtual void toObject(
            DataPtr &               data,
            void *                  pvObject,
            const std::type_info &  type,
            int                     nCount)
        {
            decodeElements( (E *) 0, data, pvObject, type, nCount);
        }
    };

    //**************************************************
    // Context handling

    class RCF_EXPORT ContextRead
    {
    public:
        ContextRead();
        ~ContextRead();
        void add(SF::UInt32 nid, const ObjectId &id);
        void add(void *ptr, const std::type_info &objType, void *pObj);
        bool query(SF::UInt32 nid, ObjectId &id);
        bool query(void *ptr, const std::type_info &objType, void *&pObj);
        void clear();

        void setEnabled(bool enabled);
        bool getEnabled() const;

    private:
        bool mEnabled;
        std::auto_ptr<std::map<UInt32, ObjectId> >                          mNidToIdMap;
        std::auto_ptr<std::map<std::string, std::map< void *, void * > > >  mTypeToObjMap;
    };

    class RCF_EXPORT ContextWrite
    {
    public:
        ContextWrite();
        ~ContextWrite();
        void setEnabled(bool enabled);
        bool getEnabled() const;
        void add(const ObjectId &id, UInt32 &nid);
        bool query(const ObjectId &id, UInt32 &nid);
        void clear();
    private:
        bool                                            mEnabled;
        UInt32                                          mCurrentId;
        std::auto_ptr<std::map<ObjectId, UInt32> >      mIdToNidMap;
    };

    //**************************************************
    // Stream local storage

    class RCF_EXPORT LocalStorage : boost::noncopyable
    {
    public:
        LocalStorage();
        ~LocalStorage();
        void setNode(Node *);
        Node *getNode();

    private:
        Node * mpNode;
    };

    //****************************************************
    // Base stream classes

    class Node;
    class SerializerBase;

    class RCF_EXPORT IStream : boost::noncopyable
    {
    public:

        IStream();

        IStream(
            RCF::MemIstream & is,
            std::size_t     archiveSize = 0,
            int             runtimeVersion = 0,
            int             archiveVersion = 0);

        IStream(
            std::istream &  is,
            std::size_t     archiveSize = 0,
            int             runtimeVersion = 0,
            int             archiveVersion = 0);

        virtual ~IStream();

        void        setIs(
                        std::istream &  is,
                        std::size_t     archiveSize = 0,
                        int             runtimeVersion = 0,
                        int             archiveVersion = 0);

        void        clearState();

        UInt32      read(Byte8 *pBytes, UInt32 nLength);

        bool        verifyAgainstArchiveSize(std::size_t bytesToRead);

        bool        begin(Node &node);
        bool        get(DataPtr &value);
        void        end();
        UInt32      read_int(UInt32 &n);
        UInt32      read_byte(Byte8 &byte);
        void        putback_byte(Byte8 byte);

        virtual I_Encoding &
                    getEncoding() = 0;

        int         getRuntimeVersion();
        int         getArchiveVersion();

        void        setArchiveVersion(int archiveVersion);
        void        setRuntimeVersion(int runtimeVersion);

        void        ignoreVersionStamp(bool ignore = true);

        void        setRemoteCallContext(
                        RCF::SerializationProtocolIn * pSerializationProtocolIn);

        RCF::SerializationProtocolIn *
                    getRemoteCallContext();

        ContextRead &           getTrackingContext();
        const ContextRead &     getTrackingContext() const;
        LocalStorage &          getLocalStorage();

        void            setEnablePointerTracking(bool enable);
        bool            getEnablePointerTracking() const;

        // Streaming operators.

        template<typename T>
        IStream & operator>>(T &t);

        template<typename T>
        IStream & operator>>(const T &t);

    private:

        ContextRead         mContextRead;
        LocalStorage        mLocalStorage;

        std::istream *      mpIs;
        std::size_t         mArchiveSize;
        int                 mRuntimeVersion;
        int                 mArchiveVersion;
        bool                mIgnoreVersionStamp;

        RCF::SerializationProtocolIn * mpSerializationProtocolIn;
    };

    class RCF_EXPORT OStream : boost::noncopyable
    {
    public:
        OStream();

        OStream(
            RCF::MemOstream &   os,
            int                 runtimeVersion = 0,
            int                 archiveVersion = 0);

        OStream(
            std::ostream &      os,
            int                 runtimeVersion = 0,
            int                 archiveVersion = 0);

        virtual ~OStream();

        void        setOs(
                        std::ostream &  os,
                        int             runtimeVersion = 0,
                        int             archiveVersion = 0);

        void        clearState();

        UInt32      writeRaw(const Byte8 *pBytes, UInt32 nLength);

        void        begin(const Node &node);
        void        put(const DataPtr &value);
        void        end();
        UInt32      write_int(UInt32 n);
        UInt32      write_byte(Byte8 byte);
        UInt32      write(const Byte8 *pBytes, UInt32 nLength);

        virtual I_Encoding &
                    getEncoding() = 0;

        int         getRuntimeVersion();
        int         getArchiveVersion();

        void        setArchiveVersion(int archiveVersion);
        void        setRuntimeVersion(int runtimeVersion);

        void        suppressArchiveMetadata(bool suppress = true);

        void        setRemoteCallContext(
                        RCF::SerializationProtocolOut * pSerializationProtocolOut);

        RCF::SerializationProtocolOut *
                    getRemoteCallContext();

        ContextWrite &          getTrackingContext();
        const ContextWrite &    getTrackingContext() const;

        LocalStorage &  getLocalStorage();

        void            setEnablePointerTracking(bool enable);
        bool            getEnablePointerTracking() const;

        // Streaming operator.

        template<typename T>
        OStream & operator<<(const T &t);

    private:

        void            writeArchiveMetadata();

        ContextWrite    mContextWrite;
        LocalStorage    mLocalStorage;

        std::ostream *  mpOs;
        int             mRuntimeVersion;
        int             mArchiveVersion;
        bool            mSuppressArchiveMetadata;
        bool            mArchiveMetadataWritten;

        RCF::SerializationProtocolOut * mpSerializationProtocolOut;
    };

} // namespace SF

#include <SF/Archive.hpp>
#include <SF/Serializer.hpp>

namespace SF {

    template<typename T>
    IStream & IStream::operator>>(T &t)
    {
        Archive ar(Archive::READ, this);
        ar & t;
        return *this;
    }

    template<typename T>
    IStream & IStream::operator>>(const T &t)
    {
        Archive ar(Archive::READ, this);
        ar & t;
        return *this;
    }

    template<typename T>
    OStream & OStream::operator<<(const T &t)
    {
        Archive ar(Archive::WRITE, this);
        ar & t;
        return *this;
    }

} // namespace SF

#endif // !INCLUDE_SF_STREAM_HPP

max_stars_repo_name: ASMlover/study

max_issues_repo_name: ASMlover/study

max_forks_repo_name: ASMlover/study


text:
/* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
/*
 * Copyright (c) 2013-2021 Regents of the University of California.
 *
 * This file is part of ndn-cxx library (NDN C++ library with eXperimental eXtensions).
 *
 * ndn-cxx library is free software: you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option) any later version.
 *
 * ndn-cxx library 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 Lesser General Public License for more details.
 *
 * You should have received copies of the GNU General Public License and GNU Lesser
 * General Public License along with ndn-cxx, e.g., in COPYING.md file.  If not, see
 * <http://www.gnu.org/licenses/>.
 *
 * See AUTHORS.md for complete list of ndn-cxx authors and contributors.
 */

#include "ndn-cxx/util/io.hpp"

#include "tests/boost-test.hpp"
#include "tests/key-chain-fixture.hpp"

#include <boost/filesystem.hpp>
#include <boost/mpl/vector.hpp>

namespace ndn {
namespace tests {

BOOST_AUTO_TEST_SUITE(Util)
BOOST_AUTO_TEST_SUITE(TestIo)

struct NoEncoding
{
  const io::IoEncoding encoding{io::NO_ENCODING};
  const std::vector<uint8_t> blob{0xd1, 0x0, 0xb0, 0x1a};
  std::istringstream stream{std::string("\xd1\x00\xb0\x1a", 4), std::ios_base::binary};
};

struct Base64Encoding
{
  const io::IoEncoding encoding = io::BASE64;
  const std::vector<uint8_t> blob{0x42, 0x61, 0x73, 0x65, 0x36, 0x34, 0x45, 0x6e, 0x63};
  std::istringstream stream{"QmFzZTY0RW5j\n", std::ios_base::binary};
};

struct HexEncoding
{
  const io::IoEncoding encoding = io::HEX;
  const std::vector<uint8_t> blob{0x48, 0x65, 0x78, 0x45, 0x6e, 0x63};
  std::istringstream stream{"486578456E63", std::ios_base::binary};
};

using Encodings = boost::mpl::vector<NoEncoding, Base64Encoding, HexEncoding>;

BOOST_AUTO_TEST_CASE_TEMPLATE(LoadBuffer, T, Encodings)
{
  T t;
  shared_ptr<Buffer> buf = io::loadBuffer(t.stream, t.encoding);
  BOOST_CHECK_EQUAL_COLLECTIONS(buf->begin(), buf->end(), t.blob.begin(), t.blob.end());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(SaveBuffer, T, Encodings)
{
  T t;
  std::ostringstream os(std::ios_base::binary);
  io::saveBuffer(t.blob.data(), t.blob.size(), os, t.encoding);
  BOOST_CHECK_EQUAL(os.str(), t.stream.str());
}

BOOST_AUTO_TEST_CASE(LoadBufferException)
{
  std::ifstream in("this-file-does-not-exist", std::ios_base::binary);
  BOOST_CHECK_THROW(io::loadBuffer(in, io::NO_ENCODING), io::Error);
}

BOOST_AUTO_TEST_CASE(SaveBufferException)
{
  class NullStreambuf : public std::streambuf
  {
  };

  NullStreambuf nullbuf;
  std::ostream out(&nullbuf);
  const Buffer buffer(1);
  BOOST_CHECK_THROW(io::saveBuffer(buffer.data(), buffer.size(), out, io::NO_ENCODING), io::Error);
}

BOOST_AUTO_TEST_CASE(UnknownIoEncoding)
{
  std::stringstream ss;
  BOOST_CHECK_THROW(io::loadTlv<Name>(ss, static_cast<io::IoEncoding>(5)), std::invalid_argument);
  BOOST_CHECK_THROW(io::loadBuffer(ss, static_cast<io::IoEncoding>(5)), std::invalid_argument);
  BOOST_CHECK_THROW(io::saveBuffer(nullptr, 0, ss, static_cast<io::IoEncoding>(5)), std::invalid_argument);
}

class FileIoFixture
{
protected:
  FileIoFixture()
    : filepath(boost::filesystem::path(UNIT_TESTS_TMPDIR) / "TestIo")
    , filename(filepath.string())
  {
    boost::filesystem::create_directories(filepath.parent_path());
  }

  ~FileIoFixture()
  {
    boost::system::error_code ec;
    boost::filesystem::remove(filepath, ec); // ignore error
  }

  /**
   * \brief create a directory at `filepath`, so that it's neither readable nor writable as a file
   */
  void
  mkdir() const
  {
    boost::filesystem::create_directory(filepath);
  }

  template<typename Container>
  Container
  readFile() const
  {
    Container container;
    std::ifstream fs(filename, std::ios_base::binary);
    BOOST_REQUIRE_MESSAGE(fs, "error opening file");
    char ch;
    while (fs.get(ch)) {
      container.push_back(static_cast<typename Container::value_type>(ch));
    }
    return container;
  }

  template<typename Container>
  void
  writeFile(const Container& content) const
  {
    std::ofstream fs(filename, std::ios_base::binary);
    BOOST_REQUIRE_MESSAGE(fs, "error opening file");
    for (auto ch : content) {
      fs.put(static_cast<char>(ch));
    }
    BOOST_REQUIRE_MESSAGE(fs, "error writing file");
  }

protected:
  const boost::filesystem::path filepath;
  const std::string filename;
};

BOOST_FIXTURE_TEST_SUITE(FileIo, FileIoFixture)

class EncodableType
{
public:
  Block
  wireEncode() const
  {
    if (shouldThrow) {
      NDN_THROW(tlv::Error("encode error"));
    }

    // block will be 0xAA, 0x01, 0xDD
    return makeNonNegativeIntegerBlock(0xAA, 0xDD);
  }

public:
  bool shouldThrow = false;
};

class DecodableType
{
public:
  DecodableType() = default;

  explicit
  DecodableType(const Block& block)
  {
    wireDecode(block);
  }

  void
  wireDecode(const Block& block)
  {
    // block must be 0xBB, 0x01, 0xEE
    BOOST_CHECK_EQUAL(block.type(), 0xBB);
    BOOST_REQUIRE_EQUAL(block.value_size(), 1);
    BOOST_CHECK_EQUAL(block.value()[0], 0xEE);
  }
};

class DecodableTypeThrow
{
public:
  DecodableTypeThrow() = default;

  explicit
  DecodableTypeThrow(const Block& block)
  {
    wireDecode(block);
  }

  void
  wireDecode(const Block&)
  {
    NDN_THROW(tlv::Error("decode error"));
  }
};

BOOST_AUTO_TEST_CASE(LoadNoEncoding)
{
  this->writeFile<std::vector<uint8_t>>({0xBB, 0x01, 0xEE});
  shared_ptr<DecodableType> decoded = io::load<DecodableType>(filename, io::NO_ENCODING);
  BOOST_CHECK(decoded != nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_NO_THROW(io::loadTlv<DecodableType>(ifs, io::NO_ENCODING));
}

BOOST_AUTO_TEST_CASE(LoadBase64)
{
  this->writeFile<std::string>("uwHu\n"); // printf '\xBB\x01\xEE' | base64
  shared_ptr<DecodableType> decoded = io::load<DecodableType>(filename, io::BASE64);
  BOOST_CHECK(decoded != nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_NO_THROW(io::loadTlv<DecodableType>(ifs, io::BASE64));
}

BOOST_AUTO_TEST_CASE(LoadBase64Newline64)
{
  this->writeFile<std::string>(
    "CEAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA\n"
    "AAAAAAAAAAAA\n");
  // printf '\x08\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00
  //         \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00
  //         \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00
  //         \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00' | base64
  shared_ptr<name::Component> decoded = io::load<name::Component>(filename, io::BASE64);
  BOOST_CHECK(decoded != nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_NO_THROW(io::loadTlv<name::Component>(ifs, io::BASE64));
}

BOOST_AUTO_TEST_CASE(LoadBase64Newline32)
{
  this->writeFile<std::string>(
    "CEAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA\n"
    "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA\n"
    "AAAAAAAAAAAA\n");
  shared_ptr<name::Component> decoded = io::load<name::Component>(filename, io::BASE64);
  BOOST_CHECK(decoded != nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_NO_THROW(io::loadTlv<name::Component>(ifs, io::BASE64));
}

BOOST_AUTO_TEST_CASE(LoadBase64NewlineEnd)
{
  this->writeFile<std::string>(
    "CEAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA\n");
  shared_ptr<name::Component> decoded = io::load<name::Component>(filename, io::BASE64);
  BOOST_CHECK(decoded != nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_NO_THROW(io::loadTlv<name::Component>(ifs, io::BASE64));
}

BOOST_AUTO_TEST_CASE(LoadBase64NoNewline)
{
  this->writeFile<std::string>(
    "CEAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA");
  shared_ptr<name::Component> decoded = io::load<name::Component>(filename, io::BASE64);
  BOOST_CHECK(decoded != nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_NO_THROW(io::loadTlv<name::Component>(ifs, io::BASE64));
}

BOOST_AUTO_TEST_CASE(LoadHex)
{
  this->writeFile<std::string>("BB01EE");
  shared_ptr<DecodableType> decoded = io::load<DecodableType>(filename, io::HEX);
  BOOST_CHECK(decoded != nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_NO_THROW(io::loadTlv<DecodableType>(ifs, io::HEX));
}

BOOST_AUTO_TEST_CASE(LoadDecodeException)
{
  this->writeFile<std::vector<uint8_t>>({0xBB, 0x01, 0xEE});
  shared_ptr<DecodableTypeThrow> decoded;
  BOOST_CHECK_NO_THROW(decoded = io::load<DecodableTypeThrow>(filename, io::NO_ENCODING));
  BOOST_CHECK(decoded == nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_THROW(io::loadTlv<DecodableTypeThrow>(ifs, io::NO_ENCODING), io::Error);
}

BOOST_AUTO_TEST_CASE(LoadNotHex)
{
  this->writeFile<std::string>("not-hex");
  shared_ptr<DecodableType> decoded;
  BOOST_CHECK_NO_THROW(decoded = io::load<DecodableType>(filename, io::HEX));
  BOOST_CHECK(decoded == nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_THROW(io::loadTlv<DecodableType>(ifs, io::HEX), io::Error);
}

BOOST_AUTO_TEST_CASE(LoadEmpty)
{
  this->writeFile<std::vector<uint8_t>>({});
  shared_ptr<DecodableType> decoded;
  BOOST_CHECK_NO_THROW(decoded = io::load<DecodableType>(filename, io::NO_ENCODING));
  BOOST_CHECK(decoded == nullptr);

  std::ifstream ifs(filename);
  BOOST_CHECK_THROW(io::loadTlv<DecodableType>(ifs, io::NO_ENCODING), io::Error);
}

BOOST_AUTO_TEST_CASE(LoadFileNotReadable)
{
  shared_ptr<DecodableType> decoded;
  BOOST_CHECK_NO_THROW(decoded = io::load<DecodableType>(filename, io::NO_ENCODING));
  BOOST_CHECK(decoded == nullptr);
}

BOOST_AUTO_TEST_CASE(SaveNoEncoding)
{
  EncodableType encoded;
  BOOST_CHECK_NO_THROW(io::save(encoded, filename, io::NO_ENCODING));
  auto content = this->readFile<std::vector<uint8_t>>();
  uint8_t expected[] = {0xAA, 0x01, 0xDD};
  BOOST_CHECK_EQUAL_COLLECTIONS(content.begin(), content.end(),
                                expected, expected + sizeof(expected));
}

BOOST_AUTO_TEST_CASE(SaveBase64)
{
  EncodableType encoded;
  BOOST_CHECK_NO_THROW(io::save(encoded, filename, io::BASE64));
  auto content = this->readFile<std::string>();
  BOOST_CHECK_EQUAL(content, "qgHd\n"); // printf '\xAA\x01\xDD' | base64
}

BOOST_AUTO_TEST_CASE(SaveHex)
{
  EncodableType encoded;
  BOOST_CHECK_NO_THROW(io::save(encoded, filename, io::HEX));
  auto content = this->readFile<std::string>();
  BOOST_CHECK_EQUAL(content, "AA01DD");
}

BOOST_AUTO_TEST_CASE(SaveException)
{
  EncodableType encoded;
  encoded.shouldThrow = true;
  BOOST_CHECK_THROW(io::save(encoded, filename, io::NO_ENCODING), io::Error);
}

BOOST_AUTO_TEST_CASE(SaveFileNotWritable)
{
  this->mkdir();
  EncodableType encoded;
  encoded.shouldThrow = true;
  BOOST_CHECK_THROW(io::save(encoded, filename, io::NO_ENCODING), io::Error);
}

BOOST_AUTO_TEST_SUITE_END() // FileIo

class IdCertFixture : public FileIoFixture
                    , public KeyChainFixture
{
};

BOOST_FIXTURE_TEST_CASE(IdCert, IdCertFixture)
{
  auto identity = m_keyChain.createIdentity("/TestIo/IdCert", RsaKeyParams());
  const auto& cert = identity.getDefaultKey().getDefaultCertificate();
  io::save(cert, filename);

  auto readCert = io::load<security::Certificate>(filename);

  BOOST_REQUIRE(readCert != nullptr);
  BOOST_CHECK_EQUAL(cert.getName(), readCert->getName());

  this->writeFile<std::string>("");
  readCert = io::load<security::Certificate>(filename);
  BOOST_REQUIRE(readCert == nullptr);
}

BOOST_AUTO_TEST_SUITE_END() // TestIo
BOOST_AUTO_TEST_SUITE_END() // Util

} // namespace tests
} // namespace ndn

max_stars_repo_name: Pesa/ndn-cxx

max_issues_repo_name: Pesa/ndn-cxx

max_forks_repo_name: Pesa/ndn-cxx


text:
/*
 * Copyright (c) 2014, ATR, Atsushi Watanabe
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*
   * This research was supported by a contract with the Ministry of Internal
   Affairs and Communications entitled, 'Novel and innovative R&D making use
   of brain structures'

   This software was implemented to accomplish the above research.
   Original idea of the implemented control scheme was published on:
   S. Iida, S. Yuta, "Vehicle command system and trajectory control for
   autonomous mobile robots," in Proceedings of the 1991 IEEE/RSJ
   International Workshop on Intelligent Robots and Systems (IROS),
   1991, pp. 212-217.
 */

#include <algorithm>
#include <cmath>
#include <limits>
#include <string>
#include <vector>

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

#include <ros/ros.h>

#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Header.h>

#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

#include <neonavigation_common/compatibility.h>
#include <trajectory_tracker_msgs/PathWithVelocity.h>
#include <trajectory_tracker_msgs/TrajectoryTrackerStatus.h>

#include <trajectory_tracker/TrajectoryTrackerConfig.h>
#include <trajectory_tracker/basic_control.h>
#include <trajectory_tracker/eigen_line.h>
#include <trajectory_tracker/path2d.h>

namespace trajectory_tracker
{
class TrackerNode
{
public:
  TrackerNode();
  ~TrackerNode();
  void spin();

private:
  std::string topic_path_;
  std::string topic_cmd_vel_;
  std::string frame_robot_;
  std::string frame_odom_;
  double hz_;
  double look_forward_;
  double curv_forward_;
  double k_[3];
  double gain_at_vel_;
  double d_lim_;
  double d_stop_;
  double vel_[2];
  double acc_[2];
  double acc_toc_[2];
  trajectory_tracker::VelAccLimitter v_lim_;
  trajectory_tracker::VelAccLimitter w_lim_;
  double rotate_ang_;
  double goal_tolerance_dist_;
  double goal_tolerance_ang_;
  double stop_tolerance_dist_;
  double stop_tolerance_ang_;
  double no_pos_cntl_dist_;
  double min_track_path_;
  int path_step_;
  int path_step_done_;
  bool allow_backward_;
  bool limit_vel_by_avel_;
  bool check_old_path_;
  double epsilon_;
  double max_dt_;
  bool use_time_optimal_control_;
  double time_optimal_control_future_gain_;
  double k_ang_rotation_;
  double k_avel_rotation_;

  ros::Subscriber sub_path_;
  ros::Subscriber sub_path_velocity_;
  ros::Subscriber sub_vel_;
  ros::Subscriber sub_odom_;
  ros::Publisher pub_vel_;
  ros::Publisher pub_status_;
  ros::Publisher pub_tracking_;
  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;
  tf2_ros::Buffer tfbuf_;
  tf2_ros::TransformListener tfl_;
  ros::Timer odom_timeout_timer_;
  double odom_timeout_sec_;

  trajectory_tracker::Path2D path_;
  std_msgs::Header path_header_;
  bool is_path_updated_;

  mutable boost::recursive_mutex parameter_server_mutex_;
  dynamic_reconfigure::Server<TrajectoryTrackerConfig> parameter_server_;

  bool use_odom_;
  bool predict_odom_;
  ros::Time prev_odom_stamp_;

  struct TrackingResult
  {
    explicit TrackingResult(const int s)
      : status(s)
      , distance_remains(0.0)
      , angle_remains(0.0)
      , distance_remains_raw(0.0)
      , angle_remains_raw(0.0)
      , turning_in_place(false)
      , signed_local_distance(0.0)
      , distance_from_target(0.0)
      , target_linear_vel(0.0)
      , tracking_point_x(0.0)
      , tracking_point_y(0.0)
      , tracking_point_curv(0.0)
      , path_step_done(0)
    {
    }

    int status;  // same as trajectory_tracker_msgs::TrajectoryTrackerStatus::status
    double distance_remains;
    double angle_remains;
    double distance_remains_raw;  // remained distance without prediction
    double angle_remains_raw;
    bool turning_in_place;
    double signed_local_distance;
    double distance_from_target;
    double target_linear_vel;
    double tracking_point_x;
    double tracking_point_y;
    double tracking_point_curv;
    int path_step_done;
  };

  template <typename MSG_TYPE>
  void cbPath(const typename MSG_TYPE::ConstPtr&);
  void cbSpeed(const std_msgs::Float32::ConstPtr&);
  void cbOdometry(const nav_msgs::Odometry::ConstPtr&);
  void cbTimer(const ros::TimerEvent&);
  void cbOdomTimeout(const ros::TimerEvent&);
  void control(const tf2::Stamped<tf2::Transform>&, const Eigen::Vector3d&, const double);
  TrackingResult getTrackingResult(
      const tf2::Stamped<tf2::Transform>&, const Eigen::Vector3d&) const;
  void cbParameter(const TrajectoryTrackerConfig& config, const uint32_t /* level */);
};

TrackerNode::TrackerNode()
  : nh_()
  , pnh_("~")
  , tfl_(tfbuf_)
  , is_path_updated_(false)
{
  neonavigation_common::compat::checkCompatMode();
  pnh_.param("frame_robot", frame_robot_, std::string("base_link"));
  pnh_.param("frame_odom", frame_odom_, std::string("odom"));
  neonavigation_common::compat::deprecatedParam(pnh_, "path", topic_path_, std::string("path"));
  neonavigation_common::compat::deprecatedParam(pnh_, "cmd_vel", topic_cmd_vel_, std::string("cmd_vel"));
  pnh_.param("hz", hz_, 50.0);
  pnh_.param("use_odom", use_odom_, false);
  pnh_.param("predict_odom", predict_odom_, true);
  pnh_.param("max_dt", max_dt_, 0.1);
  pnh_.param("odom_timeout_sec", odom_timeout_sec_, 0.1);

  sub_path_ = neonavigation_common::compat::subscribe<nav_msgs::Path>(
      nh_, "path",
      pnh_, topic_path_, 2,
      boost::bind(&TrackerNode::cbPath<nav_msgs::Path>, this, _1));
  sub_path_velocity_ = nh_.subscribe<trajectory_tracker_msgs::PathWithVelocity>(
      "path_velocity", 2,
      boost::bind(&TrackerNode::cbPath<trajectory_tracker_msgs::PathWithVelocity>, this, _1));
  sub_vel_ = neonavigation_common::compat::subscribe(
      nh_, "speed",
      pnh_, "speed", 20, &TrackerNode::cbSpeed, this);
  pub_vel_ = neonavigation_common::compat::advertise<geometry_msgs::Twist>(
      nh_, "cmd_vel",
      pnh_, topic_cmd_vel_, 10);
  pub_status_ = pnh_.advertise<trajectory_tracker_msgs::TrajectoryTrackerStatus>("status", 10, true);
  pub_tracking_ = pnh_.advertise<geometry_msgs::PoseStamped>("tracking", 10, true);
  if (use_odom_)
  {
    sub_odom_ = nh_.subscribe<nav_msgs::Odometry>("odom", 10, &TrackerNode::cbOdometry, this,
                                                  ros::TransportHints().reliable().tcpNoDelay(true));
  }

  boost::recursive_mutex::scoped_lock lock(parameter_server_mutex_);
  parameter_server_.setCallback(boost::bind(&TrackerNode::cbParameter, this, _1, _2));
}

void TrackerNode::cbParameter(const TrajectoryTrackerConfig& config, const uint32_t /* level */)
{
  boost::recursive_mutex::scoped_lock lock(parameter_server_mutex_);
  look_forward_ = config.look_forward;
  curv_forward_ = config.curv_forward;
  k_[0] = config.k_dist;
  k_[1] = config.k_ang;
  k_[2] = config.k_avel;
  gain_at_vel_ = config.gain_at_vel;
  d_lim_ = config.dist_lim;
  d_stop_ = config.dist_stop;
  rotate_ang_ = config.rotate_ang;
  vel_[0] = config.max_vel;
  vel_[1] = config.max_angvel;
  acc_[0] = config.max_acc;
  acc_[1] = config.max_angacc;
  acc_toc_[0] = acc_[0] * config.acc_toc_factor;
  acc_toc_[1] = acc_[1] * config.angacc_toc_factor;
  path_step_ = config.path_step;
  goal_tolerance_dist_ = config.goal_tolerance_dist;
  goal_tolerance_ang_ = config.goal_tolerance_ang;
  stop_tolerance_dist_ = config.stop_tolerance_dist;
  stop_tolerance_ang_ = config.stop_tolerance_ang;
  no_pos_cntl_dist_ = config.no_position_control_dist;
  min_track_path_ = config.min_tracking_path;
  allow_backward_ = config.allow_backward;
  limit_vel_by_avel_ = config.limit_vel_by_avel;
  check_old_path_ = config.check_old_path;
  epsilon_ = config.epsilon;
  use_time_optimal_control_ = config.use_time_optimal_control;
  time_optimal_control_future_gain_ = config.time_optimal_control_future_gain;
  k_ang_rotation_ = config.k_ang_rotation;
  k_avel_rotation_ = config.k_avel_rotation;
}

TrackerNode::~TrackerNode()
{
  geometry_msgs::Twist cmd_vel;
  cmd_vel.linear.x = 0;
  cmd_vel.angular.z = 0;
  pub_vel_.publish(cmd_vel);
}

void TrackerNode::cbSpeed(const std_msgs::Float32::ConstPtr& msg)
{
  vel_[0] = msg->data;
}

namespace
{
float getVelocity(const geometry_msgs::PoseStamped& msg)
{
  return std::numeric_limits<float>::quiet_NaN();
}
float getVelocity(const trajectory_tracker_msgs::PoseStampedWithVelocity& msg)
{
  return msg.linear_velocity.x;
}
}  // namespace

template <typename MSG_TYPE>
void TrackerNode::cbPath(const typename MSG_TYPE::ConstPtr& msg)
{
  path_header_ = msg->header;
  path_.clear();
  is_path_updated_ = true;
  path_step_done_ = 0;
  if (msg->poses.size() == 0)
    return;

  trajectory_tracker::Pose2D in_place_turn_end;
  bool in_place_turning = false;

  auto j = msg->poses.begin();
  path_.push_back(trajectory_tracker::Pose2D(j->pose, getVelocity(*j)));
  for (++j; j < msg->poses.end(); ++j)
  {
    const float velocity = getVelocity(*j);
    if (std::isfinite(velocity) && velocity < -0.0)
    {
      ROS_ERROR_THROTTLE(1.0, "path_velocity.velocity.x must be positive");
      path_.clear();
      return;
    }
    const trajectory_tracker::Pose2D next(j->pose, velocity);

    if ((path_.back().pos_ - next.pos_).squaredNorm() >= std::pow(epsilon_, 2))
    {
      if (in_place_turning)
      {
        path_.push_back(in_place_turn_end);
        in_place_turning = false;
      }
      path_.push_back(next);
    }
    else
    {
      in_place_turn_end = trajectory_tracker::Pose2D(
          path_.back().pos_, next.yaw_, next.velocity_);
      in_place_turning = true;
    }
  }
  if (in_place_turning)
    path_.push_back(in_place_turn_end);
}

void TrackerNode::cbOdometry(const nav_msgs::Odometry::ConstPtr& odom)
{
  if (odom->header.frame_id != frame_odom_)
  {
    ROS_WARN("frame_odom is invalid. Update from \"%s\" to \"%s\"", frame_odom_.c_str(), odom->header.frame_id.c_str());
    frame_odom_ = odom->header.frame_id;
  }
  if (odom->child_frame_id != frame_robot_)
  {
    ROS_WARN("frame_robot is invalid. Update from \"%s\" to \"%s\"",
             frame_robot_.c_str(), odom->child_frame_id.c_str());
    frame_robot_ = odom->child_frame_id;
  }
  if (odom_timeout_sec_ != 0.0)
  {
    if (odom_timeout_timer_.isValid())
    {
      odom_timeout_timer_.setPeriod(ros::Duration(odom_timeout_sec_), true);
    }
    else
    {
      odom_timeout_timer_ =
          nh_.createTimer(ros::Duration(odom_timeout_sec_), &TrackerNode::cbOdomTimeout, this, true, true);
    }
  }

  if (prev_odom_stamp_ != ros::Time())
  {
    const double dt = std::min(max_dt_, (odom->header.stamp - prev_odom_stamp_).toSec());
    nav_msgs::Odometry odom_compensated = *odom;
    Eigen::Vector3d prediction_offset(0, 0, 0);
    if (predict_odom_)
    {
      const double predict_dt = std::max(0.0, std::min(max_dt_, (ros::Time::now() - odom->header.stamp).toSec()));
      tf2::Transform trans;
      const tf2::Quaternion rotation(tf2::Vector3(0, 0, 1), odom->twist.twist.angular.z * predict_dt);
      const tf2::Vector3 translation(odom->twist.twist.linear.x * predict_dt, 0, 0);

      prediction_offset[0] = odom->twist.twist.linear.x * predict_dt;
      prediction_offset[1] = odom->twist.twist.angular.z * predict_dt;

      tf2::fromMsg(odom->pose.pose, trans);
      trans.setOrigin(trans.getOrigin() + tf2::Transform(trans.getRotation()) * translation);
      trans.setRotation(trans.getRotation() * rotation);
      tf2::toMsg(trans, odom_compensated.pose.pose);
    }

    tf2::Transform odom_to_robot;
    tf2::fromMsg(odom_compensated.pose.pose, odom_to_robot);
    const tf2::Stamped<tf2::Transform> robot_to_odom(
        odom_to_robot.inverse(),
        odom->header.stamp, odom->header.frame_id);

    control(robot_to_odom, prediction_offset, dt);
  }
  prev_odom_stamp_ = odom->header.stamp;
}

void TrackerNode::cbTimer(const ros::TimerEvent& event)
{
  try
  {
    tf2::Stamped<tf2::Transform> transform;
    tf2::fromMsg(
        tfbuf_.lookupTransform(frame_robot_, frame_odom_, ros::Time(0)), transform);
    control(transform, Eigen::Vector3d(0, 0, 0), 1.0 / hz_);
  }
  catch (tf2::TransformException& e)
  {
    ROS_WARN("TF exception: %s", e.what());
    trajectory_tracker_msgs::TrajectoryTrackerStatus status;
    status.header.stamp = ros::Time::now();
    status.distance_remains = 0.0;
    status.angle_remains = 0.0;
    status.path_header = path_header_;
    status.status = trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH;
    pub_status_.publish(status);
    return;
  }
}

void TrackerNode::cbOdomTimeout(const ros::TimerEvent& event)
{
  ROS_WARN_STREAM("Odometry timeout. Last odometry stamp: " << prev_odom_stamp_);
  v_lim_.clear();
  w_lim_.clear();
  geometry_msgs::Twist cmd_vel;
  cmd_vel.linear.x = 0.0;
  cmd_vel.angular.z = 0.0;
  pub_vel_.publish(cmd_vel);

  trajectory_tracker_msgs::TrajectoryTrackerStatus status;
  status.header.stamp = ros::Time::now();
  status.distance_remains = 0.0;
  status.angle_remains = 0.0;
  status.path_header = path_header_;
  status.status = trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH;
  pub_status_.publish(status);
}

void TrackerNode::spin()
{
  ros::Timer timer;
  if (!use_odom_)
  {
    timer = nh_.createTimer(ros::Duration(1.0 / hz_), &TrackerNode::cbTimer, this);
  }
  ros::spin();
}

void TrackerNode::control(
    const tf2::Stamped<tf2::Transform>& robot_to_odom,
    const Eigen::Vector3d& prediction_offset,
    const double dt)
{
  trajectory_tracker_msgs::TrajectoryTrackerStatus status;
  status.header.stamp = ros::Time::now();
  status.path_header = path_header_;
  if (is_path_updated_)
  {
    // Call getTrackingResult to update path_step_done_.
    const TrackingResult initial_tracking_result = getTrackingResult(robot_to_odom, prediction_offset);
    path_step_done_ = initial_tracking_result.path_step_done;
    is_path_updated_ = false;
  }
  const TrackingResult tracking_result = getTrackingResult(robot_to_odom, prediction_offset);
  switch (tracking_result.status)
  {
    case trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH:
    case trajectory_tracker_msgs::TrajectoryTrackerStatus::FAR_FROM_PATH:
    {
      v_lim_.clear();
      w_lim_.clear();
      geometry_msgs::Twist cmd_vel;
      cmd_vel.linear.x = 0;
      cmd_vel.angular.z = 0;
      pub_vel_.publish(cmd_vel);
      break;
    }
    default:
    {
      if (tracking_result.turning_in_place)
      {
        v_lim_.set(0.0, tracking_result.target_linear_vel, acc_[0], dt);

        if (use_time_optimal_control_)
        {
          const double expected_angle_remains =
              tracking_result.angle_remains + w_lim_.get() * dt * time_optimal_control_future_gain_;
          w_lim_.set(trajectory_tracker::timeOptimalControl(expected_angle_remains, acc_toc_[1]), vel_[1], acc_[1], dt);
        }
        else
        {
          const double wvel_increment =
              (-tracking_result.angle_remains * k_ang_rotation_ - w_lim_.get() * k_avel_rotation_) * dt;
          w_lim_.increment(wvel_increment, vel_[1], acc_[1], dt);
        }
        ROS_DEBUG(
            "trajectory_tracker: angular residual %0.3f, angular vel %0.3f",
            tracking_result.angle_remains, w_lim_.get());
      }
      else
      {
        v_lim_.set(
            trajectory_tracker::timeOptimalControl(tracking_result.signed_local_distance, acc_toc_[0]),
            tracking_result.target_linear_vel, acc_[0], dt);

        float wref = std::abs(v_lim_.get()) * tracking_result.tracking_point_curv;

        if (limit_vel_by_avel_ && std::abs(wref) > vel_[1])
        {
          v_lim_.set(
              std::copysign(1.0, v_lim_.get()) * std::abs(vel_[1] / tracking_result.tracking_point_curv),
              tracking_result.target_linear_vel, acc_[0], dt);
          wref = std::copysign(1.0, wref) * vel_[1];
        }

        const double k_ang =
            (gain_at_vel_ == 0.0) ? (k_[1]) : (k_[1] * tracking_result.target_linear_vel / gain_at_vel_);
        const double dist_diff = tracking_result.distance_from_target;
        const double angle_diff = tracking_result.angle_remains;
        const double wvel_diff = w_lim_.get() - wref;
        w_lim_.increment(dt * (-dist_diff * k_[0] - angle_diff * k_ang - wvel_diff * k_[2]), vel_[1], acc_[1], dt);

        ROS_DEBUG(
            "trajectory_tracker: distance residual %0.3f, angular residual %0.3f, ang vel residual %0.3f"
            ", v_lim %0.3f, w_lim %0.3f signed_local_distance %0.3f, k_ang %0.3f",
            dist_diff, angle_diff, wvel_diff, v_lim_.get(), w_lim_.get(), tracking_result.signed_local_distance, k_ang);
      }
      if (std::abs(tracking_result.distance_remains) < stop_tolerance_dist_ &&
          std::abs(tracking_result.angle_remains) < stop_tolerance_ang_ &&
          std::abs(tracking_result.distance_remains_raw) < stop_tolerance_dist_ &&
          std::abs(tracking_result.angle_remains_raw) < stop_tolerance_ang_)
      {
        v_lim_.clear();
        w_lim_.clear();
      }
      geometry_msgs::Twist cmd_vel;
      cmd_vel.linear.x = v_lim_.get();
      cmd_vel.angular.z = w_lim_.get();
      pub_vel_.publish(cmd_vel);
      path_step_done_ = tracking_result.path_step_done;
      break;
    }
  }
  status.status = tracking_result.status;
  status.distance_remains = tracking_result.distance_remains;
  status.angle_remains = tracking_result.angle_remains;
  pub_status_.publish(status);
}

TrackerNode::TrackingResult TrackerNode::getTrackingResult(
    const tf2::Stamped<tf2::Transform>& robot_to_odom, const Eigen::Vector3d& prediction_offset) const
{
  if (path_header_.frame_id.size() == 0 || path_.size() == 0)
  {
    return TrackingResult(trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH);
  }
  // Transform
  trajectory_tracker::Path2D lpath;
  double transform_delay = 0;
  tf2::Stamped<tf2::Transform> transform = robot_to_odom;
  try
  {
    tf2::Stamped<tf2::Transform> odom_to_path;
    tf2::fromMsg(
        tfbuf_.lookupTransform(frame_odom_, path_header_.frame_id, ros::Time(0)), odom_to_path);
    transform *= odom_to_path;
    transform_delay = (ros::Time::now() - transform.stamp_).toSec();
    if (std::abs(transform_delay) > 0.1 && check_old_path_)
    {
      ROS_ERROR_THROTTLE(
          1.0, "Timestamp of the transform is too old %f %f",
          ros::Time::now().toSec(), transform.stamp_.toSec());
    }

    const float trans_yaw = tf2::getYaw(transform.getRotation());
    const Eigen::Transform<double, 2, Eigen::TransformTraits::AffineCompact> trans =
        Eigen::Translation2d(
            Eigen::Vector2d(transform.getOrigin().x(), transform.getOrigin().y())) *
        Eigen::Rotation2Dd(trans_yaw);

    for (size_t i = 0; i < path_.size(); i += path_step_)
      lpath.push_back(
          trajectory_tracker::Pose2D(
              trans * path_[i].pos_, trans_yaw + path_[i].yaw_, path_[i].velocity_));
  }
  catch (tf2::TransformException& e)
  {
    ROS_WARN("TF exception: %s", e.what());
    return TrackingResult(trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH);
  }

  const Eigen::Vector2d origin_raw = prediction_offset.head<2>();
  const float yaw_raw = prediction_offset[2];

  const float yaw_predicted = w_lim_.get() * look_forward_ / 2;
  const Eigen::Vector2d origin =
      Eigen::Vector2d(std::cos(yaw_predicted), std::sin(yaw_predicted)) * v_lim_.get() * look_forward_;

  const double path_length = lpath.length();

  // Find nearest line strip
  const trajectory_tracker::Path2D::ConstIterator it_local_goal =
      lpath.findLocalGoal(lpath.cbegin() + path_step_done_, lpath.cend(), allow_backward_);

  const float max_search_range = (path_step_done_ > 0) ? 1.0 : 0.0;
  const trajectory_tracker::Path2D::ConstIterator it_nearest =
      lpath.findNearest(lpath.cbegin() + path_step_done_, it_local_goal, origin,
                        max_search_range, epsilon_);

  if (it_nearest == lpath.end())
  {
    return TrackingResult(trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH);
  }

  const int i_nearest = std::distance(lpath.cbegin(), it_nearest);
  const int i_nearest_prev = std::max(0, i_nearest - 1);
  const int i_local_goal = std::distance(lpath.cbegin(), it_local_goal);

  const Eigen::Vector2d pos_on_line =
      trajectory_tracker::projection2d(lpath[i_nearest_prev].pos_, lpath[i_nearest].pos_, origin);
  const Eigen::Vector2d pos_on_line_raw =
      trajectory_tracker::projection2d(lpath[i_nearest_prev].pos_, lpath[i_nearest].pos_, origin_raw);

  const float linear_vel =
      std::isnan(lpath[i_nearest].velocity_) ? vel_[0] : lpath[i_nearest].velocity_;

  // Remained distance to the local goal
  float remain_local = lpath.remainedDistance(lpath.cbegin(), it_nearest, it_local_goal, pos_on_line);
  // Remained distance to the final goal
  float distance_remains = lpath.remainedDistance(lpath.cbegin(), it_nearest, lpath.cend(), pos_on_line);
  float distance_remains_raw = lpath.remainedDistance(lpath.cbegin(), it_nearest, lpath.cend(), pos_on_line_raw);
  if (path_length < no_pos_cntl_dist_)
    distance_remains = distance_remains_raw = remain_local = 0;

  // Signed distance error
  const float dist_err = trajectory_tracker::lineDistance(
      lpath[i_nearest_prev].pos_, lpath[i_nearest].pos_, origin);

  // Angular error
  const Eigen::Vector2d vec = lpath[i_nearest].pos_ - lpath[i_nearest_prev].pos_;
  float angle_remains = -atan2(vec[1], vec[0]);
  const float angle_pose = allow_backward_ ? lpath[i_nearest].yaw_ : -angle_remains;
  float sign_vel = 1.0;
  if (std::cos(-angle_remains) * std::cos(angle_pose) + std::sin(-angle_remains) * std::sin(angle_pose) < 0)
  {
    sign_vel = -1.0;
    angle_remains = angle_remains + M_PI;
  }
  angle_remains = trajectory_tracker::angleNormalized(angle_remains);

  // Curvature
  const float curv = lpath.getCurvature(it_nearest, it_local_goal, pos_on_line, curv_forward_);

  ROS_DEBUG(
      "trajectory_tracker: nearest: %d, local goal: %d, done: %d, goal: %lu, remain: %0.3f, remain_local: %0.3f",
      i_nearest, i_local_goal, path_step_done_, lpath.size(), distance_remains, remain_local);

  bool arrive_local_goal(false);
  bool in_place_turning = (vec[1] == 0.0 && vec[0] == 0.0);

  TrackingResult result(trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING);

  // Stop and rotate
  const bool large_angle_error = std::abs(rotate_ang_) < M_PI && std::cos(rotate_ang_) > std::cos(angle_remains);
  if (large_angle_error ||
      std::abs(remain_local) < stop_tolerance_dist_ ||
      path_length < min_track_path_ ||
      in_place_turning)
  {
    if (large_angle_error)
    {
      ROS_INFO_THROTTLE(1.0, "Stop and rotate due to large angular error: %0.3f", angle_remains);
    }

    if (path_length < min_track_path_ ||
        std::abs(remain_local) < stop_tolerance_dist_ ||
        in_place_turning)
    {
      angle_remains = trajectory_tracker::angleNormalized(-(it_local_goal - 1)->yaw_);
      if (it_local_goal != lpath.end())
        arrive_local_goal = true;
    }
    if (path_length < stop_tolerance_dist_ || in_place_turning)
      distance_remains = distance_remains_raw = 0.0;

    result.turning_in_place = true;
    result.target_linear_vel = linear_vel;
    result.distance_remains = distance_remains;
    result.distance_remains_raw = distance_remains_raw;
    result.angle_remains = angle_remains;
  }
  else
  {
    // Too far from given path
    float dist_from_path = dist_err;
    if (i_nearest == 0)
      dist_from_path = -(lpath[i_nearest].pos_ - origin).norm();
    else if (i_nearest + 1 >= static_cast<int>(path_.size()))
      dist_from_path = -(lpath[i_nearest].pos_ - origin).norm();
    if (std::abs(dist_from_path) > d_stop_)
    {
      result.distance_remains = distance_remains;
      result.distance_remains_raw = distance_remains_raw;
      result.angle_remains = angle_remains;
      result.angle_remains_raw = angle_remains + yaw_raw;
      result.status = trajectory_tracker_msgs::TrajectoryTrackerStatus::FAR_FROM_PATH;
      return result;
    }

    // Path following control
    result.turning_in_place = false;
    result.target_linear_vel = linear_vel;
    result.distance_remains = distance_remains;
    result.distance_remains_raw = distance_remains_raw;
    result.angle_remains = angle_remains;
    result.angle_remains_raw = angle_remains + yaw_raw;
    result.distance_from_target = trajectory_tracker::clip(dist_err, d_lim_);
    result.signed_local_distance = -remain_local * sign_vel;
    result.tracking_point_curv = curv;
    result.tracking_point_x = pos_on_line[0];
    result.tracking_point_y = pos_on_line[1];
  }

  if (std::abs(result.distance_remains) < goal_tolerance_dist_ &&
      std::abs(result.angle_remains) < goal_tolerance_ang_ &&
      std::abs(result.distance_remains_raw) < goal_tolerance_dist_ &&
      std::abs(result.angle_remains_raw) < goal_tolerance_ang_ &&
      it_local_goal == lpath.end())
  {
    result.status = trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL;
  }

  if (arrive_local_goal)
    result.path_step_done = i_local_goal;
  else
    result.path_step_done = std::max(path_step_done_, i_nearest - 1);

  return result;
}
}  // namespace trajectory_tracker

int main(int argc, char** argv)
{
  ros::init(argc, argv, "trajectory_tracker");
  trajectory_tracker::TrackerNode track;
  track.spin();

  return 0;
}

max_stars_repo_name: Taka-Kazu/neonavigation

max_issues_repo_name: Taka-Kazu/neonavigation

max_forks_repo_name: Taka-Kazu/neonavigation


text:
#include "moja/pocojsonutils.h"

#include <Poco/JSON/Parser.h>

#include <boost/algorithm/string.hpp>

namespace moja {

// --------------------------------------------------------------------------------------------
// "yyyy/mm/dd" -> DateTime

DateTime parseSimpleDate(const std::string& yyyymmdd) {
   std::vector<std::string> tokens;
   boost::split(tokens, yyyymmdd, boost::is_any_of("/"));
   return DateTime(std::stoi(tokens[0]), std::stoi(tokens[1]), std::stoi(tokens[2]));
}

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

DynamicVector parsePocoJSONToDynamic(const Poco::JSON::Array::Ptr& val) {
   DynamicVector arrayDocument;
   const auto count = val->size();
   for (auto i = 0; i < count; i++) {
      if (val->isObject(i)) {
         auto object = val->getObject(i);
         auto subDocument = parsePocoJSONToDynamic(object);
         arrayDocument.push_back(subDocument);
      } else if (val->isArray(i)) {
         auto object = val->getArray(i);
         auto subDocument = parsePocoJSONToDynamic(object);
         arrayDocument.push_back(subDocument);
      } else {
         auto object = val->get(i);
         if (object.isArray()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Array");
         } else if (object.isBoolean()) {
            auto value = object.extract<bool>();
            arrayDocument.push_back(value);
         } else if (object.isDeque()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Deque");
         } else if (object.isEmpty()) {
         } else if (object.isInteger()) {
            try {
               auto value = object.extract<int>();
               arrayDocument.push_back(value);
            } catch (Poco::BadCastException&) {
               auto value = object.extract<Int64>();
               arrayDocument.push_back(value);
            }
         } else if (object.isList()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - List");
         } else if (object.isNumeric()) {
            auto value = object.extract<double>();
            arrayDocument.push_back(value);
         } else if (object.isSigned()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Signed");
         } else if (object.isString()) {
            auto value = object.extract<std::string>();
            arrayDocument.push_back(value);
         } else if (object.isStruct()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Struct");
         } else if (object.isVector()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Vector");
         } else {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Unexpected type");
         }
      }
   }
   return arrayDocument;
}

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

DynamicVar parsePocoJSONToDynamic(const Poco::JSON::Object::Ptr& val) {
   const auto& data = *(val.get());

   // Check for flint dspecial data $date, $time_series etc
   if (data.size() == 1) {
      auto item = *data.begin();
      if (item.first == "$date") {
         const auto& dateStr = item.second.extract<std::string>();
         DynamicVar result = parseSimpleDate(dateStr);
         return result;
      }
   }
   auto document = DynamicObject();
   for (auto& var : data) {
      if (val->isObject(var.first)) {
         const auto& object = var.second.extract<Poco::JSON::Object::Ptr>();
         const auto subDocument = parsePocoJSONToDynamic(object);
         document[var.first] = subDocument;
      } else if (val->isArray(var.first)) {
         const auto object = var.second.extract<Poco::JSON::Array::Ptr>();
         const auto subDocument = parsePocoJSONToDynamic(object);
         document[var.first] = subDocument;
      } else {
         if (var.second.isArray()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Array");
         } else if (var.second.isBoolean()) {
            auto value = var.second.extract<bool>();
            document[var.first] = value;
         } else if (var.second.isDeque()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Deque");
         } else if (var.second.isEmpty()) {
         } else if (var.second.isInteger()) {
            try {
               auto value = var.second.extract<int>();
               document[var.first] = value;
            } catch (Poco::BadCastException&) {
               auto value = var.second.extract<Int64>();
               document[var.first] = value;
            }
         } else if (var.second.isList()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - List");

         } else if (var.second.isNumeric()) {
            auto value = var.second.extract<double>();
            document[var.first] = value;
         } else if (var.second.isSigned()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Signed");
         } else if (var.second.isString()) {
            auto value = var.second.extract<std::string>();
            document[var.first] = value;
         } else if (var.second.isStruct()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Struct");
         } else if (var.second.isVector()) {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Vector");
         } else {
            throw std::runtime_error("Unhandled data type in parse of json into dynamic - Unexpected type");
         }
      }
   }
   return document;
}

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

DynamicVar parsePocoJSONToDynamic(const DynamicVar& data) {
   if (data.isEmpty()) return DynamicVar();
   if (data.type() == typeid(Poco::JSON::Object::Ptr)) {
      const auto& object = data.extract<Poco::JSON::Object::Ptr>();
      return parsePocoJSONToDynamic(object);
   }
   if (data.type() == typeid(Poco::JSON::Array::Ptr)) {
      const auto& object = data.extract<Poco::JSON::Array::Ptr>();
      return parsePocoJSONToDynamic(object);
   }
   if (data.isEmpty()) {
      return DynamicVar();
   }
   if (data.isBoolean()) {
      DynamicVar result = data.extract<bool>();
      return result;
   }
   if (data.isString()) {
      DynamicVar result = data.extract<const std::string>();
      return result;
   }
   if (data.isInteger()) {
      DynamicVar result = data.extract<int>();
      return result;
   }
   if (data.isNumeric()) {
      DynamicVar result = data.extract<double>();
      return result;
   }
   throw std::runtime_error("Unhandled data type in parse of json into dynamic");
}

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

DynamicVar parsePocoVarToDynamic(const DynamicVar& var) {
   if (var.isEmpty()) {
      return DynamicVar();
   }
   if (var.isString()) {
      return DynamicVar(var.extract<std::string>());
   }
   if (var.isBoolean()) {
      return DynamicVar(var.extract<bool>());
   }
   if (var.isInteger()) {
      try {
         return DynamicVar(var.extract<int>());
      } catch (Poco::BadCastException&) {
         return DynamicVar(var.convert<Int64>());
      }
   }
   if (var.isNumeric()) {
      return DynamicVar(var.extract<double>());
   }
   if (var.type() == typeid(Poco::JSON::Array::Ptr)) {
      auto arr = var.extract<Poco::JSON::Array::Ptr>();
      std::vector<DynamicVar> values;
      for (auto it = arr->begin(); it != arr->end(); ++it) {
         values.push_back(parsePocoVarToDynamic(*it));
      }
      return values;
   }
   if (var.isArray()) {
      auto arr = var.extract<Poco::JSON::Array::ValueVec>();
      std::vector<DynamicVar> values;
      for (auto value : arr) {
         values.push_back(parsePocoVarToDynamic(value));
      }
      return values;
   }
   if (var.isStruct()) {
      auto s = var.extract<Poco::DynamicStruct>();
      if (s.size() == 1) {  // We may have a special data type here - "$date" ...
         auto keyValPair = s.begin();
         if (keyValPair->first == "$date") {
            // we have found a date
            // so are expecting a string as the value
            // Expecting { "$date" : "2006/01/01" }
            auto dateStr = keyValPair->second.extract<std::string>();
            DynamicVar result = parseSimpleDate(dateStr);
            return result;
         }
         if (keyValPair->first == "$uint32") {
            // we have found a uint32
            // so are expecting a string as the value
            // Expecting { "$uint32" : 1158849901 }
            int num = keyValPair->second;
            DynamicVar result = UInt32(num);
            return result;
         }
      }
      DynamicObject result;
      for (auto [key, value] : s) {
         result[key] = parsePocoVarToDynamic(value);
      }
      return result;
   }

   auto obj = var.extract<Poco::JSON::Object::Ptr>();
   DynamicObject result;
   for (auto [key, value] : *obj) {
      result[key] = parsePocoVarToDynamic(value);
   }
   return result;

}

}  // namespace moja

max_stars_repo_name: YevenLourance/FLINT

max_issues_repo_name: YevenLourance/FLINT

max_forks_repo_name: YevenLourance/FLINT


text:
#pragma once
#include <glog/logging.h>

#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/iostreams/stream.hpp>
#include <type_traits>

#include "MemDevice.hpp"

namespace zlcook {

/**
 * 通过Emit方法添加Msg元素，Msg会被序列化存储在SINK中
 **/
template <typename MSG, typename SINK = MemSink, typename ENABLE = void>
class OutMsgArchive {
 public:
  using binary_oarchive_t = boost::archive::binary_oarchive;
  using ostream_t = boost::iostreams::stream<SINK>;

  OutMsgArchive(size_t reserved)
      : count_(0),
        postream_(new ostream_t(reserved)),
        poarchive_(new binary_oarchive_t(*postream_)) {}

  OutMsgArchive(const OutMsgArchive& oma) = delete;

  OutMsgArchive(OutMsgArchive&& oma) = delete;

  void Emit(const MSG& msg) {
    (*poarchive_) & msg;
    count_++;
  }

  // flush buffer into SINK
  void Flush() { postream_->flush(); }

  // return count of msg
  size_t Count() const { return count_; }

  // return size of bytes used by msg
  size_t Size() const { return (*postream_)->Size(); }

  // return msg bytes
  SharedBuffer GetBuffer() { return (*postream_)->GetBuffer(); }

 private:
  std::size_t count_;
  std::shared_ptr<ostream_t> postream_;
  std::shared_ptr<binary_oarchive_t> poarchive_;
};

/**
 *  通过Absorb得到MSG实例，该方法会反序列化存储在SOURCE中的数据得到实例
 * */
template <typename MSG, typename SOURCE = MemSource, typename ENABLE = void>
class InMsgArchive {
 public:
  using binary_iarchive_t = boost::archive::binary_iarchive;
  using istream_t = boost::iostreams::stream<SOURCE>;

  InMsgArchive(const SharedBuffer& buffer, size_t count)
      : count_(count),
        pistream_(new istream_t(buffer)),
        piarchive_(new binary_iarchive_t(*pistream_)) {}

  // return MSG install by deserialize
  std::unique_ptr<MSG> Absorb() {
    if (count_ <= 0) {
      return std::unique_ptr<MSG>(nullptr);
    }
    std::unique_ptr<MSG> pmsg(new MSG());
    (*piarchive_) & (*pmsg);
    --count_;
    return pmsg;
  }

  // the count of msg
  size_t Count() { return count_; }

  // the bytes size of all msgs
  size_t Size() { return (*pistream_)->Size(); }

 private:
  size_t count_;
  std::shared_ptr<istream_t> pistream_;
  std::shared_ptr<binary_iarchive_t> piarchive_;
};

// template specialization
// 对于trivial类，不需要oarchive，直接进行内存拷贝即可
template <typename MSG, typename SINK>
class OutMsgArchive<
    MSG, SINK,
    typename std::enable_if<std::is_trivial<MSG>::value &&
                            std::is_standard_layout<MSG>::value>::type> {
 public:
  OutMsgArchive(size_t reserved) : count_(0), psink_(new SINK(reserved)) {}

  OutMsgArchive(const OutMsgArchive& oma) = delete;

  OutMsgArchive(OutMsgArchive&& oma) = delete;

  void Emit(const MSG& msg) {
    psink_->write(&msg, sizeof(msg));
    count_++;
  }

  void Flush() {}

  // return count of msg
  size_t Count() const { return count_; }

  // return size of bytes used by msg
  size_t Size() const { return psink_->Size(); }

  // return msg bytes
  SharedBuffer GetBuffer() { return psink_->GetBuffer(); }

 private:
  size_t count_;
  std::shared_ptr<SINK> psink_;
};

// template specialization
// 对于trivial类，不需要iarchive，直接进行内存拷贝即可
template <typename MSG, typename SOURCE>
class InMsgArchive<
    MSG, SOURCE,
    typename std::enable_if<std::is_trivial<MSG>::value &&
                            std::is_standard_layout<MSG>::value>::type> {
 public:
  InMsgArchive(const SharedBuffer& buffer, size_t count)
      : count_(count), psource_(new SOURCE(buffer)) {}

  std::unique_ptr<MSG> Absorb() {
    if (count_ <= 0) {
      return std::unique_ptr<MSG>(nullptr);
    }
    std::unique_ptr<MSG> pmsg(new MSG());
    CHECK_EQ(sizeof(MSG), psource_->read(pmsg.get(), sizeof(MSG)));
    --count_;
    return pmsg;
  }

  // the count of msg
  size_t Count() { return count_; }

  // the bytes size of all msgs
  size_t Size() { return psource_->Size(); }

 private:
  size_t count_;
  std::shared_ptr<SOURCE> psource_;
};
}  // namespace zlcook

max_stars_repo_name: zlcook/serialize

max_issues_repo_name: zlcook/serialize

max_forks_repo_name: zlcook/serialize


text:
#include "Update.h"

#include <algorithm>
#include <boost/algorithm/string/join.hpp>
#include <storm/exceptions/IllegalArgumentException.h>

#include "storm/utility/macros.h"
#include "storm/exceptions/OutOfRangeException.h"

namespace storm {
    namespace prism {
        Update::Update(uint_fast64_t globalIndex, storm::expressions::Expression const& likelihoodExpression, std::vector<storm::prism::Assignment> const& assignments, std::string const& filename, uint_fast64_t lineNumber) : LocatedInformation(filename, lineNumber), likelihoodExpression(likelihoodExpression), assignments(assignments), variableToAssignmentIndexMap(), globalIndex(globalIndex) {
            STORM_LOG_ASSERT(likelihoodExpression.containsVariables() || likelihoodExpression.evaluateAsRational() >= 0, "Negative likelihood expressions are not allowed.");
            std::sort(this->assignments.begin(), this->assignments.end(), [] (storm::prism::Assignment const& assignment1, storm::prism::Assignment const& assignment2) {
                bool smaller = assignment1.getVariable().getType().isBooleanType() && !assignment2.getVariable().getType().isBooleanType();
                if (!smaller) {
                    smaller = assignment1.getVariable() < assignment2.getVariable();
                }
                return smaller;
            });
            this->createAssignmentMapping();
        }

        storm::expressions::Expression const& Update::getLikelihoodExpression() const {
            return likelihoodExpression;
        }

        std::size_t Update::getNumberOfAssignments() const {
            return this->assignments.size();
        }

        std::vector<storm::prism::Assignment> const& Update::getAssignments() const {
            return this->assignments;
        }

        std::vector<storm::prism::Assignment>& Update::getAssignments() {
            return this->assignments;
        }

        storm::prism::Assignment const& Update::getAssignment(std::string const& variableName) const {
            auto const& variableIndexPair = this->variableToAssignmentIndexMap.find(variableName);
            STORM_LOG_THROW(variableIndexPair != this->variableToAssignmentIndexMap.end(), storm::exceptions::OutOfRangeException, "Variable '" << variableName << "' is not assigned in update.");
            return this->getAssignments()[variableIndexPair->second];
        }

        std::map<storm::expressions::Variable, storm::expressions::Expression> Update::getAsVariableToExpressionMap() const {
            std::map<storm::expressions::Variable, storm::expressions::Expression> result;

            for (auto const& assignment : this->getAssignments()) {
                result.emplace(assignment.getVariable(), assignment.getExpression());
            }

            return result;
        }

        uint_fast64_t Update::getGlobalIndex() const {
            return this->globalIndex;
        }

        void Update::createAssignmentMapping() {
            this->variableToAssignmentIndexMap.clear();
            for (uint_fast64_t assignmentIndex = 0; assignmentIndex < this->getAssignments().size(); ++assignmentIndex) {
                this->variableToAssignmentIndexMap[this->getAssignments()[assignmentIndex].getVariableName()] = assignmentIndex;
            }
        }

        Update Update::substitute(std::map<storm::expressions::Variable, storm::expressions::Expression> const& substitution) const {
            std::vector<Assignment> newAssignments;
            newAssignments.reserve(this->getNumberOfAssignments());
            for (auto const& assignment : this->getAssignments()) {
                newAssignments.emplace_back(assignment.substitute(substitution));
            }
            auto newLikelihoodExpression = this->getLikelihoodExpression().substitute(substitution);
            STORM_LOG_THROW(likelihoodExpression.containsVariables() || likelihoodExpression.evaluateAsRational() >= 0, storm::exceptions::IllegalArgumentException, "Substitution yielding negative probabilities in '" << this->getLikelihoodExpression() << "' are not allowed.");
            // FIXME: The expression could be simplified, but 1/K (where K is an int) is then resolved to 0, which is incorrect (for probabilities).
            return Update(this->getGlobalIndex(), newLikelihoodExpression, newAssignments, this->getFilename(), this->getLineNumber());
        }

        Update Update::removeIdentityAssignments() const {
            std::vector<Assignment> newAssignments;
            newAssignments.reserve(getNumberOfAssignments());
            for(auto const& ass : this->assignments) {
                if(!ass.isIdentity()) {
                    newAssignments.push_back(ass);
                }
            }
            return Update(this->globalIndex, this->likelihoodExpression, newAssignments, getFilename(), getLineNumber());
        }

        Update Update::simplify() const {
            std::vector<Assignment> newAssignments;
            newAssignments.reserve(getNumberOfAssignments());
            for (auto const& ass : this->assignments) {
                Assignment newAssignment(ass.getVariable(), ass.getExpression().simplify().reduceNesting(), ass.getFilename(), ass.getLineNumber());
                if (!newAssignment.isIdentity()) {
                    newAssignments.push_back(std::move(newAssignment));
                }
            }
            return Update(this->globalIndex, this->likelihoodExpression.simplify().reduceNesting(), newAssignments, getFilename(), getLineNumber());
        }

        std::ostream& operator<<(std::ostream& stream, Update const& update) {
            stream << update.getLikelihoodExpression() << " : ";
            if (update.getAssignments().empty()) {
                stream << "true";
            } else {
                std::vector<std::string> assignmentsAsStrings;
                std::for_each(update.getAssignments().cbegin(), update.getAssignments().cend(), [&assignmentsAsStrings] (Assignment const& assignment) { std::stringstream stream; stream << assignment; assignmentsAsStrings.push_back(stream.str()); });
                stream << boost::algorithm::join(assignmentsAsStrings, " & ");
            }
            return stream;
        }

    } // namespace prism
} // namespace storm