text:
/**
 * \file      ekf-flexibility-estimator-base.hpp
 * \author    Mehdi Benallegue
 * \date      2013
 * \brief     Declare the class of the flexibility estimation using the extended
 *            Kalman Filter.
 *
 * \details
 *
 *
 */

#ifndef FLEXIBILITYESTIMATION_EKFFLEXIBILITYESTIMATORBASE_H
#define FLEXIBILITYESTIMATION_EKFFLEXIBILITYESTIMATORBASE_H

#include <boost/utility.hpp>

#include <state-observation/api.h>
#include <state-observation/observer/extended-kalman-filter.hpp>

#include <state-observation/flexibility-estimation/flexibility-estimator-base.hpp>

namespace stateObservation
{
namespace flexibilityEstimation
{
/**
 * \class  EKFFlexibilityEstimatorBase
 * \brief  This class is the base class of the flexibility estimators that
 *         use an extended Kalman Filter. Several methods require to be overloaded
 *         to derive an implementation from this base class.
 *
 */

class STATE_OBSERVATION_DLLAPI EKFFlexibilityEstimatorBase : public FlexibilityEstimatorBase
{
public:
  /// The constructor.
  ///  \li stateSize : size of the state vector
  ///  \li measurementSize : size of the measurements vector
  ///  \li inputSize : size of the input vector
  ///  \li dx gives the derivation step for a finite differences derivation method

  EKFFlexibilityEstimatorBase(Index stateSize,
                              Index measurementSize,
                              Index inputSize,
                              const Vector & dx = Vector::Zero(0));

  /// virtual destructor
  virtual ~EKFFlexibilityEstimatorBase();

  /// Sets a value of the flexibility x_k provided from another source
  /// can be used for initialization of the estimator
  /// This is a pure virtual function that requires to be overloaded in
  /// implementation
  virtual void setFlexibilityGuess(const Matrix &) = 0;

  /// Sets the covariance matrix of the flexibility Guess
  virtual void setFlexibilityCovariance(const Matrix & P);

  /// Gets the covariance matrix of the flexibility
  virtual Matrix getFlexibilityCovariance() const;

  /// Sets the covariance matrices for the process noises
  /// \li Q process noise
  virtual void setProcessNoiseCovariance(const Matrix & Q);

  /// Sets the covariance matrices for the sensor noises
  /// \li R sensor noise
  virtual void setMeasurementNoiseCovariance(const Matrix & R);

  /// gets the covariance matrices for the process noises
  virtual Matrix getProcessNoiseCovariance() const;

  /// gets the covariance matrices for the sensor noises
  virtual Matrix getMeasurementNoiseCovariance() const;

  /// Sets the value of the next sensor measurement y_{k+1}
  virtual void setMeasurement(const Vector & y);

  virtual Vector getMeasurement();

  /// Sets the value of the next input for the state process dynamics
  /// i.e. : gives u_k such that x_{k+1} = f(x_k,u_k)
  virtual void setInput(const Vector & u);

  /// Sets the value of the next  measurement
  /// i.e. : gives u_{k+1} such that y_{k+1}=h(x_{k+1},u_{k+1})
  virtual void setMeasurementInput(const Vector & u);

  virtual Vector getInput();

  virtual Vector getMeasurementInput();

  /// Gets an estimation of the flexibility in the form of a state vector \hat{x_{k+1}}
  virtual const Vector & getFlexibilityVector();

  /// Gets an estimation of the flexibility in the form of a homogeneous matrix
  virtual Matrix4 getFlexibility() = 0;

  /// Gets a const reference on the extended Kalman filter
  virtual const stateObservation::ExtendedKalmanFilter & getEKF() const;

  /// Gets a reference on the extended Kalman filter
  virtual stateObservation::ExtendedKalmanFilter & getEKF();

  /// Gets the state size
  /// this method is pure virtual and reauires to be overloaded in implementation
  virtual Index getStateSize() const = 0;

  /// Gets the measurements size
  /// this method is pure virtual and reauires to be overloaded in implementation
  virtual Index getMeasurementSize() const = 0;

  /// Gets the input size
  /// this method is pure virtual and reauires to be overloaded in implementation
  virtual Index getInputSize() const = 0;

  /// Gets a simulation of the
  virtual Vector getSimulatedMeasurement();

  /// Resets the covariance matrices to their original values
  virtual void resetCovarianceMatrices() = 0;

  /// Get the last vector of inovation of the Kalman filter
  virtual Vector getInnovation();

  /// Get the simulated measurement of the predicted state
  virtual Vector getPredictedMeasurement();

  /// Get the predicted state
  virtual Vector getPrediction();

  /// Get the last simulated measurement
  virtual Vector getLastPredictedMeasurement();

  /// Get the last predicted state
  virtual Vector getLastPrediction();

protected:
  virtual void setJacobians(const Matrix & A, const Matrix & C);

  virtual void useFiniteDifferencesJacobians(Vector dx);

  stateObservation::ExtendedKalmanFilter ekf_;

  bool finiteDifferencesJacobians_;

  Vector dx_;

  Vector lastX_;

  TimeIndex k_;

private:
};
} // namespace flexibilityEstimation
} // namespace stateObservation
#endif // FLEXIBILITYESTIMATION_EKFFLEXIBILITYESTIMATORBASE_H

max_stars_repo_name: mmurooka/state-observation

max_issues_repo_name: mmurooka/state-observation

max_forks_repo_name: mmurooka/state-observation


text:
//
// Copyright (c) 2013-2016 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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)
//

#ifndef BEAST_DETAIL_UNIT_TEST_REPORTER_HPP
#define BEAST_DETAIL_UNIT_TEST_REPORTER_HPP

#include <beast/detail/unit_test/amount.hpp>
#include <beast/detail/unit_test/recorder.hpp>
#include <beast/detail/stream/abstract_ostream.hpp>
#include <beast/detail/stream/basic_std_ostream.hpp>
#include <boost/optional.hpp>
#include <algorithm>
#include <chrono>
#include <functional>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include <utility>

namespace beast {
namespace detail {

inline
namespace unit_test {

namespace detail {

/** A simple test runner that writes everything to a stream in real time.
    The totals are output when the object is destroyed.
*/
template <class = void>
class reporter : public runner
{
private:
    using clock_type = std::chrono::steady_clock;

    struct case_results
    {
        std::string name;
        std::size_t total = 0;
        std::size_t failed = 0;

        case_results (std::string const& name_ = "");
    };

    struct suite_results
    {
        std::string name;
        std::size_t cases = 0;
        std::size_t total = 0;
        std::size_t failed = 0;
        typename clock_type::time_point start =
            clock_type::now();

        explicit
        suite_results (std::string const& name_ = "");

        void
        add (case_results const& r);
    };

    struct results
    {
        using run_time = std::pair<std::string,
            typename clock_type::duration>;

        enum
        {
            max_top = 10
        };

        std::size_t suites = 0;
        std::size_t cases = 0;
        std::size_t total = 0;
        std::size_t failed = 0;
        std::vector<run_time> top;
        typename clock_type::time_point start =
            clock_type::now();

        void
        add (suite_results const& r);
    };

    boost::optional <std_ostream> std_ostream_;
    std::reference_wrapper <beast::detail::abstract_ostream> stream_;
    results results_;
    suite_results suite_results_;
    case_results case_results_;

public:
    reporter (reporter const&) = delete;
    reporter& operator= (reporter const&) = delete;

    ~reporter();

    explicit
    reporter (std::ostream& stream = std::cout);

    explicit
    reporter (beast::detail::abstract_ostream& stream);

private:
    static
    std::string
    fmtdur (typename clock_type::duration const& d);

    virtual
    void
    on_suite_begin (suite_info const& info) override;

    virtual
    void
    on_suite_end() override;

    virtual
    void
    on_case_begin (std::string const& name) override;

    virtual
    void
    on_case_end() override;

    virtual
    void
    on_pass() override;

    virtual
    void
    on_fail (std::string const& reason) override;

    virtual
    void
    on_log (std::string const& s) override;
};

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

template <class _>
reporter<_>::case_results::case_results (
        std::string const& name_)
    : name (name_)
{
}

template <class _>
reporter<_>::suite_results::suite_results (
        std::string const& name_)
    : name (name_)
{
}

template <class _>
void
reporter<_>::suite_results::add (case_results const& r)
{
    ++cases;
    total += r.total;
    failed += r.failed;
}

template <class _>
void
reporter<_>::results::add (
    suite_results const& r)
{
    ++suites;
    total += r.total;
    cases += r.cases;
    failed += r.failed;

    auto const elapsed =
        clock_type::now() - r.start;
    if (elapsed >= std::chrono::seconds(1))
    {
        auto const iter = std::lower_bound(top.begin(),
            top.end(), elapsed,
            [](run_time const& t1,
                typename clock_type::duration const& t2)
            {
                return t1.second > t2;
            });
        if (iter != top.end())
        {
            if (top.size() == max_top)
                top.resize(top.size() - 1);
            top.emplace(iter, r.name, elapsed);
        }
        else if (top.size() < max_top)
        {
            top.emplace_back(r.name, elapsed);
        }
    }
}

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

template <class _>
reporter<_>::reporter (
        std::ostream& stream)
    : std_ostream_ (std::ref (stream))
    , stream_ (*std_ostream_)
{
}

template <class _>
reporter<_>::~reporter()
{
    if (results_.top.size() > 0)
    {
        stream_.get() << "Longest suite times:";
        for (auto const& i : results_.top)
            stream_.get() << std::setw(8) <<
                fmtdur(i.second) << " " << i.first;
    }
    auto const elapsed =
        clock_type::now() - results_.start;
    stream_.get() <<
        fmtdur(elapsed) << ", " <<
        amount (results_.suites, "suite") << ", " <<
        amount (results_.cases, "case") << ", " <<
        amount (results_.total, "test") << " total, " <<
        amount (results_.failed, "failure");
}

template <class _>
reporter<_>::reporter (
        beast::detail::abstract_ostream& stream)
    : stream_ (stream)
{
}

template <class _>
std::string
reporter<_>::fmtdur (
    typename clock_type::duration const& d)
{
    using namespace std::chrono;
    auto const ms =
        duration_cast<milliseconds>(d);
    if (ms < seconds(1))
        return std::to_string(ms.count()) + "ms";
    std::stringstream ss;
    ss << std::fixed << std::setprecision(1) <<
        (ms.count()/1000.) << "s";
    return ss.str();
}

template <class _>
void
reporter<_>::on_suite_begin (
    suite_info const& info)
{
    suite_results_ = suite_results (info.full_name());
}

template <class _>
void
reporter<_>::on_suite_end()
{
    results_.add (suite_results_);
}

template <class _>
void
reporter<_>::on_case_begin (
    std::string const& name)
{
    case_results_ = case_results (name);

    stream_.get() <<
        suite_results_.name <<
        (case_results_.name.empty() ?
            "" : (" " + case_results_.name));
}

template <class _>
void
reporter<_>::on_case_end()
{
    suite_results_.add (case_results_);
}

template <class _>
void
reporter<_>::on_pass()
{
    ++case_results_.total;
}

template <class _>
void
reporter<_>::on_fail (
    std::string const& reason)
{
    ++case_results_.failed;
    ++case_results_.total;
    stream_.get() <<
        "#" << case_results_.total <<
        " failed" <<
        (reason.empty() ? "" : ": ") << reason;
}

template <class _>
void
reporter<_>::on_log (
    std::string const& s)
{
    stream_.get() << s;
}

} // detail

using reporter = detail::reporter<>;

} // unit_test
} // detail
} // beast

#endif

ext: hpp


text:
// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:t -*-
// vim: ts=8 sw=2 smarttab
/*
 * Ceph - scalable distributed file system
 *
 * Copyright (C) 2004-2006 Sage Weil <sage@newdream.net>
 * Portions Copyright (C) 2013 CohortFS, LLC
 *
 * This is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License version 2.1, as published by the Free Software
 * Foundation.  See file COPYING.
 *
 */

#include <arpa/inet.h>
#include <boost/lexical_cast.hpp>
#include <set>
#include <stdlib.h>
#include <memory>

#include "XioMsg.h"
#include "XioMessenger.h"
#include "common/address_helper.h"
#include "common/code_environment.h"
// #include "messages/MNop.h"

#define dout_subsys ceph_subsys_xio
#undef dout_prefix
#define dout_prefix *_dout << "xio."

Mutex mtx("XioMessenger Package Lock");
std::atomic<bool> initialized = { false };

std::atomic<unsigned> XioMessenger::nInstances = { 0 };

struct xio_mempool *xio_msgr_noreg_mpool;

static struct xio_session_ops xio_msgr_ops;

/* Accelio API callouts */

namespace xio_log
{
typedef pair<const char*, int> level_pair;
static const level_pair LEVELS[] = {
  make_pair("fatal", 0),
  make_pair("error", 0),
  make_pair("warn", 1),
  make_pair("info", 1),
  make_pair("debug", 2),
  make_pair("trace", 20)
};

static CephContext *context;

int get_level()
{
  int level = 0;
  for (size_t i = 0; i < sizeof(LEVELS); i++) {
    if (!ldlog_p1(context, dout_subsys, LEVELS[i].second))
      break;
    level++;
  }
  return level;
}

void log_dout(const char *file, unsigned line,
	      const char *function, unsigned level,
	      const char *fmt, ...)
{
  char buffer[2048];
  va_list args;
  va_start(args, fmt);
  int n = vsnprintf(buffer, sizeof(buffer), fmt, args);
  va_end(args);

  if (n > 0) {
    const char *short_file = strrchr(file, '/');
    short_file = (short_file == NULL) ? file : short_file + 1;

    const level_pair &lvl = LEVELS[level];
    ldout(context, lvl.second) << '[' << lvl.first << "] "
      << short_file << ':' << line << ' '
      << function << " - " << buffer << dendl;
  }
}
}

static int on_session_event(struct xio_session *session,
			    struct xio_session_event_data *event_data,
			    void *cb_user_context)
{
  XioMessenger *msgr = static_cast<XioMessenger*>(cb_user_context);
  CephContext *cct = msgr->cct;

  ldout(cct,4) << "session event: " << xio_session_event_str(event_data->event)
    << ". reason: " << xio_strerror(event_data->reason) << dendl;

  return msgr->session_event(session, event_data, cb_user_context);
}

static int on_new_session(struct xio_session *session,
			  struct xio_new_session_req *req,
			  void *cb_user_context)
{
  XioMessenger *msgr = static_cast<XioMessenger*>(cb_user_context);
  CephContext *cct = msgr->cct;

  ldout(cct,4) << "new session " << session
    << " user_context " << cb_user_context << dendl;

  return (msgr->new_session(session, req, cb_user_context));
}

static int on_msg(struct xio_session *session,
		  struct xio_msg *req,
		  int more_in_batch,
		  void *cb_user_context)
{
  XioConnection* xcon __attribute__((unused)) =
    static_cast<XioConnection*>(cb_user_context);
  CephContext *cct = xcon->get_messenger()->cct;

  ldout(cct,25) << "on_msg session " << session << " xcon " << xcon << dendl;

  if (unlikely(XioPool::trace_mempool)) {
    static uint32_t nreqs;
    if (unlikely((++nreqs % 65536) == 0)) {
      xp_stats.dump(__func__, nreqs);
    }
  }

  return xcon->on_msg(session, req, more_in_batch,
			  cb_user_context);
}

static int on_ow_msg_send_complete(struct xio_session *session,
				   struct xio_msg *msg,
				   void *conn_user_context)
{
  XioConnection *xcon =
    static_cast<XioConnection*>(conn_user_context);
  CephContext *cct = xcon->get_messenger()->cct;

  ldout(cct,25) << "msg delivered session: " << session
		<< " msg: " << msg << " conn_user_context "
		<< conn_user_context << dendl;

  return xcon->on_ow_msg_send_complete(session, msg, conn_user_context);
}

static int on_msg_error(struct xio_session *session,
			enum xio_status error,
			enum xio_msg_direction dir,
			struct xio_msg  *msg,
			void *conn_user_context)
{
  /* XIO promises to flush back undelivered messages */
  XioConnection *xcon =
    static_cast<XioConnection*>(conn_user_context);
  CephContext *cct = xcon->get_messenger()->cct;

  ldout(cct,4) << "msg error session: " << session
    << " error: " << xio_strerror(error) << " msg: " << msg
    << " conn_user_context " << conn_user_context << dendl;

  return xcon->on_msg_error(session, error, msg, conn_user_context);
}

static int on_cancel(struct xio_session *session,
		     struct xio_msg  *msg,
		     enum xio_status result,
		     void *conn_user_context)
{
  XioConnection* xcon __attribute__((unused)) =
    static_cast<XioConnection*>(conn_user_context);
  CephContext *cct = xcon->get_messenger()->cct;

  ldout(cct,25) << "on cancel: session: " << session << " msg: " << msg
    << " conn_user_context " << conn_user_context << dendl;

  return 0;
}

static int on_cancel_request(struct xio_session *session,
			     struct xio_msg  *msg,
			     void *conn_user_context)
{
  XioConnection* xcon __attribute__((unused)) =
    static_cast<XioConnection*>(conn_user_context);
  CephContext *cct = xcon->get_messenger()->cct;

  ldout(cct,25) << "on cancel request: session: " << session << " msg: " << msg
    << " conn_user_context " << conn_user_context << dendl;

  return 0;
}

/* free functions */
static string xio_uri_from_entity(const string &type,
				  const entity_addr_t& addr, bool want_port)
{
  const char *host = NULL;
  char addr_buf[129];
  string xio_uri;

  switch(addr.get_family()) {
  case AF_INET:
    host = inet_ntop(AF_INET, &addr.in4_addr().sin_addr, addr_buf,
		     INET_ADDRSTRLEN);
    break;
  case AF_INET6:
    host = inet_ntop(AF_INET6, &addr.in6_addr().sin6_addr, addr_buf,
		     INET6_ADDRSTRLEN);
    break;
  default:
    abort();
    break;
  };

  if (type == "rdma" || type == "tcp")
      xio_uri = type + "://";
  else
      xio_uri = "rdma://";

  /* The following can only succeed if the host is rdma-capable */
  xio_uri += host;
  if (want_port) {
    xio_uri += ":";
    xio_uri += boost::lexical_cast<std::string>(addr.get_port());
  }

  return xio_uri;
} /* xio_uri_from_entity */

void XioInit::package_init(CephContext *cct) {
   if (! initialized) {

     mtx.Lock();
     if (! initialized) {

       xio_init();

       // claim a reference to the first context we see
       xio_log::context = cct->get();

       int xopt;
       xopt = xio_log::get_level();
       xio_set_opt(NULL, XIO_OPTLEVEL_ACCELIO, XIO_OPTNAME_LOG_LEVEL,
 		  &xopt, sizeof(xopt));
       xio_set_opt(NULL, XIO_OPTLEVEL_ACCELIO, XIO_OPTNAME_LOG_FN,
 		  (const void*)xio_log::log_dout, sizeof(xio_log_fn));

       xopt = 1;
       xio_set_opt(NULL, XIO_OPTLEVEL_ACCELIO, XIO_OPTNAME_DISABLE_HUGETBL,
 		  &xopt, sizeof(xopt));

       if (g_code_env == CODE_ENVIRONMENT_DAEMON) {
         xopt = 1;
         xio_set_opt(NULL, XIO_OPTLEVEL_RDMA, XIO_OPTNAME_ENABLE_FORK_INIT,
 		    &xopt, sizeof(xopt));
       }

       xopt = XIO_MSGR_IOVLEN;
       xio_set_opt(NULL, XIO_OPTLEVEL_ACCELIO, XIO_OPTNAME_MAX_IN_IOVLEN,
 		  &xopt, sizeof(xopt));
       xio_set_opt(NULL, XIO_OPTLEVEL_ACCELIO, XIO_OPTNAME_MAX_OUT_IOVLEN,
 		  &xopt, sizeof(xopt));

       /* enable flow-control */
       xopt = 1;
       xio_set_opt(NULL, XIO_OPTLEVEL_ACCELIO, XIO_OPTNAME_ENABLE_FLOW_CONTROL,
                  &xopt, sizeof(xopt));

       /* and set threshold for buffer callouts */
       xopt = max(cct->_conf->xio_max_send_inline, 512);
       xio_set_opt(NULL, XIO_OPTLEVEL_ACCELIO, XIO_OPTNAME_MAX_INLINE_XIO_DATA,
                  &xopt, sizeof(xopt));

       xopt = XioMsgHdr::get_max_encoded_length();
       ldout(cct,2) << "setting accelio max header size " << xopt << dendl;
       xio_set_opt(NULL, XIO_OPTLEVEL_ACCELIO, XIO_OPTNAME_MAX_INLINE_XIO_HEADER,
                  &xopt, sizeof(xopt));

       size_t queue_depth = cct->_conf->xio_queue_depth;
       struct xio_mempool_config mempool_config = {
         6,
         {
           {1024,  0,  queue_depth,  262144},
           {4096,  0,  queue_depth,  262144},
           {16384, 0,  queue_depth,  262144},
           {65536, 0,  128,  65536},
           {262144, 0,  32,  16384},
           {1048576, 0, 8,  8192}
         }
       };
       xio_set_opt(NULL,
                   XIO_OPTLEVEL_ACCELIO, XIO_OPTNAME_CONFIG_MEMPOOL,
                   &mempool_config, sizeof(mempool_config));

       /* and unregisterd one */
 #define XMSG_MEMPOOL_QUANTUM 4096

       xio_msgr_noreg_mpool =
 	xio_mempool_create(-1 /* nodeid */,
 			   XIO_MEMPOOL_FLAG_REGULAR_PAGES_ALLOC);

       (void) xio_mempool_add_slab(xio_msgr_noreg_mpool, 64,
 				       cct->_conf->xio_mp_min,
 				       cct->_conf->xio_mp_max_64,
 				       XMSG_MEMPOOL_QUANTUM, 0);
       (void) xio_mempool_add_slab(xio_msgr_noreg_mpool, 256,
 				       cct->_conf->xio_mp_min,
 				       cct->_conf->xio_mp_max_256,
 				       XMSG_MEMPOOL_QUANTUM, 0);
       (void) xio_mempool_add_slab(xio_msgr_noreg_mpool, 1024,
 				       cct->_conf->xio_mp_min,
 				       cct->_conf->xio_mp_max_1k,
 				       XMSG_MEMPOOL_QUANTUM, 0);
       (void) xio_mempool_add_slab(xio_msgr_noreg_mpool, getpagesize(),
 				       cct->_conf->xio_mp_min,
 				       cct->_conf->xio_mp_max_page,
 				       XMSG_MEMPOOL_QUANTUM, 0);

       /* initialize ops singleton */
       xio_msgr_ops.on_session_event = on_session_event;
       xio_msgr_ops.on_new_session = on_new_session;
       xio_msgr_ops.on_session_established = NULL;
       xio_msgr_ops.on_msg = on_msg;
       xio_msgr_ops.on_ow_msg_send_complete = on_ow_msg_send_complete;
       xio_msgr_ops.on_msg_error = on_msg_error;
       xio_msgr_ops.on_cancel = on_cancel;
       xio_msgr_ops.on_cancel_request = on_cancel_request;

       /* mark initialized */
       initialized = true;
     }
     mtx.Unlock();
   }
 }

/* XioMessenger */
#undef dout_prefix
#define dout_prefix _prefix(_dout, this)
static ostream& _prefix(std::ostream *_dout, XioMessenger *msgr) {
  return *_dout << "-- " << msgr->get_myaddr_legacy() << " ";
}

XioMessenger::XioMessenger(CephContext *cct, entity_name_t name,
			   string mname, uint64_t _nonce,
			   uint64_t cflags, DispatchStrategy *ds)
  : SimplePolicyMessenger(cct, name, mname, _nonce),
    XioInit(cct),
    portals(this, get_nportals(cflags), get_nconns_per_portal(cflags)),
    dispatch_strategy(ds),
    loop_con(new XioLoopbackConnection(this)),
    special_handling(0),
    sh_mtx("XioMessenger session mutex"),
    sh_cond(),
    need_addr(true),
    did_bind(false),
    nonce(_nonce)
{

  if (cct->_conf->xio_trace_xcon)
    magic |= MSG_MAGIC_TRACE_XCON;

  XioPool::trace_mempool = (cct->_conf->xio_trace_mempool);
  XioPool::trace_msgcnt = (cct->_conf->xio_trace_msgcnt);

  dispatch_strategy->set_messenger(this);

  /* update class instance count */
  nInstances++;

  loop_con->set_features(CEPH_FEATURES_ALL);

  ldout(cct,2) << "Create msgr: " << this << " instance: "
    << nInstances << " type: " << name.type_str()
    << " subtype: " << mname << " nportals: " << get_nportals(cflags)
    << " nconns_per_portal: " << get_nconns_per_portal(cflags)
    << dendl;

} /* ctor */

int XioMessenger::pool_hint(uint32_t dsize) {
  if (dsize > 1024*1024)
    return 0;

  /* if dsize is already present, returns -EEXIST */
  return xio_mempool_add_slab(xio_msgr_noreg_mpool, dsize, 0,
				   cct->_conf->xio_mp_max_hint,
				   XMSG_MEMPOOL_QUANTUM, 0);
}

int XioMessenger::get_nconns_per_portal(uint64_t cflags)
{
  const int XIO_DEFAULT_NUM_CONNS_PER_PORTAL = 8;
  int nconns = XIO_DEFAULT_NUM_CONNS_PER_PORTAL;

  if (cflags & Messenger::HAS_MANY_CONNECTIONS)
    nconns = max(cct->_conf->xio_max_conns_per_portal, XIO_DEFAULT_NUM_CONNS_PER_PORTAL);
  else if (cflags & Messenger::HEARTBEAT)
    nconns = max(cct->_conf->osd_heartbeat_min_peers * 4, XIO_DEFAULT_NUM_CONNS_PER_PORTAL);

  return nconns;
}

int XioMessenger::get_nportals(uint64_t cflags)
{
  int nportals = 1;

  if (cflags & Messenger::HAS_HEAVY_TRAFFIC)
    nportals = max(cct->_conf->xio_portal_threads, 1);

  return nportals;
}

void XioMessenger::learned_addr(const entity_addr_t &peer_addr_for_me)
{
  // be careful here: multiple threads may block here, and readers of
  // my_inst.addr do NOT hold any lock.

  // this always goes from true -> false under the protection of the
  // mutex.  if it is already false, we need not retake the mutex at
  // all.
  if (!need_addr)
    return;

  sh_mtx.Lock();
  if (need_addr) {
    entity_addr_t t = peer_addr_for_me;
    t.set_port(my_inst.addr.get_port());
    my_inst.addr.set_sockaddr(t.get_sockaddr());
    ldout(cct,2) << "learned my addr " << my_inst.addr << dendl;
    need_addr = false;
    // init_local_connection();
  }
  sh_mtx.Unlock();

}

int XioMessenger::new_session(struct xio_session *session,
			      struct xio_new_session_req *req,
			      void *cb_user_context)
{
  if (shutdown_called) {
    return xio_reject(
      session, XIO_E_SESSION_REFUSED, NULL /* udata */, 0 /* udata len */);
  }
  int code = portals.accept(session, req, cb_user_context);
  if (! code)
    nsessions++;
  return code;
} /* new_session */

int XioMessenger::session_event(struct xio_session *session,
				struct xio_session_event_data *event_data,
				void *cb_user_context)
{
  XioConnection *xcon;

  switch (event_data->event) {
  case XIO_SESSION_CONNECTION_ESTABLISHED_EVENT:
  {
    struct xio_connection *conn = event_data->conn;
    struct xio_connection_attr xcona;
    entity_addr_t peer_addr_for_me, paddr;

    xcon = static_cast<XioConnection*>(event_data->conn_user_context);

    ldout(cct,2) << "connection established " << event_data->conn
      << " session " << session << " xcon " << xcon << dendl;

    (void) xio_query_connection(conn, &xcona,
				XIO_CONNECTION_ATTR_LOCAL_ADDR|
				XIO_CONNECTION_ATTR_PEER_ADDR);
    peer_addr_for_me.set_sockaddr((struct sockaddr *)&xcona.local_addr);
    paddr.set_sockaddr((struct sockaddr *)&xcona.peer_addr);
    //set_myaddr(peer_addr_for_me);
    learned_addr(peer_addr_for_me);
    ldout(cct,2) << "client: connected from " << peer_addr_for_me << " to " << paddr << dendl;

    /* notify hook */
    this->ms_deliver_handle_connect(xcon);
    this->ms_deliver_handle_fast_connect(xcon);
  }
  break;

  case XIO_SESSION_NEW_CONNECTION_EVENT:
  {
    struct xio_connection *conn = event_data->conn;
    struct xio_connection_attr xcona;
    entity_inst_t s_inst;
    entity_addr_t peer_addr_for_me;

    (void) xio_query_connection(conn, &xcona,
				XIO_CONNECTION_ATTR_CTX|
				XIO_CONNECTION_ATTR_PEER_ADDR|
				XIO_CONNECTION_ATTR_LOCAL_ADDR);
    /* XXX assumes RDMA */
    s_inst.addr.set_sockaddr((struct sockaddr *)&xcona.peer_addr);
    peer_addr_for_me.set_sockaddr((struct sockaddr *)&xcona.local_addr);

    xcon = new XioConnection(this, XioConnection::PASSIVE, s_inst);
    xcon->session = session;

    struct xio_context_attr xctxa;
    (void) xio_query_context(xcona.ctx, &xctxa, XIO_CONTEXT_ATTR_USER_CTX);

    xcon->conn = conn;
    xcon->portal = static_cast<XioPortal*>(xctxa.user_context);
    ceph_assert(xcon->portal);

    xcona.user_context = xcon;
    (void) xio_modify_connection(conn, &xcona, XIO_CONNECTION_ATTR_USER_CTX);

    xcon->connected = true;

    /* sentinel ref */
    xcon->get(); /* xcon->nref == 1 */
    conns_sp.lock();
    conns_list.push_back(*xcon);
    /* XXX we can't put xcon in conns_entity_map becase we don't yet know
     * it's peer address */
    conns_sp.unlock();

    /* XXXX pre-merge of session startup negotiation ONLY! */
    xcon->cstate.state_up_ready(XioConnection::CState::OP_FLAG_NONE);

    ldout(cct,2) << "New connection session " << session
      << " xcon " << xcon << " on msgr: " << this << " portal: " << xcon->portal << dendl;
    ldout(cct,2) << "Server: connected from " << s_inst.addr << " to " << peer_addr_for_me << dendl;
  }
  break;
  case XIO_SESSION_CONNECTION_ERROR_EVENT:
  case XIO_SESSION_CONNECTION_CLOSED_EVENT: /* orderly discon */
  case XIO_SESSION_CONNECTION_DISCONNECTED_EVENT: /* unexpected discon */
  case XIO_SESSION_CONNECTION_REFUSED_EVENT:
    xcon = static_cast<XioConnection*>(event_data->conn_user_context);
    ldout(cct,2) << xio_session_event_str(event_data->event)
      << " xcon " << xcon << " session " << session  << dendl;
    if (likely(!!xcon)) {
      unregister_xcon(xcon);
      xcon->on_disconnect_event();
    }
    break;
  case XIO_SESSION_CONNECTION_TEARDOWN_EVENT:
    xcon = static_cast<XioConnection*>(event_data->conn_user_context);
    ldout(cct,2) << xio_session_event_str(event_data->event)
      << " xcon " << xcon << " session " << session << dendl;
    /*
     * There are flows where Accelio sends teardown event without going
     * through disconnect event. so we make sure we cleaned the connection.
     */
    unregister_xcon(xcon);
    xcon->on_teardown_event();
    break;
  case XIO_SESSION_TEARDOWN_EVENT:
    ldout(cct,2) << xio_session_event_str(event_data->event)
      << " session " << session << dendl;
    if (unlikely(XioPool::trace_mempool)) {
      xp_stats.dump("xio session dtor", reinterpret_cast<uint64_t>(session));
    }
    xio_session_destroy(session);
    if (--nsessions == 0) {
      Mutex::Locker lck(sh_mtx);
      if (nsessions == 0)
	sh_cond.Signal();
    }
    break;
  default:
    break;
  };

  return 0;
}

enum bl_type
{
  BUFFER_PAYLOAD,
  BUFFER_MIDDLE,
  BUFFER_DATA
};

#define MAX_XIO_BUF_SIZE 1044480

static inline int
xio_count_buffers(const buffer::list& bl, int& req_size, int& msg_off, int& req_off)
{

  const std::list<buffer::ptr>& buffers = bl.buffers();
  list<bufferptr>::const_iterator pb;
  size_t size, off;
  int result;
  int first = 1;

  off = size = 0;
  result = 0;
  for (;;) {
    if (off >= size) {
      if (first) pb = buffers.begin(); else ++pb;
      if (pb == buffers.end()) {
	break;
      }
      off = 0;
      size = pb->length();
      first = 0;
    }
    size_t count = size - off;
    if (!count) continue;
    if (req_size + count > MAX_XIO_BUF_SIZE) {
	count = MAX_XIO_BUF_SIZE - req_size;
    }

    ++result;

    /* advance iov and perhaps request */

    off += count;
    req_size += count;
    ++msg_off;
    if (unlikely(msg_off >= XIO_MSGR_IOVLEN || req_size >= MAX_XIO_BUF_SIZE)) {
      ++req_off;
      msg_off = 0;
      req_size = 0;
    }
  }

  return result;
}

static inline void
xio_place_buffers(const buffer::list& bl, XioMsg *xmsg, struct xio_msg*& req,
		  struct xio_iovec_ex*& msg_iov, int& req_size,
		  int ex_cnt, int& msg_off, int& req_off, bl_type type)
{

  const std::list<buffer::ptr>& buffers = bl.buffers();
  list<bufferptr>::const_iterator pb;
  struct xio_iovec_ex* iov;
  size_t size, off;
  const char *data = NULL;
  int first = 1;

  off = size = 0;
  for (;;) {
    if (off >= size) {
      if (first) pb = buffers.begin(); else ++pb;
      if (pb == buffers.end()) {
	break;
      }
      off = 0;
      size = pb->length();
      data = pb->c_str();	 // is c_str() efficient?
      first = 0;
    }
    size_t count = size - off;
    if (!count) continue;
    if (req_size + count > MAX_XIO_BUF_SIZE) {
	count = MAX_XIO_BUF_SIZE - req_size;
    }

    /* assign buffer */
    iov = &msg_iov[msg_off];
    iov->iov_base = (void *) (&data[off]);
    iov->iov_len = count;

    switch (type) {
    case BUFFER_DATA:
      //break;
    default:
    {
      struct xio_reg_mem *mp = get_xio_mp(*pb);
      iov->mr = (mp) ? mp->mr : NULL;
    }
      break;
    }

    /* advance iov(s) */

    off += count;
    req_size += count;
    ++msg_off;

    /* next request if necessary */

    if (unlikely(msg_off >= XIO_MSGR_IOVLEN || req_size >= MAX_XIO_BUF_SIZE)) {
      /* finish this request */
      req->out.pdata_iov.nents = msg_off;
      /* advance to next, and write in it if it's not the last one. */
      if (++req_off >= ex_cnt) {
	req = 0;	/* poison.  trap if we try to use it. */
	msg_iov = NULL;
      } else {
	req = &xmsg->req_arr[req_off].msg;
	msg_iov = req->out.pdata_iov.sglist;
      }
      msg_off = 0;
      req_size = 0;
    }
  }
}

int XioMessenger::bind(const entity_addr_t& addr)
{
  if (addr.is_blank_ip()) {
      lderr(cct) << "ERROR: need rdma ip for remote use! " << dendl;
      cout << "Error: xio bind failed. public/cluster ip not specified" << std::endl;
      return -1;
  }

  entity_addr_t shift_addr = addr;
  string base_uri = xio_uri_from_entity(cct->_conf->xio_transport_type,
					shift_addr, false /* want_port */);
  ldout(cct,4) << "XioMessenger " << this << " bind: xio_uri "
    << base_uri << ':' << shift_addr.get_port() << dendl;

  uint16_t port0;
  int r = portals.bind(&xio_msgr_ops, base_uri, shift_addr.get_port(), &port0);
  if (r == 0) {
    shift_addr.set_port(port0);
    shift_addr.nonce = nonce;
    set_myaddr(shift_addr);
    need_addr = false;
    did_bind = true;
  }
  return r;
} /* bind */

int XioMessenger::rebind(const set<int>& avoid_ports)
{
  ldout(cct,4) << "XioMessenger " << this << " rebind attempt" << dendl;
  return 0;
} /* rebind */

int XioMessenger::start()
{
  portals.start();
  dispatch_strategy->start();
  if (!did_bind) {
	  my_inst.addr.nonce = nonce;
  }
  started = true;
  return 0;
}

void XioMessenger::wait()
{
  portals.join();
  dispatch_strategy->wait();
} /* wait */

int XioMessenger::_send_message(Message *m, const entity_inst_t& dest)
{
  ConnectionRef conn = get_connection(dest);
  if (conn)
    return _send_message(m, &(*conn));
  else
    return EINVAL;
} /* send_message(Message *, const entity_inst_t&) */

static inline XioMsg* pool_alloc_xio_msg(Message *m, XioConnection *xcon,
  int ex_cnt)
{
  struct xio_reg_mem mp_mem;
  int e = xpool_alloc(xio_msgr_noreg_mpool, sizeof(XioMsg), &mp_mem);
  if (!!e)
    return NULL;
  XioMsg *xmsg = reinterpret_cast<XioMsg*>(mp_mem.addr);
  ceph_assert(!!xmsg);
  new (xmsg) XioMsg(m, xcon, mp_mem, ex_cnt, CEPH_FEATURES_ALL);
  return xmsg;
}

XioCommand* pool_alloc_xio_command(XioConnection *xcon)
{
  struct xio_reg_mem mp_mem;
  int e = xpool_alloc(xio_msgr_noreg_mpool, sizeof(XioCommand), &mp_mem);
  if (!!e)
    return NULL;
  XioCommand *xcmd = reinterpret_cast<XioCommand*>(mp_mem.addr);
  ceph_assert(!!xcmd);
  new (xcmd) XioCommand(xcon, mp_mem);
  return xcmd;
}

int XioMessenger::_send_message(Message *m, Connection *con)
{
  if (con == loop_con.get() /* intrusive_ptr get() */) {
    m->set_connection(con);
    m->set_src(get_myinst().name);
    m->set_seq(loop_con->next_seq());
    ds_dispatch(m);
    return 0;
  }

  XioConnection *xcon = static_cast<XioConnection*>(con);

  /* If con is not in READY state, we have to enforce policy */
  if (xcon->cstate.session_state.read() != XioConnection::UP) {
    std::lock_guard<decltype(xcon->sp) lg(xcon->sp);

    if (xcon->cstate.session_state.read() != XioConnection::UP) {
      xcon->outgoing.mqueue.push_back(*m);
      return 0;
    }
  }

  return _send_message_impl(m, xcon);
} /* send_message(Message* m, Connection *con) */

int XioMessenger::_send_message_impl(Message* m, XioConnection* xcon)
{
  int code = 0;

  Mutex::Locker l(xcon->lock);
  if (unlikely(XioPool::trace_mempool)) {
    static uint32_t nreqs;
    if (unlikely((++nreqs % 65536) == 0)) {
      xp_stats.dump(__func__, nreqs);
    }
  }

  m->set_seq(xcon->state.next_out_seq());
  m->set_magic(magic); // trace flags and special handling

  m->encode(xcon->get_features(), this->crcflags);

  buffer::list &payload = m->get_payload();
  buffer::list &middle = m->get_middle();
  buffer::list &data = m->get_data();

  int msg_off = 0;
  int req_off = 0;
  int req_size = 0;
  int nbuffers =
    xio_count_buffers(payload, req_size, msg_off, req_off) +
    xio_count_buffers(middle, req_size, msg_off, req_off) +
    xio_count_buffers(data, req_size, msg_off, req_off);

  int ex_cnt = req_off;
  if (msg_off == 0 && ex_cnt > 0) {
    // no buffers for last msg
    ldout(cct,10) << "msg_off 0, ex_cnt " << ex_cnt << " -> " << ex_cnt-1 << dendl;
    ex_cnt--;
  }

  /* get an XioMsg frame */
  XioMsg *xmsg = pool_alloc_xio_msg(m, xcon, ex_cnt);
  if (! xmsg) {
    /* could happen if Accelio has been shutdown */
    return ENOMEM;
  }

  ldout(cct,4) << __func__ << " " << m << " new XioMsg " << xmsg
       << " tag " << (int)xmsg->hdr.tag
       << " req_0 " << xmsg->get_xio_msg() << " msg type " << m->get_type()
       << " features: " << xcon->get_features()
       << " conn " << xcon->conn << " sess " << xcon->session << dendl;

  if (magic & (MSG_MAGIC_XIO)) {

    /* XXXX verify */
    switch (m->get_type()) {
    case 43:
    // case 15:
      ldout(cct,4) << __func__ << " stop 43 " << m->get_type() << " " << *m << dendl;
      buffer::list &payload = m->get_payload();
      ldout(cct,4) << __func__ << " payload dump:" << dendl;
      payload.hexdump(cout);
    }
  }

  struct xio_msg *req = xmsg->get_xio_msg();
  struct xio_iovec_ex *msg_iov = req->out.pdata_iov.sglist;

  if (magic & (MSG_MAGIC_XIO)) {
    ldout(cct,4) << "payload: " << payload.buffers().size() <<
      " middle: " << middle.buffers().size() <<
      " data: " << data.buffers().size() <<
      dendl;
  }

  if (unlikely(ex_cnt > 0)) {
    ldout(cct,4) << __func__ << " buffer cnt > XIO_MSGR_IOVLEN (" <<
      ((XIO_MSGR_IOVLEN-1) + nbuffers) << ")" << dendl;
  }

  /* do the invariant part */
  msg_off = 0;
  req_off = -1; /* most often, not used */
  req_size = 0;

  xio_place_buffers(payload, xmsg, req, msg_iov, req_size, ex_cnt, msg_off,
		    req_off, BUFFER_PAYLOAD);

  xio_place_buffers(middle, xmsg, req, msg_iov, req_size, ex_cnt, msg_off,
		    req_off, BUFFER_MIDDLE);

  xio_place_buffers(data, xmsg, req, msg_iov, req_size, ex_cnt, msg_off,
		    req_off, BUFFER_DATA);
  ldout(cct,10) << "ex_cnt " << ex_cnt << ", req_off " << req_off
    << ", msg_cnt " << xmsg->get_msg_count() << dendl;

  /* finalize request */
  if (msg_off)
    req->out.pdata_iov.nents = msg_off;

  /* fixup first msg */
  req = xmsg->get_xio_msg();

  const std::list<buffer::ptr>& header = xmsg->hdr.get_bl().buffers();
  ceph_assert(header.size() == 1); /* XXX */
  list<bufferptr>::const_iterator pb = header.begin();
  req->out.header.iov_base = (char*) pb->c_str();
  req->out.header.iov_len = pb->length();

  /* deliver via xio, preserve ordering */
  if (xmsg->get_msg_count() > 1) {
    struct xio_msg *head = xmsg->get_xio_msg();
    struct xio_msg *tail = head;
    for (req_off = 0; ((unsigned) req_off) < xmsg->get_msg_count()-1; ++req_off) {
      req = &xmsg->req_arr[req_off].msg;
assert(!req->in.pdata_iov.nents);
assert(req->out.pdata_iov.nents || !nbuffers);
      tail->next = req;
      tail = req;
     }
    tail->next = NULL;
  }
  xmsg->trace = m->trace;
  m->trace.event("xio portal enqueue for send");
  m->trace.keyval("xio message segments", xmsg->hdr.msg_cnt);
  xcon->portal->enqueue_for_send(xcon, xmsg);

  return code;
} /* send_message(Message *, Connection *) */

int XioMessenger::shutdown()
{
  shutdown_called = true;
  conns_sp.lock();
  XioConnection::ConnList::iterator iter;
  iter = conns_list.begin();
  for (iter = conns_list.begin(); iter != conns_list.end(); ++iter) {
    (void) iter->disconnect(); // XXX mark down?
  }
  conns_sp.unlock();
  while(nsessions > 0) {
    Mutex::Locker lck(sh_mtx);
    if (nsessions > 0)
      sh_cond.Wait(sh_mtx);
  }
  portals.shutdown();
  dispatch_strategy->shutdown();
  did_bind = false;
  started = false;
  return 0;
} /* shutdown */

ConnectionRef XioMessenger::get_connection(const entity_inst_t& dest)
{
  if (shutdown_called)
    return NULL;

  const entity_inst_t& self_inst = get_myinst();
  if ((&dest == &self_inst) ||
      (dest == self_inst)) {
    return get_loopback_connection();
  }

  conns_sp.lock();
  XioConnection::EntitySet::iterator conn_iter =
    conns_entity_map.find(dest, XioConnection::EntityComp());
  if (conn_iter != conns_entity_map.end()) {
    ConnectionRef cref = &(*conn_iter);
    conns_sp.unlock();
    return cref;
  }
  else {
    conns_sp.unlock();
    string xio_uri = xio_uri_from_entity(cct->_conf->xio_transport_type,
					 dest.addr, true /* want_port */);

    ldout(cct,4) << "XioMessenger " << this << " get_connection: xio_uri "
      << xio_uri << dendl;

    /* XXX client session creation parameters */
    struct xio_session_params params = {};
    params.type         = XIO_SESSION_CLIENT;
    params.ses_ops      = &xio_msgr_ops;
    params.user_context = this;
    params.uri          = xio_uri.c_str();

    XioConnection *xcon = new XioConnection(this, XioConnection::ACTIVE,
					    dest);

    xcon->session = xio_session_create(&params);
    if (! xcon->session) {
      delete xcon;
      return NULL;
    }

    /* this should cause callbacks with user context of conn, but
     * we can always set it explicitly */
    struct xio_connection_params xcp = {};
    xcp.session           = xcon->session;
    xcp.ctx               = xcon->portal->ctx;
    xcp.conn_user_context = xcon;

    xcon->conn = xio_connect(&xcp);
    if (!xcon->conn) {
      xio_session_destroy(xcon->session);
      delete xcon;
      return NULL;
    }

    nsessions++;
    xcon->connected = true;

    /* sentinel ref */
    xcon->get(); /* xcon->nref == 1 */
    conns_sp.lock();
    conns_list.push_back(*xcon);
    conns_entity_map.insert(*xcon);
    conns_sp.unlock();

    /* XXXX pre-merge of session startup negotiation ONLY! */
    xcon->cstate.state_up_ready(XioConnection::CState::OP_FLAG_NONE);

    ldout(cct,2) << "New connection xcon: " << xcon <<
      " up_ready on session " << xcon->session <<
      " on msgr: " << this << " portal: " << xcon->portal << dendl;

    return xcon->get(); /* nref +1 */
  }
} /* get_connection */

ConnectionRef XioMessenger::get_loopback_connection()
{
  return (loop_con.get());
} /* get_loopback_connection */

void XioMessenger::unregister_xcon(XioConnection *xcon)
{
  std::lock_guard<decltype(conns_sp)> lckr(conns_sp);

  XioConnection::EntitySet::iterator conn_iter =
	conns_entity_map.find(xcon->peer, XioConnection::EntityComp());
  if (conn_iter != conns_entity_map.end()) {
	XioConnection *xcon2 = &(*conn_iter);
	if (xcon == xcon2) {
	  conns_entity_map.erase(conn_iter);
	}
  }

  /* check if citer on conn_list */
  if (xcon->conns_hook.is_linked()) {
    /* now find xcon on conns_list and erase */
    XioConnection::ConnList::iterator citer =
        XioConnection::ConnList::s_iterator_to(*xcon);
    conns_list.erase(citer);
  }
}

void XioMessenger::mark_down(const entity_addr_t& addr)
{
  entity_inst_t inst(entity_name_t(), addr);
  std::lock_guard<decltype(conns_sp)> lckr(conns_sp);
  XioConnection::EntitySet::iterator conn_iter =
    conns_entity_map.find(inst, XioConnection::EntityComp());
  if (conn_iter != conns_entity_map.end()) {
      (*conn_iter)._mark_down(XioConnection::CState::OP_FLAG_NONE);
    }
} /* mark_down(const entity_addr_t& */

void XioMessenger::mark_down(Connection* con)
{
  XioConnection *xcon = static_cast<XioConnection*>(con);
  xcon->_mark_down(XioConnection::CState::OP_FLAG_NONE);
} /* mark_down(Connection*) */

void XioMessenger::mark_down_all()
{
  std::lock_guard<decltype(conns_sp)> lckr(conns_sp);
  XioConnection::EntitySet::iterator conn_iter;
  for (conn_iter = conns_entity_map.begin(); conn_iter !=
	 conns_entity_map.begin(); ++conn_iter) {
    (*conn_iter)._mark_down(XioConnection::CState::OP_FLAG_NONE);
  }
} /* mark_down_all */

static inline XioMarkDownHook* pool_alloc_markdown_hook(
  XioConnection *xcon, Message *m)
{
  struct xio_reg_mem mp_mem;
  int e = xio_mempool_alloc(xio_msgr_noreg_mpool,
			    sizeof(XioMarkDownHook), &mp_mem);
  if (!!e)
    return NULL;
  XioMarkDownHook *hook = static_cast<XioMarkDownHook*>(mp_mem.addr);
  new (hook) XioMarkDownHook(xcon, m, mp_mem);
  return hook;
}

void XioMessenger::mark_down_on_empty(Connection* con)
{
  XioConnection *xcon = static_cast<XioConnection*>(con);
  MNop* m = new MNop();
  m->tag = XIO_NOP_TAG_MARKDOWN;
  m->set_completion_hook(pool_alloc_markdown_hook(xcon, m));
  // stall new messages
  xcon->cstate.session_state = XioConnection::session_states::BARRIER;
  (void) _send_message_impl(m, xcon);
}

void XioMessenger::mark_disposable(Connection *con)
{
  XioConnection *xcon = static_cast<XioConnection*>(con);
  xcon->_mark_disposable(XioConnection::CState::OP_FLAG_NONE);
}

void XioMessenger::try_insert(XioConnection *xcon)
{
  std::lock_guard<decltype(conns_sp)> lckr(conns_sp);
  /* already resident in conns_list */
  conns_entity_map.insert(*xcon);
}

XioMessenger::~XioMessenger()
{
  delete dispatch_strategy;
  nInstances--;
} /* dtor */

ext: cc

max_stars_repo_name: liucxer/ceph-msg

max_issues_repo_name: liucxer/ceph-msg

max_forks_repo_name: liucxer/ceph-msg


text:
#include "order.h"
#include <boost/scoped_ptr.hpp>
#include "depth_feed_subscriber.h"
#include <Codecs/DataSourceBuffer.h>
#include <Codecs/GenericMessageBuilder.h>
#include <Codecs/SingleMessageConsumer.h>
#include <Messages/MessageAccessor.h>

namespace liquibook { namespace examples {

const uint64_t DepthFeedSubscriber::MSG_TYPE_DEPTH(11);
const uint64_t DepthFeedSubscriber::MSG_TYPE_TRADE(22);

using QuickFAST::ValueType;

DepthFeedSubscriber::DepthFeedSubscriber(
        const QuickFAST::Codecs::TemplateRegistryPtr& templates)
: decoder_(templates),
  expected_seq_(1)
{
}

void
DepthFeedSubscriber::handle_reset()
{
  expected_seq_ = 1;
}

bool
DepthFeedSubscriber::handle_message(BufferPtr& bp, size_t bytes_transferred)
{
  // Decode the message
  QuickFAST::Codecs::DataSourceBuffer source(bp->c_array(), bytes_transferred);
  QuickFAST::Codecs::SingleMessageConsumer consumer;
  QuickFAST::Codecs::GenericMessageBuilder builder(consumer);
  decoder_.decodeMessage(source, builder);
  QuickFAST::Messages::Message& msg(consumer.message());

  // Examine message contents
  uint64_t seq_num, msg_type, timestamp;
  const QuickFAST::StringBuffer* string_buffer;
  size_t bids_length, asks_length;
  std::string symbol;
  if (!msg.getUnsignedInteger(id_seq_num_, ValueType::UINT32, seq_num)) {
    std::cout << "Could not get seq num from msg" << std::endl;
    return false;
  }
  if (seq_num != expected_seq_) {
    std::cout << "ERROR: Got Seq num " << seq_num << ", expected "
              << expected_seq_ << std::endl;
    return false;
  }
  if (!msg.getUnsignedInteger(id_msg_type_, ValueType::UINT32, msg_type)) {
    std::cout << "Could not get msg type from msg" << std::endl;
    return false;
  }
  if (!msg.getString(id_symbol_, ValueType::ASCII, string_buffer)) {
    std::cout << "Could not get symbol from msg" << std::endl;
    return false;
  }
  if (!msg.getUnsignedInteger(id_timestamp_, ValueType::UINT32, timestamp)) {
    std::cout << "Could not get timestamp from msg" << std::endl;
    return false;
  }
  bool result = false;
  symbol = (std::string)*string_buffer;
  switch (msg_type) {
  case MSG_TYPE_DEPTH:
    result = handle_depth_message(symbol, seq_num, timestamp, msg);
    break;
  case MSG_TYPE_TRADE:
    result = handle_trade_message(symbol, seq_num, timestamp, msg);
    break;
  default:
    std::cout << "ERROR: Unknown message type " << msg_type
              << " seq num " << seq_num << std::endl;
    return false;
  }
  ++expected_seq_;
  return result;
}

void
DepthFeedSubscriber::log_depth(book::Depth<5>& depth)
{
  book::DepthLevel* bid = depth.bids();
  book::DepthLevel* ask = depth.asks();
  printf("----------BID----------    ----------ASK----------\n");
  while (bid || ask) {
    if (bid && bid->order_count()) {
      printf("%8.2f %9d [%2d]",
             (double)bid->price() / Order::precision_,
             bid->aggregate_qty(), bid->order_count());
      if (bid == depth.last_bid_level()) {
        bid = NULL;
      } else {
        ++bid;
      }
    } else {
      // Blank lines
      printf("                       ");
      bid = NULL;
    }

    if (ask && ask->order_count()) {
      printf("    %8.2f %9d [%2d]\n",
             (double)ask->price() / Order::precision_,
             ask->aggregate_qty(), ask->order_count());
      if (ask == depth.last_ask_level()) {
        ask = NULL;
      } else {
        ++ask;
      }
    } else {
      // Newline
      printf("\n");
      ask = NULL;
    }
  }
}

bool
DepthFeedSubscriber::handle_depth_message(
  const std::string& symbol,
  uint64_t& seq_num,
  uint64_t& timestamp,
  QuickFAST::Messages::Message& msg)
{
  size_t bids_length, asks_length;
  std::cout << timestamp
            << " Got depth msg " << seq_num
            << " for symbol " << symbol << std::endl;

  // Create or find depth
  std::pair<DepthMap::iterator, bool> results = depth_map_.insert(
      std::make_pair(symbol, book::Depth<5>()));
  book::Depth<5>& depth = results.first->second;

  if (msg.getSequenceLength(id_bids_, bids_length)) {
    for (size_t i = 0; i < bids_length; ++i) {
      const QuickFAST::Messages::MessageAccessor* accessor;
      if (msg.getSequenceEntry(id_bids_, i, accessor)) {
        uint64_t level_num, price, order_count, aggregate_qty;
        if (!accessor->getUnsignedInteger(id_level_num_, ValueType::UINT8,
                                         level_num)) {
          std::cout << "Could not get Bid level from depth msg" << std::endl;
          return false;
        }
        if (!accessor->getUnsignedInteger(id_price_, ValueType::UINT32,
                                         price)) {
          std::cout << "Could not get Bid price from depth msg" << std::endl;
          return false;
        }
        if (!accessor->getUnsignedInteger(id_order_count_, ValueType::UINT32,
                                         order_count)) {
          std::cout << "Could not get Bid count from depth msg" << std::endl;
          return false;
        }
        if (!accessor->getUnsignedInteger(id_size_, ValueType::UINT32,
                                         aggregate_qty)) {
          std::cout << "Could not get Bid agg qty  from depth msg" << std::endl;
          return false;
        }

        book::DepthLevel& level = depth.bids()[level_num];
        level.set(price, aggregate_qty, order_count);

      } else {
        std::cout << "Failed to get bid " << i << std::endl;
        return false;
      }
      msg.endSequenceEntry(id_bids_, i, accessor);
    }
  }
  if (msg.getSequenceLength(id_asks_, asks_length)) {
    for (size_t i = 0; i < asks_length; ++i) {
      const QuickFAST::Messages::MessageAccessor* accessor;
      if (msg.getSequenceEntry(id_asks_, i, accessor)) {
        uint64_t level_num, price, order_count, aggregate_qty;
        if (!accessor->getUnsignedInteger(id_level_num_, ValueType::UINT8,
                                         level_num)) {
          std::cout << "Could not get Ask level from depth msg " << std::endl;
          return false;
        }
        if (!accessor->getUnsignedInteger(id_price_, ValueType::UINT32,
                                         price)) {
          std::cout << "Could not get Ask price  from depth msg" << std::endl;
          return false;
        }
        if (!accessor->getUnsignedInteger(id_order_count_, ValueType::UINT32,
                                         order_count)) {
          std::cout << "Could not get Ask count from depth msg " << std::endl;
          return false;
        }
        if (!accessor->getUnsignedInteger(id_size_, ValueType::UINT32,
                                         aggregate_qty)) {
          std::cout << "Could not get Ask agg qty from depth msg " << std::endl;
          return false;
        }

        book::DepthLevel& level = depth.asks()[level_num];
        level.set(price, aggregate_qty, order_count);

      } else {
        std::cout << "Failed to get ask " << i << std::endl;
        return false;
      }
      msg.endSequenceEntry(id_asks_, i, accessor);
    }
  }
  log_depth(depth);
  return true;
}

bool
DepthFeedSubscriber::handle_trade_message(
  const std::string& symbol,
  uint64_t& seq_num,
  uint64_t& timestamp,
  QuickFAST::Messages::Message& msg)
{
  uint64_t qty, cost;
  // Get trade fields
  if (!msg.getUnsignedInteger(id_qty_, ValueType::UINT32, qty)) {
    std::cout << "Could not qty from trade msg" << std::endl;
    return false;
  }
  if (!msg.getUnsignedInteger(id_cost_, ValueType::UINT32, cost)) {
    std::cout << "Could not get cost from trade msg" << std::endl;
    return false;
  }

  double price = (double) cost / (qty * Order::precision_);
  std::cout << timestamp
            << " Got trade msg " << seq_num
            << " for symbol " << symbol
            << ": " << qty << "@" << price
            << std::endl;

  return true;
}

} }

ext: cpp

max_stars_repo_name: arthurcolle/liquibook

max_issues_repo_name: arthurcolle/liquibook

max_forks_repo_name: arthurcolle/liquibook


text:
/*
 * Low level data bit manipulation
 */

#ifndef DATA_BIT_HPP_INCLUCED
#define DATA_BIT_HPP_INCLUCED

#include <boost/mpl/at.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/map.hpp>
#include <aero/aero_basic_type.hpp>

namespace aero
{
	namespace databit { namespace detail {

		struct underflow_traits
		{
			typedef underflow_traits this_traits;
			typedef this_traits value_type;

			typedef this_traits half_traits;
			typedef half_traits::value_type half_value_type;

			enum { size = -1 };
		};

		struct data8_traits
		{
			typedef data8_traits this_traits;
			typedef boost::uint8_t value_type;

			typedef underflow_traits half_traits;
			typedef half_traits::value_type half_value_type;

			enum { size = 8 * sizeof(value_type) };
		};

		struct data16_traits
		{
			typedef data16_traits this_traits;
			typedef boost::uint16_t value_type;

			typedef data8_traits half_traits;
			typedef half_traits::value_type half_value_type;

			enum { size = 8 * sizeof(value_type) };
		};

		struct data32_traits
		{
			typedef data32_traits this_traits;
			typedef boost::uint32_t value_type;

			typedef data16_traits half_traits;
			typedef half_traits::value_type half_value_type;

			enum { size = 8 * sizeof(value_type) };
		};

		struct data64_traits
		{
			typedef data64_traits this_traits;
			typedef boost::uint64_t value_type;

			typedef data32_traits::value_type half_value_type;
			typedef data32_traits half_traits;

			enum { size = 8 * sizeof(value_type) };
		};

		template <typename TypeTraits, unsigned int splitCount>
		struct split_type_worker
		{
			typedef typename split_type_worker
			<
				typename TypeTraits::half_traits,
				splitCount / 2
			>::type_traits type_traits;
		};

		template <typename TypeTraits>
		struct split_type_worker<TypeTraits, 1>
		{
			typedef typename TypeTraits type_traits;
		};

		template <typename TypeTraits>
		struct split_type_worker<TypeTraits, 0>
		{
			typedef typename TypeTraits type_traits;
		};

		typedef boost::mpl::map
		<
#if defined(UCHAR_MAX) && UCHAR_MAX == 0xFF
			boost::mpl::pair<char, data8_traits>,
			boost::mpl::pair<unsigned char, data8_traits>,
#elif defined(UCHAR_MAX) && UCHAR_MAX == 0xFFFF
			boost::mpl::pair<char, data16_traits>,
			boost::mpl::pair<unsigned char, data16_traits>,
#elif defined(UCHAR_MAX) && UCHAR_MAX == 0xFFFFFFFF
			boost::mpl::pair<char, data32_traits>,
			boost::mpl::pair<unsigned char, data32_traits>,
#elif defined(UCHAR_MAX) && UCHAR_MAX == 0xFFFFFFFFFFFFFFFF
			boost::mpl::pair<char, data64_traits>,
			boost::mpl::pair<unsigned char, data64_traits>,
#else
			boost::mpl::pair<char, data8_traits>,
			boost::mpl::pair<unsigned char, data8_traits>,
#endif

#if defined(USHRT_MAX) && USHRT_MAX == 0xFF
			boost::mpl::pair<short, data8_traits>,
			boost::mpl::pair<unsigned short, data8_traits>,
#elif defined(USHRT_MAX) && USHRT_MAX == 0xFFFF
			boost::mpl::pair<short, data16_traits>,
			boost::mpl::pair<unsigned short, data16_traits>,
#elif defined(USHRT_MAX) && USHRT_MAX == 0xFFFFFFFF
			boost::mpl::pair<short, data32_traits>,
			boost::mpl::pair<unsigned short, data32_traits>,
#elif defined(USHRT_MAX) && USHRT_MAX == 0xFFFFFFFFFFFFFFFF
			boost::mpl::pair<short, data64_traits>,
			boost::mpl::pair<unsigned short, data64_traits>,
#else
			boost::mpl::pair<short, data16_traits>,
			boost::mpl::pair<unsigned short, data16_traits>,
#endif

#if defined(UINT_MAX) && UINT_MAX == 0xFF
			boost::mpl::pair<int, data8_traits>,
			boost::mpl::pair<unsigned int, data8_traits>,
#elif defined(UINT_MAX) && UINT_MAX == 0xFFFF
			boost::mpl::pair<int, data16_traits>,
			boost::mpl::pair<unsigned int, data16_traits>,
#elif defined(UINT_MAX) && UINT_MAX == 0xFFFFFFFF
			boost::mpl::pair<int, data32_traits>,
			boost::mpl::pair<unsigned int, data32_traits>,
#elif defined(UINT_MAX) && UINT_MAX == 0xFFFFFFFFFFFFFFFF
			boost::mpl::pair<int, data64_traits>,
			boost::mpl::pair<unsigned int, data64_traits>,
#else
			boost::mpl::pair<int, data32_traits>,
			boost::mpl::pair<unsigned int, data32_traits>,
#endif

#if defined(ULONG_MAX) && ULONG_MAX == 0xFF
			boost::mpl::pair<long, data8_traits>,
			boost::mpl::pair<unsigned long, data8_traits>,
#elif defined(ULONG_MAX) && ULONG_MAX == 0xFFFF
			boost::mpl::pair<long, data16_traits>,
			boost::mpl::pair<unsigned long, data16_traits>,
#elif defined(ULONG_MAX) && ULONG_MAX == 0xFFFFFFFF
			boost::mpl::pair<long, data32_traits>,
			boost::mpl::pair<unsigned long, data32_traits>,
#elif defined(ULONG_MAX) && ULONG_MAX == 0xFFFFFFFFFFFFFFFF
			boost::mpl::pair<long, data64_traits>,
			boost::mpl::pair<unsigned long, data64_traits>,
#else
			boost::mpl::pair<long, data32_traits>,
			boost::mpl::pair<unsigned long, data32_traits>,
#endif

#ifdef _MSC_VER
			boost::mpl::pair<__int64, data64_traits>,
			boost::mpl::pair<unsigned __int64, data64_traits>,
#endif
			boost::mpl::pair<long long, data64_traits>,
			boost::mpl::pair<unsigned long long, data64_traits>
		> type_traits_map;

		template <typename IntType, unsigned int i>
		struct split_type
		{
			typedef IntType int_type;
			typedef typename boost::mpl::if_
			<
				boost::is_same
				<
					typename boost::mpl::at<type_traits_map, int_type>::type,
					boost::mpl::void_
				>,
				boost::mpl::void_,
				typename boost::mpl::at<type_traits_map, int_type>::type
			>::type original_type_traits;
			typedef typename original_type_traits::value_type original_value_type;  // Change from signed to unsigned, if any.

			typedef typename split_type_worker<original_type_traits, i>::type_traits type_traits;
			typedef typename type_traits::value_type value_type;
		};
	} }

	template <unsigned int splitCount, typename Type>
	inline
	typename databit::detail::split_type<Type, splitCount>::value_type number_split(Type n, unsigned int splitIndex)
	{
		typedef typename databit::detail::split_type<Type, splitCount> split_type;

		return (split_type::value_type)
		(
			((split_type::original_value_type)((split_type::value_type)-1)) /* mask*/ &
			(
				(split_type::original_type_traits::size * splitIndex / splitCount) ?
					(((split_type::original_value_type)n) >>
						(split_type::original_type_traits::size * splitIndex / splitCount))
					: ((split_type::original_value_type)n)
			)
		);
	}

	template <unsigned int splitCount, unsigned int splitIndex, typename Type>
	inline
	typename databit::detail::split_type<Type, splitCount>::value_type number_split(Type n)
	{
		typedef typename databit::detail::split_type<Type, splitCount> split_type;

		return (split_type::value_type)
		(
			((split_type::original_value_type)((split_type::value_type)-1)) /* mask*/ &
			(
				(split_type::original_type_traits::size * splitIndex / splitCount) ?
					(((split_type::original_value_type)n) >>
						(split_type::original_type_traits::size * splitIndex / splitCount))
					: ((split_type::original_value_type)n)
			)
		);
	}
}

#endif // DATA_BIT_HPP_INCLUCED

max_stars_repo_name: shawwwn/YDWE

max_issues_repo_name: shawwwn/YDWE

max_forks_repo_name: shawwwn/YDWE


text:
// Boost.Range library
//
//  Copyright Neil Groves 2009. Use, modification and
//  distribution is subject to 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)
//
//
// For more information, see http://www.boost.org/libs/range/
//
#include <boost/range/algorithm/fill.hpp>

#include <boost/test/test_tools.hpp>
#include <boost/test/unit_test.hpp>

#include <boost/assign.hpp>
#include <algorithm>
#include <functional>
#include <list>
#include <numeric>
#include <deque>
#include <vector>

namespace boost
{
    namespace
    {
        template< class Container >
        void test_fill_impl(Container& cont)
        {
            Container reference(cont);
            std::fill(reference.begin(), reference.end(), 1);

            Container target(cont);
            boost::fill(target, 1);

            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                target.begin(), target.end() );

            Container target2(cont);
            boost::fill(boost::make_iterator_range(target2), 1);

            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                                           target2.begin(), target2.end() );
        }

        template< class Container >
        void test_fill_impl()
        {
            using namespace boost::assign;

            Container cont;
            test_fill_impl(cont);

            cont.clear();
            cont += 2;
            test_fill_impl(cont);

            cont.clear();
            cont += 1,2;
            test_fill_impl(cont);

            cont.clear();
            cont += 1,2,3,4,5,6,7,8,9;
            test_fill_impl(cont);
        }

        void test_fill()
        {
            test_fill_impl< std::vector<int> >();
            test_fill_impl< std::list<int> >();
            test_fill_impl< std::deque<int> >();
        }
    }
}

boost::unit_test::test_suite*
init_unit_test_suite(int argc, char* argv[])
{
    boost::unit_test::test_suite* test
        = BOOST_TEST_SUITE( "RangeTestSuite.algorithm.fill" );

    test->add( BOOST_TEST_CASE( &boost::test_fill ) );

    return test;
}

max_stars_repo_name: randolphwong/mcsema

max_issues_repo_name: randolphwong/mcsema

max_forks_repo_name: randolphwong/mcsema


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

#include <sstream>
#include <boost/filesystem.hpp>

#include "gazebo/common/KeyEvent.hh"
#include "gazebo/common/Exception.hh"

#include "gazebo/rendering/UserCamera.hh"
#include "gazebo/rendering/Visual.hh"
#include "gazebo/rendering/Scene.hh"

#include "gazebo/math/Quaternion.hh"

#include "gazebo/transport/Publisher.hh"
#include "gazebo/transport/Node.hh"

#include "gazebo/physics/Inertial.hh"

#include "gazebo/gui/Actions.hh"
#include "gazebo/gui/KeyEventHandler.hh"
#include "gazebo/gui/MouseEventHandler.hh"
#include "gazebo/gui/GuiEvents.hh"
#include "gazebo/gui/GuiIface.hh"
#include "gazebo/gui/ModelManipulator.hh"

#include "gazebo/gui/model/JointMaker.hh"
#include "gazebo/gui/model/ModelCreator.hh"

using namespace gazebo;
using namespace gui;

/////////////////////////////////////////////////
ModelCreator::ModelCreator()
{
  this->modelName = "";

  this->modelTemplateSDF.reset(new sdf::SDF);
  this->modelTemplateSDF->SetFromString(this->GetTemplateSDFString());

  this->boxCounter = 0;
  this->cylinderCounter = 0;
  this->sphereCounter = 0;
  this->modelCounter = 0;

  this->node = transport::NodePtr(new transport::Node());
  this->node->Init();
  this->makerPub = this->node->Advertise<msgs::Factory>("~/factory");
  this->requestPub = this->node->Advertise<msgs::Request>("~/request");

  this->jointMaker = new JointMaker();

  connect(g_deleteAct, SIGNAL(DeleteSignal(const std::string &)), this,
          SLOT(OnDelete(const std::string &)));

  this->Reset();
}

/////////////////////////////////////////////////
ModelCreator::~ModelCreator()
{
  this->Reset();
  this->node->Fini();
  this->node.reset();
  this->modelTemplateSDF.reset();
  this->requestPub.reset();
  this->makerPub.reset();

  delete jointMaker;
}

/////////////////////////////////////////////////
std::string ModelCreator::CreateModel()
{
  this->Reset();
  return this->modelName;
}

/////////////////////////////////////////////////
void ModelCreator::AddJoint(JointMaker::JointType _type)
{
  this->Stop();
  if (this->jointMaker)
    this->jointMaker->CreateJoint(_type);
}

/////////////////////////////////////////////////
std::string ModelCreator::AddBox(const math::Vector3 &_size,
    const math::Pose &_pose)
{
  if (!this->modelVisual)
  {
    this->Reset();
  }

  std::ostringstream linkNameStream;
  linkNameStream << "unit_box_" << this->boxCounter++;
  std::string linkName = linkNameStream.str();

  rendering::VisualPtr linkVisual(new rendering::Visual(linkName,
      this->modelVisual));
  linkVisual->Load();

  std::ostringstream visualName;
  visualName << linkName << "_visual";
  rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
      linkVisual));
  sdf::ElementPtr visualElem =  this->modelTemplateSDF->root
      ->GetElement("model")->GetElement("link")->GetElement("visual");
  visualElem->GetElement("material")->GetElement("script")
      ->GetElement("name")->Set("Gazebo/GreyTransparent");

  sdf::ElementPtr geomElem =  visualElem->GetElement("geometry");
  geomElem->ClearElements();
  ((geomElem->AddElement("box"))->AddElement("size"))->Set(_size);

  visVisual->Load(visualElem);

  linkVisual->SetPose(_pose);
  if (_pose == math::Pose::Zero)
  {
    linkVisual->SetPosition(math::Vector3(_pose.pos.x, _pose.pos.y,
    _pose.pos.z + _size.z/2));
  }

  this->CreatePart(visVisual);

  this->mouseVisual = linkVisual;

  return linkName;
}

/////////////////////////////////////////////////
std::string ModelCreator::AddSphere(double _radius,
    const math::Pose &_pose)
{
  if (!this->modelVisual)
    this->Reset();

  std::ostringstream linkNameStream;
  linkNameStream << "unit_sphere_" << this->sphereCounter++;
  std::string linkName = linkNameStream.str();

  rendering::VisualPtr linkVisual(new rendering::Visual(
      linkName, this->modelVisual));
  linkVisual->Load();

  std::ostringstream visualName;
  visualName << linkName << "_visual";
  rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
        linkVisual));
  sdf::ElementPtr visualElem =  this->modelTemplateSDF->root
      ->GetElement("model")->GetElement("link")->GetElement("visual");
  visualElem->GetElement("material")->GetElement("script")
      ->GetElement("name")->Set("Gazebo/GreyTransparent");

  sdf::ElementPtr geomElem =  visualElem->GetElement("geometry");
  geomElem->ClearElements();
  ((geomElem->AddElement("sphere"))->GetElement("radius"))->Set(_radius);

  visVisual->Load(visualElem);

  linkVisual->SetPose(_pose);
  if (_pose == math::Pose::Zero)
  {
    linkVisual->SetPosition(math::Vector3(_pose.pos.x, _pose.pos.y,
    _pose.pos.z + _radius));
  }

  this->CreatePart(visVisual);
  this->mouseVisual = linkVisual;

  return linkName;
}

/////////////////////////////////////////////////
std::string ModelCreator::AddCylinder(double _radius, double _length,
    const math::Pose &_pose)
{
  if (!this->modelVisual)
    this->Reset();

  std::ostringstream linkNameStream;
  linkNameStream << "unit_cylinder_" << this->cylinderCounter++;
  std::string linkName = linkNameStream.str();

  rendering::VisualPtr linkVisual(new rendering::Visual(
      linkName, this->modelVisual));
  linkVisual->Load();

  std::ostringstream visualName;
  visualName << linkName << "_visual";
  rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
        linkVisual));
  sdf::ElementPtr visualElem =  this->modelTemplateSDF->root
      ->GetElement("model")->GetElement("link")->GetElement("visual");
  visualElem->GetElement("material")->GetElement("script")
      ->GetElement("name")->Set("Gazebo/GreyTransparent");

  sdf::ElementPtr geomElem =  visualElem->GetElement("geometry");
  geomElem->ClearElements();
  sdf::ElementPtr cylinderElem = geomElem->AddElement("cylinder");
  (cylinderElem->GetElement("radius"))->Set(_radius);
  (cylinderElem->GetElement("length"))->Set(_length);

  visVisual->Load(visualElem);

  linkVisual->SetPose(_pose);
  if (_pose == math::Pose::Zero)
  {
    linkVisual->SetPosition(math::Vector3(_pose.pos.x, _pose.pos.y,
    _pose.pos.z + _length/2));
  }

  this->CreatePart(visVisual);
  this->mouseVisual = linkVisual;

  return linkName;
}

/////////////////////////////////////////////////
std::string ModelCreator::AddCustom(const std::string &_path,
    const math::Vector3 &_scale, const math::Pose &_pose)
{
  if (!this->modelVisual)
    this->Reset();

  std::string path = _path;

  std::ostringstream linkNameStream;
  linkNameStream << "custom_" << this->customCounter++;
  std::string linkName = linkNameStream.str();

  rendering::VisualPtr linkVisual(new rendering::Visual(this->modelName + "::" +
        linkName, this->modelVisual));
  linkVisual->Load();

  std::ostringstream visualName;
  visualName << linkName << "_visual";
  rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
        linkVisual));
  sdf::ElementPtr visualElem =  this->modelTemplateSDF->root
      ->GetElement("model")->GetElement("link")->GetElement("visual");
  visualElem->GetElement("material")->GetElement("script")
      ->GetElement("name")->Set("Gazebo/GreyTransparent");

  sdf::ElementPtr geomElem =  visualElem->GetElement("geometry");
  geomElem->ClearElements();
  sdf::ElementPtr meshElem = geomElem->AddElement("mesh");
  meshElem->GetElement("scale")->Set(_scale);
  meshElem->GetElement("uri")->Set(path);
  visVisual->Load(visualElem);

  linkVisual->SetPose(_pose);
  if (_pose == math::Pose::Zero)
  {
    linkVisual->SetPosition(math::Vector3(_pose.pos.x, _pose.pos.y,
    _pose.pos.z + _scale.z/2));
  }

  this->CreatePart(visVisual);
  this->mouseVisual = linkVisual;

  return linkName;
}

/////////////////////////////////////////////////
void ModelCreator::CreatePart(const rendering::VisualPtr &_visual)
{
  PartData *part = new PartData;
  part->name = _visual->GetName();
  part->visuals.push_back(_visual);

  part->gravity = true;
  part->selfCollide = false;
  part->kinematic = false;

  part->inertial = new physics::Inertial;
  part->sensorData = new SensorData;

  this->allParts[part->name] = part;
}

/////////////////////////////////////////////////
void ModelCreator::RemovePart(const std::string &_partName)
{
  if (!this->modelVisual)
  {
    this->Reset();
    return;
  }

  if (this->allParts.find(_partName) == this->allParts.end())
    return;

  PartData *part = this->allParts[_partName];
  if (!part)
    return;

  for (unsigned int i = 0; i < part->visuals.size(); ++i)
  {
    rendering::VisualPtr vis = part->visuals[i];
    rendering::VisualPtr visParent = vis->GetParent();
    rendering::ScenePtr scene = vis->GetScene();
    scene->RemoveVisual(vis);
    if (visParent)
      scene->RemoveVisual(visParent);
  }

  delete part->inertial;
  delete part->sensorData;

  this->allParts.erase(_partName);
}

/////////////////////////////////////////////////
void ModelCreator::Reset()
{
  if (!gui::get_active_camera() ||
      !gui::get_active_camera()->GetScene())
    return;

  KeyEventHandler::Instance()->AddPressFilter("model_part",
      boost::bind(&ModelCreator::OnKeyPressPart, this, _1));

  MouseEventHandler::Instance()->AddReleaseFilter("model_part",
      boost::bind(&ModelCreator::OnMouseReleasePart, this, _1));

  MouseEventHandler::Instance()->AddMoveFilter("model_part",
      boost::bind(&ModelCreator::OnMouseMovePart, this, _1));

  MouseEventHandler::Instance()->AddDoubleClickFilter("model_part",
      boost::bind(&ModelCreator::OnMouseDoubleClickPart, this, _1));

  this->jointMaker->Reset();
  this->selectedVis.reset();

  std::stringstream ss;
  ss << "defaultModel_" << this->modelCounter++;
  this->modelName = ss.str();

  rendering::ScenePtr scene = gui::get_active_camera()->GetScene();

  this->isStatic = false;
  this->autoDisable = true;

  while (this->allParts.size() > 0)
    this->RemovePart(this->allParts.begin()->first);
  this->allParts.clear();

  if (this->modelVisual)
    scene->RemoveVisual(this->modelVisual);

  this->modelVisual.reset(new rendering::Visual(this->modelName,
      scene->GetWorldVisual()));

  this->modelVisual->Load();
  this->modelPose = math::Pose::Zero;
  this->modelVisual->SetPose(this->modelPose);
  scene->AddVisual(this->modelVisual);
}

/////////////////////////////////////////////////
void ModelCreator::SetModelName(const std::string &_modelName)
{
  this->modelName = _modelName;
}

/////////////////////////////////////////////////
std::string ModelCreator::GetModelName() const
{
  return this->modelName;
}

/////////////////////////////////////////////////
void ModelCreator::SetStatic(bool _static)
{
  this->isStatic = _static;
}

/////////////////////////////////////////////////
void ModelCreator::SetAutoDisable(bool _auto)
{
  this->autoDisable = _auto;
}

/////////////////////////////////////////////////
void ModelCreator::SaveToSDF(const std::string &_savePath)
{
  std::ofstream savefile;
  boost::filesystem::path path;
  path = boost::filesystem::operator/(_savePath, this->modelName + ".sdf");
  savefile.open(path.string().c_str());
  if (savefile.is_open())
  {
    savefile << this->modelSDF->ToString();
    savefile.close();
  }
  else
  {
    gzerr << "Unable to open file for writing: '" << path.string().c_str()
        << "'. Possibly a permission issue." << std::endl;
  }
}

/////////////////////////////////////////////////
void ModelCreator::FinishModel()
{
  event::Events::setSelectedEntity("", "normal");
  this->Reset();
  this->CreateTheEntity();
}

/////////////////////////////////////////////////
void ModelCreator::CreateTheEntity()
{
  msgs::Factory msg;
  msg.set_sdf(this->modelSDF->ToString());
  this->makerPub->Publish(msg);
}

/////////////////////////////////////////////////
std::string ModelCreator::GetTemplateSDFString()
{
  std::ostringstream newModelStr;
  newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
    << "<model name='template_model'>"
    << "<pose>0 0 0.0 0 0 0</pose>"
    << "<link name ='link'>"
    <<   "<visual name ='visual'>"
    <<     "<pose>0 0 0.0 0 0 0</pose>"
    <<     "<geometry>"
    <<       "<box>"
    <<         "<size>1.0 1.0 1.0</size>"
    <<       "</box>"
    <<     "</geometry>"
    <<     "<material>"
    <<       "<script>"
    <<         "<uri>file://media/materials/scripts/gazebo.material</uri>"
    <<         "<name>Gazebo/Grey</name>"
    <<       "</script>"
    <<     "</material>"
    <<   "</visual>"
    << "</link>"
    << "<static>true</static>"
    << "</model>"
    << "</sdf>";

  return newModelStr.str();
}

/////////////////////////////////////////////////
void ModelCreator::AddPart(PartType _type)
{
  this->Stop();

  this->addPartType = _type;
  if (_type != PART_NONE)
  {
    switch (_type)
    {
      case PART_BOX:
      {
        this->AddBox();
        break;
      }
      case PART_SPHERE:
      {
        this->AddSphere();
        break;
      }
      case PART_CYLINDER:
      {
        this->AddCylinder();
        break;
      }
      default:
      {
        gzwarn << "Unknown part type '" << _type << "'. " <<
            "Part not added" << std::endl;
        break;
      }
    }
  }
}

/////////////////////////////////////////////////
void ModelCreator::Stop()
{
  if (this->addPartType != PART_NONE && this->mouseVisual)
  {
    for (unsigned int i = 0; i < this->mouseVisual->GetChildCount(); ++i)
        this->RemovePart(this->mouseVisual->GetChild(0)->GetName());
    this->mouseVisual.reset();
  }
  if (this->jointMaker)
    this->jointMaker->Stop();
}

/////////////////////////////////////////////////
void ModelCreator::OnDelete(const std::string &_part)
{
  if (_part == this->modelName)
  {
    this->Reset();
    return;
  }

  if (this->jointMaker)
    this->jointMaker->RemoveJointsByPart(_part);

  this->RemovePart(_part);
}

/////////////////////////////////////////////////
bool ModelCreator::OnKeyPressPart(const common::KeyEvent &_event)
{
  if (_event.key == Qt::Key_Escape)
  {
    this->Stop();
  }
  else if (_event.key == Qt::Key_Delete)
  {
    if (this->selectedVis)
    {
      this->OnDelete(this->selectedVis->GetName());
      this->selectedVis.reset();
    }
  }
  return false;
}

/////////////////////////////////////////////////
bool ModelCreator::OnMouseReleasePart(const common::MouseEvent &_event)
{
  if (_event.button != common::MouseEvent::LEFT)
    return false;

  if (this->mouseVisual)
  {
    emit PartAdded();
    this->mouseVisual.reset();
    this->AddPart(PART_NONE);
    return true;
  }

  // In mouse normal mode, let users select a part if the parent model
  // is currently selected.
  rendering::VisualPtr vis = gui::get_active_camera()->GetVisual(_event.pos);
  if (vis)
  {
    if (this->allParts.find(vis->GetName()) !=
        this->allParts.end())
    {
      if (gui::get_active_camera()->GetScene()->GetSelectedVisual()
          == this->modelVisual || this->selectedVis)
      {
        if (this->selectedVis)
          this->selectedVis->SetHighlighted(false);
        else
          event::Events::setSelectedEntity("", "normal");

        this->selectedVis = vis;
        this->selectedVis->SetHighlighted(true);
        return true;
      }
    }
    else if (this->selectedVis)
    {
      this->selectedVis->SetHighlighted(false);
      this->selectedVis.reset();
    }
  }
  return false;
}

/////////////////////////////////////////////////
bool ModelCreator::OnMouseMovePart(const common::MouseEvent &_event)
{
  if (!this->mouseVisual)
    return false;

  if (!gui::get_active_camera())
    return false;

  math::Pose pose = this->mouseVisual->GetWorldPose();
  pose.pos = ModelManipulator::GetMousePositionOnPlane(
      gui::get_active_camera(), _event);

  if (!_event.shift)
  {
    pose.pos = ModelManipulator::SnapPoint(pose.pos);
  }
  pose.pos.z = this->mouseVisual->GetWorldPose().pos.z;

  this->mouseVisual->SetWorldPose(pose);

  return true;
}

/////////////////////////////////////////////////
bool ModelCreator::OnMouseDoubleClickPart(const common::MouseEvent &_event)
{
  rendering::VisualPtr vis = gui::get_active_camera()->GetVisual(_event.pos);
  if (vis)
  {
    if (this->allParts.find(vis->GetName()) != this->allParts.end())
    {
      // TODO part inspector code goes here
      return true;
    }
  }
  return false;
}

/////////////////////////////////////////////////
JointMaker *ModelCreator::GetJointMaker() const
{
  return this->jointMaker;
}

/////////////////////////////////////////////////
void ModelCreator::GenerateSDF()
{
  sdf::ElementPtr modelElem;
  sdf::ElementPtr linkElem;
  sdf::ElementPtr visualElem;
  sdf::ElementPtr collisionElem;

  this->modelSDF.reset(new sdf::SDF);
  this->modelSDF->SetFromString(this->GetTemplateSDFString());

  modelElem = this->modelSDF->root->GetElement("model");

  linkElem = modelElem->GetElement("link");
  sdf::ElementPtr templateLinkElem = linkElem->Clone();
  sdf::ElementPtr templateVisualElem = templateLinkElem->GetElement(
      "visual")->Clone();
  sdf::ElementPtr templateCollisionElem = templateLinkElem->GetElement(
      "collision")->Clone();
  modelElem->ClearElements();
  std::stringstream visualNameStream;
  std::stringstream collisionNameStream;

  modelElem->GetAttribute("name")->Set(this->modelName);

  boost::unordered_map<std::string, PartData *>::iterator partsIt;

  // set center of all parts to be origin
  math::Vector3 mid;
  for (partsIt = this->allParts.begin(); partsIt != this->allParts.end();
      ++partsIt)
  {
    PartData *part = partsIt->second;
    rendering::VisualPtr visual = part->visuals[0];
    mid += visual->GetParent()->GetWorldPose().pos;
  }
  mid /= this->allParts.size();
  this->origin.pos = mid;
  modelElem->GetElement("pose")->Set(this->origin);

  // loop through all parts and generate sdf
  for (partsIt = this->allParts.begin(); partsIt != this->allParts.end();
      ++partsIt)
  {
    visualNameStream.str("");
    collisionNameStream.str("");

    PartData *part = partsIt->second;
    rendering::VisualPtr visual = part->visuals[0];
    sdf::ElementPtr newLinkElem = templateLinkElem->Clone();
    visualElem = newLinkElem->GetElement("visual");
    collisionElem = newLinkElem->GetElement("collision");
    newLinkElem->GetAttribute("name")->Set(visual->GetParent()->GetName());
    newLinkElem->GetElement("pose")->Set(visual->GetParent()->GetWorldPose()
        - this->origin);
    newLinkElem->GetElement("gravity")->Set(part->gravity);
    newLinkElem->GetElement("self_collide")->Set(part->selfCollide);
    newLinkElem->GetElement("kinematic")->Set(part->kinematic);
    sdf::ElementPtr inertialElem = newLinkElem->GetElement("inertial");
    inertialElem->GetElement("mass")->Set(part->inertial->GetMass());
    inertialElem->GetElement("pose")->Set(part->inertial->GetPose());
    sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
    inertiaElem->GetElement("ixx")->Set(part->inertial->GetIXX());
    inertiaElem->GetElement("iyy")->Set(part->inertial->GetIYY());
    inertiaElem->GetElement("izz")->Set(part->inertial->GetIZZ());
    inertiaElem->GetElement("ixy")->Set(part->inertial->GetIXY());
    inertiaElem->GetElement("ixz")->Set(part->inertial->GetIXZ());
    inertiaElem->GetElement("iyz")->Set(part->inertial->GetIYZ());

    modelElem->InsertElement(newLinkElem);

    visualElem->GetAttribute("name")->Set(visual->GetParent()->GetName()
        + "_visual");
    collisionElem->GetAttribute("name")->Set(visual->GetParent()->GetName()
        + "_collision");
    visualElem->GetElement("pose")->Set(visual->GetPose());
    collisionElem->GetElement("pose")->Set(visual->GetPose());

    sdf::ElementPtr geomElem =  visualElem->GetElement("geometry");
    geomElem->ClearElements();

    math::Vector3 scale = visual->GetScale();
    if (visual->GetParent()->GetName().find("unit_box") != std::string::npos)
    {
      sdf::ElementPtr boxElem = geomElem->AddElement("box");
      (boxElem->GetElement("size"))->Set(scale);
    }
    else if (visual->GetParent()->GetName().find("unit_cylinder")
       != std::string::npos)
    {
      sdf::ElementPtr cylinderElem = geomElem->AddElement("cylinder");
      (cylinderElem->GetElement("radius"))->Set(scale.x/2.0);
      (cylinderElem->GetElement("length"))->Set(scale.z);
    }
    else if (visual->GetParent()->GetName().find("unit_sphere")
        != std::string::npos)
    {
      sdf::ElementPtr sphereElem = geomElem->AddElement("sphere");
      (sphereElem->GetElement("radius"))->Set(scale.x/2.0);
    }
    else if (visual->GetParent()->GetName().find("custom")
        != std::string::npos)
    {
      sdf::ElementPtr customElem = geomElem->AddElement("mesh");
      (customElem->GetElement("scale"))->Set(scale);
      (customElem->GetElement("uri"))->Set(visual->GetMeshName());
    }
    sdf::ElementPtr geomElemClone = geomElem->Clone();
    geomElem =  collisionElem->GetElement("geometry");
    geomElem->ClearElements();
    geomElem->InsertElement(geomElemClone->GetFirstElement());
  }

  // Add joint sdf elements
  this->jointMaker->GenerateSDF();
  sdf::ElementPtr jointsElem = this->jointMaker->GetSDF();

  sdf::ElementPtr jointElem;
  if (jointsElem->HasElement("joint"))
    jointElem = jointsElem->GetElement("joint");
  while (jointElem)
  {
    modelElem->InsertElement(jointElem->Clone());
    jointElem = jointElem->GetNextElement("joint");
  }

  // Model settings
  modelElem->GetElement("static")->Set(this->isStatic);
  modelElem->GetElement("allow_auto_disable")->Set(this->autoDisable);
}

max_stars_repo_name: hyunoklee/Gazebo

max_issues_repo_name: hyunoklee/Gazebo

max_forks_repo_name: hyunoklee/Gazebo


text:
/**
 * @file VO_tools.hpp
 * @brief This file is mainly to realize the visual odometer which will be used to judge the error between critical frames.
 */

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <boost/filesystem.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <eigen3/Eigen/Eigenvalues>
#include <opencv2/core/eigen.hpp>
#include <sys/types.h>
#include <sys/stat.h>
#include "../ba/BA_tools.hpp"
using namespace std;
using namespace cv;
/**
 *@brief The main structure for VO algorithm, which contains the parameters
  we will use in the VO algorithm.
 */
class VO
{
private:
    /**
    *@brief The step size for take critical frames.
    */
    int step_size;

    /**
    *@brief The path to save the final result.
    */
    string store_path;

    /**
    *@brief The path we get data from.
    */
    string data_path;

    /**
    *@brief The internal parameter matrix.
    */
    Mat K;

    /**
    *@brief The eigen form of internal parameter matrix k.
    */
    Eigen::VectorXd K_;

    /**
    *@brief The parameter waiting to be used.
    */
    vector<string> vector_of_files;

    vector<Mat> Ps;
public:
    /**
    * VO() The constuctor of class VO.
    * VO() Pass store_path_, data_path_, step_size_ to the corresponding private parameter.
    * @param[in] store_path_ The value to be pass to store_path
    * @param[in] data_path_ The value to be pass to data_path
    * @param[in] step_size_ The value to be pass to step_size
    */
    VO(string data_path_ = "", string store_path_ = "", int step_size_ = 10);
    /**
    * ~VO() The destuctor of class VO.
    */
    ~VO();
    /**
     * SetK() This function which is used to pass the parameters in to the internal parameter matrix.
     * @param[in] focal_x The position of focal on x-axis.
     * @param[in] focal_y The position of focal on y-axis.
     * @param[in] principal_x The position of camera center on x-axis.
     * @param[in] principal_y The position of camera center on y-axis.
     * @retval NULL void output
     */
    void SetK(double focal_x,double focal_y, int principal_x, int principal_y);
    /**
     * read_file_names() This function is used to extract the names of files in a specific directory.
       the internal parameter matrix k will need and send it to k.
     * @return Return the number of file in this directory.
     */
    int read_file_names();
     /**
     * solve() The main structure of VO algorithm.
     * @param[in] store_trajectory Whether we need to store the trajectory parameters into a specific file
     * @param[in] need_plot Whether we need to plot the result
     * @param[in] all_R The vector which contains the rotation information between two step
     * @param[in] all_t The vector which contains the translation information between two step
     * @retval 1 Algorithm runs successfully
     * @retval 0 Fails
     */
    void solve( std::vector<Eigen::Matrix3f>& all_R,
                std::vector<Eigen::Vector3f>& all_t,
                bool store_trajectory = false,
                bool need_plot = false);
    /**
     * solve_once() Runs the vo algorithm once.
     * @param[in] all_R The vector which contains the rotation information between two step
     * @param[in] all_t The vector which contains the translation information between two step
     * @param[in] pre_all_inliers1 The vector which contains previous all inliers of the first frame.
     * @param[in] pre_all_inliers2 The vector which contains previous all inliers of the second frame.

     * @param[in] all_2d_inliers The inliers waiting to be stored.
     * @param[in] all_indexes The mapping between all the matched frames.
     * @param[in] index_points THe mapping between keypoint and its corresponing index.
     * @param[in] index_descriptors The mapping between the index and their corresponding feature descriptors.
     * @param[in] f2d The SIFT feature extractor.
     * @param[in] index The index of the frame that will be process in the solve percedure.
     * @param[in] files The index of file.
     * @return void
     */
    void solve_once(std::vector<Eigen::Matrix3f>& all_R,
                        std::vector<Eigen::Vector3f>& all_t,
                        vector<Eigen::Vector2f>& pre_all_inliers1,
                        vector<Eigen::Vector2f>& pre_all_inliers2,
                        vector<std::vector<Eigen::Vector2f>>&  all_2d_inliers,
                        vector<std::map<int, int> >&  all_indexes,
                        map<int, vector<KeyPoint>>& index_points,
                        map<int, Mat>& index_descriptors,
                        Ptr<Feature2D>& f2d,
                        int& index,
                        int files);
    /**
     * add_points() Be used to add new point into corresponding inlier vectors.
     * @param[in] POINT_1 The inlier which waiting to push into inlier vector as the first frame.
     * @param[in] POINT_2 The inlier which waiting to push into inlier vector as the second frame.
     * @param[in] mask The indicator to determine whether the point should be added.
     * @param[in] all_inliers1 The vector which contains all inliers of the first frame.
     * @param[in] all_inliers2 The vector which contains all inliers of the second frame.
     * @return void
     */
    void add_points(vector<Point2d> & POINT_1,
                    vector<Point2d> & POINT_2,
                    Eigen::VectorXi & mask,
                    std::vector<Eigen::Vector2f> & all_inliers1,
                    std::vector<Eigen::Vector2f> & all_inliers2);
     /**
     * read_gt() This function reads the ground truth from specific file.
     * @return void
     */
    void read_gt(std::string file_path);
      /**
     * get_K_() Get the K of the class.
     * @return The private parameter K
     */
    Eigen::VectorXd get_K_();
};


text:
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Array3d v(0, sqrt(2.)/2, 1);
cout << v.acos() << endl;

  return 0;
}

max_issues_repo_name: TANHAIYU/planecalib

max_forks_repo_name: TANHAIYU/planecalib


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

#ifndef CHECKPOINT_HPP
#define CHECKPOINT_HPP

#include "component_event.hpp"
#include <map>
#include <string>
#include <vector>
#include <set>
#include <dlib/array.h>

class Checkpoint {
public:
  Checkpoint();
  Checkpoint(Checkpoint &aCheckpoint, std::set<std::string> *aFilter = NULL);
  ~Checkpoint();

  void addComponentEvent(ComponentEvent *aEvent);
  void copy(Checkpoint &aCheckpoint, std::set<std::string> *aFilter = NULL);
  void clear();
  void filter(std::set<std::string> &aFilter);

  std::map<std::string, ComponentEventPtr*> &getEvents() { return mEvents; }
  void getComponentEvents(ComponentEventPtrArray &list,
                          std::set<std::string> *aFilter = NULL);
  ComponentEventPtr *getEventPtr(std::string anId) { return mEvents[anId]; }

protected:
  std::map<std::string, ComponentEventPtr*> mEvents;
  std::set<std::string> mFilter;
  bool mHasFilter;
};

#endif

max_stars_repo_name: markovchainz/cppagent

max_issues_repo_name: markovchainz/cppagent

max_forks_repo_name: markovchainz/cppagent


text:
/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
 */
#ifndef vts_libs_vts_urltemplate_hpp_included_
#define vts_libs_vts_urltemplate_hpp_included_

#include <string>
#include <functional>
#include <iostream>
#include <sstream>

#include <boost/optional.hpp>

#include "utility/enum-io.hpp"

#include "vts-libs/vts/nodeinfo.hpp"
#include "vts-libs/vts/mapconfig.hpp"

namespace vtslibs { namespace vts {

/** Wraps VTS URL template.
 */
class UrlTemplate {
public:
    UrlTemplate() = default;
    UrlTemplate(const UrlTemplate&) = default;
    UrlTemplate& operator=(const UrlTemplate&) = default;

    UrlTemplate(const std::string &str) { parse(str); }

    void parse(const std::string &str);
    void clear() { tokens_.clear(); }
    bool empty() const { return tokens_.empty(); }

    struct Vars {
        vts::TileId tileId;
        vts::TileId localId;
        std::string srs;
        unsigned int subMesh;
        std::string rf;

        std::vector<std::string> params;

        Vars(const vts::TileId &tileId, unsigned int subMesh = 0)
            : tileId(tileId), localId(tileId), subMesh(subMesh)
        {}

        Vars(const vts::TileId &tileId, const vts::TileId &localId
             , unsigned int subMesh = 0)
            : tileId(tileId), localId(localId), subMesh(subMesh)
        {}

        Vars(const vts::TileId &tileId, const vts::TileId &localId
             , const std::string &srs, unsigned int subMesh = 0)
            : tileId(tileId), localId(localId), srs(srs), subMesh(subMesh)
        {}

        Vars() : subMesh() {}

        Vars addSubmesh(int subMesh) {
            Vars out(*this);
            out.subMesh = subMesh;
            return out;
        }
    };

    std::string operator()(const Vars &vars) const;

    void expand(std::ostream &os, const Vars &vars) const;

    void dump(std::ostream &os) const;

    enum Variable {
        lod, x, y, loclod, locx, locy, sub, srs, rf
    };

    typedef std::function<void(std::ostream &os
                               , const Vars &vars)> Expander;

    struct Token {

        std::string value;
        boost::optional<Variable> variable;
        Expander expander;

        void expand(std::ostream &os, const Vars &vars) const;
        void dump(std::ostream &os) const;

        Token(const std::string &value) : value(value) {}
        Token(const Variable &variable) : variable(variable) {}
        Token(const std::string &value, const Expander &expander)
            : value(value), expander(expander)
        {}

        typedef std::vector<Token> list;
    };

private:
    Token::list tokens_;
};

// inlines

UTILITY_GENERATE_ENUM_IO(UrlTemplate::Variable,
                         ((lod))
                         ((x))
                         ((y))
                         ((loclod))
                         ((locx))
                         ((locy))
                         ((sub))
                         ((srs))
                         ((rf))
)

inline std::ostream& operator<<(std::ostream &os, const UrlTemplate &t)
{
    t.dump(os);
    return os;
}

inline std::string UrlTemplate::operator()(const Vars &vars) const
{
    std::ostringstream os;
    expand(os, vars);
    return os.str();
}

} } // namespace vtslibs::vts

#endif // vts_libs_vts_urltemplate_hpp_included_

max_stars_repo_name: melowntech/vts-libs

max_issues_repo_name: HanochZhu/vts-browser-unity-plugin

max_forks_repo_name: HanochZhu/vts-browser-unity-plugin