text:
//
//  Copyright (c) 2002-2003
//  Toon Knapen, Kresimir Fresl, Joerg Walter
//
//  Permission to use, copy, modify, distribute and sell this software
//  and its documentation for any purpose is hereby granted without fee,
//  provided that the above copyright notice appear in all copies and
//  that both that copyright notice and this permission notice appear
//  in supporting documentation.  The authors make no representations
//  about the suitability of this software for any purpose.
//  It is provided "as is" without express or implied warranty.
//
//

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_SYMM_HERM_RAW_HPP
#define BOOST_NUMERIC_BINDINGS_TRAITS_SYMM_HERM_RAW_HPP

#include <boost/numeric/bindings/traits/matrix_raw.hpp>
#ifndef BOOST_UBLAS_HAVE_BINDINGS
#  include <boost/numeric/ublas/symmetric.hpp>
#  include <boost/numeric/ublas/hermitian.hpp>
#endif

namespace boost { namespace numeric { namespace bindings { namespace traits {

  namespace ublas = boost::numeric::ublas;

  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::symmetric_adaptor<M, F> &m) {
    return bindings::traits::leading_dimension (m.data());
  }

  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::hermitian_adaptor<M, F> &m) {
    return bindings::traits::leading_dimension (m.data());
  }

  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::symmetric_adaptor<M, F> &m) {
    return matrix_storage_size (m.data());
  }

  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::hermitian_adaptor<M, F> &m) {
    return matrix_storage_size (m.data());
  }

  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::symmetric_matrix<T,F1,F2,A> &m) {
    return (int) ((m.size1() * (m.size1() + 1)) / 2);
  }

  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::hermitian_matrix<T,F1,F2,A> &m) {
    return (int) ((m.size1() * (m.size1() + 1)) / 2);
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::symmetric_matrix<T,F1,F2,A>::const_pointer
  matrix_storage (const ublas::symmetric_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }
#endif
  // We need data_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::symmetric_matrix<T,F1,F2,A>::const_pointer
  matrix_storage_const (const ublas::symmetric_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::symmetric_matrix<T,F1,F2,A>::pointer
  matrix_storage (ublas::symmetric_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage (const ublas::symmetric_adaptor<M, F> &m) {
    return matrix_storage (m.data());
  }
#endif
  // We need data_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage_const (const ublas::symmetric_adaptor<M, F> &m) {
    return matrix_storage_const (m.data());
  }
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::pointer matrix_storage (ublas::symmetric_adaptor<M, F> &m) {
    return matrix_storage (m.data());
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::hermitian_matrix<T,F1,F2,A>::const_pointer
  matrix_storage (const ublas::hermitian_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }
#endif
  // We need data_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::hermitian_matrix<T,F1,F2,A>::const_pointer
  matrix_storage_const (const ublas::hermitian_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::hermitian_matrix<T,F1,F2,A>::pointer
  matrix_storage (ublas::hermitian_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage (const ublas::hermitian_adaptor<M, F> &m) {
    return matrix_storage (m.data());
  }
#endif
  // We need data_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage_const (const ublas::hermitian_adaptor<M, F> &m) {
    return matrix_storage_const (m.data());
  }
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::pointer matrix_storage (ublas::hermitian_adaptor<M, F> &m) {
    return matrix_storage (m.data());
  }

  namespace detail {

    inline char m_uplo_tag (ublas::upper_tag const&) { return 'U'; }
    inline char m_uplo_tag (ublas::lower_tag const&) { return 'L'; }

  }

  template <typename SymmM>
  inline
  char matrix_uplo_tag (SymmM&) {
      typedef typename SymmM::packed_category uplo_t;
      return detail::m_uplo_tag (uplo_t());
  }

}}}}

#endif


text:
<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/Ephemerides/synchronousRotationalEphemeris.h"
#include "Tudat/SimulationSetup/tudatSimulationHeader.h"

namespace tudat
{
namespace unit_tests
{

using namespace ephemerides;
using namespace simulation_setup;
using namespace input_output;

BOOST_AUTO_TEST_SUITE( test_synchronous_rotational_ephemeris )

BOOST_AUTO_TEST_CASE( test_SynchronousRotationModel )
{

    // Load Spice kernels
    tudat::spice_interface::loadStandardSpiceKernels( );
<<<<<<< HEAD
    tudat::spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "jup310_small.bsp" );
=======
>>>>>>> origin/master

    // Create relevant bodies
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Io" );
    bodyNames.push_back( "Europa" );
    bodyNames.push_back( "Ganymede" );
    bodyNames.push_back( "Sun" );
    bodyNames.push_back( "Jupiter" );

    std::vector< std::string > bodiesToTest;
    bodiesToTest.push_back( "Io" );
    bodiesToTest.push_back( "Europa" );
    bodiesToTest.push_back( "Ganymede" );

    // Create bodies needed in simulation
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodyNames );
    for( unsigned int i = 0; i < bodiesToTest.size( ); i++ )
    {
        bodySettings[ bodiesToTest.at( i ) ]->rotationModelSettings = std::make_shared< SynchronousRotationModelSettings >(
                    "Jupiter", "ECLIPJ2000", "IAU_" + bodiesToTest.at( i ) );
        bodySettings[ bodiesToTest.at( i ) ]->ephemerisSettings->resetFrameOrigin( "Jupiter" );
    }
    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Test rotation model when body is in propagation, and outside of propagation
    for( unsigned int areBodiesInPropagation = 0; areBodiesInPropagation < 2; areBodiesInPropagation++ )
    {
        setAreBodiesInPropagation( bodyMap, areBodiesInPropagation );

        // Test different bodies
        for( unsigned int i = 0; i < bodiesToTest.size( ); i++ )
        {
            // Get synchronous rotation model and ephemeris
            std::shared_ptr< SynchronousRotationalEphemeris > currentRotationalEphemeris =
                    std::dynamic_pointer_cast< SynchronousRotationalEphemeris >(
                        bodyMap.at( bodiesToTest.at( i ) )->getRotationalEphemeris( ) );
            std::shared_ptr< Ephemeris > currentEphemeris = bodyMap.at( bodiesToTest.at( i ) )->getEphemeris( );

            // Test for various times (and anomalies)
            for( int j = 0; j < 100; j ++ )
            {
                // Determine current test time
                double testTime = 86400.0 * static_cast< double >( j );

                // Compute current satellite state
                Eigen::Vector6d currentSatelliteState;
                if( !areBodiesInPropagation )
                {
                    currentSatelliteState = currentEphemeris->getCartesianState( testTime );
                }
                else
                {
                    bodyMap[ bodiesToTest.at( i ) ]->setStateFromEphemeris( testTime );
                    bodyMap[ "Jupiter" ]->setStateFromEphemeris( testTime );
                    currentSatelliteState = bodyMap[ bodiesToTest.at( i ) ]->getState( ) -
                             bodyMap[ "Jupiter" ]->getState(  );
                }

                // Retrieve rotation matrix
                Eigen::Matrix3d currentRotationToBodyFixedFrame =
                        currentRotationalEphemeris->getRotationToTargetFrame( testTime ).toRotationMatrix( );

                // Transform r, v and r x v vectors to body-fixed frame
                Eigen::Vector3d bodyFixedRadialVector =
                        currentRotationToBodyFixedFrame * currentSatelliteState.segment( 0, 3 ).normalized( );
                Eigen::Vector3d bodyFixedVelocityVector =
                        currentRotationToBodyFixedFrame * currentSatelliteState.segment( 3, 3 ).normalized( );
                Eigen::Vector3d bodyFixedOrbitalAngularMomentumVelocityVector =
                        currentRotationToBodyFixedFrame *
                        ( Eigen::Vector3d( currentSatelliteState.segment( 0, 3 ) ).cross(
                              Eigen::Vector3d( currentSatelliteState.segment( 3, 3 ) ) ) ).normalized( );

                // Test vector magnitudes
                BOOST_CHECK_SMALL( std::fabs( bodyFixedRadialVector( 0 ) + 1.0 ), 10.0 * std::numeric_limits< double >::epsilon( ) );
                BOOST_CHECK_SMALL( std::fabs( bodyFixedRadialVector( 1 ) ), 10.0 * std::numeric_limits< double >::epsilon( ) );
                BOOST_CHECK_SMALL( std::fabs( bodyFixedRadialVector( 2 ) ), 10.0 * std::numeric_limits< double >::epsilon( ) );

                BOOST_CHECK_SMALL( std::fabs( bodyFixedVelocityVector( 0 ) ), 0.01 );
                BOOST_CHECK_SMALL( std::fabs( bodyFixedVelocityVector( 1 ) + 1.0 ), 0.01 );
                BOOST_CHECK_SMALL( std::fabs( bodyFixedVelocityVector( 2 ) ), 10.0 * std::numeric_limits< double >::epsilon( ) );

                BOOST_CHECK_SMALL( std::fabs( bodyFixedOrbitalAngularMomentumVelocityVector( 0 ) ), 10.0 * std::numeric_limits< double >::epsilon( ) );
                BOOST_CHECK_SMALL( std::fabs( bodyFixedOrbitalAngularMomentumVelocityVector( 1 ) ), 10.0 * std::numeric_limits< double >::epsilon( ) );
                BOOST_CHECK_SMALL( std::fabs( bodyFixedOrbitalAngularMomentumVelocityVector( 2 ) - 1.0 ), 10.0 * std::numeric_limits< double >::epsilon( ) );
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat

ext: cpp

max_stars_repo_name: ViktorJordanov/tudat

max_issues_repo_name: ViktorJordanov/tudat

max_forks_repo_name: ViktorJordanov/tudat


text:
// Licensed to the Apache Software Foundation (ASF) under one
// or more contributor license agreements.  See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  The ASF licenses this file
// to you 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.
//
// The following only applies to changes made to this file as part of YugaByte development.
//
// Portions Copyright (c) YugaByte, 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.
//

#include "yb/rpc/service_pool.h"

#include <memory>
#include <queue>
#include <string>
#include <vector>

#include <boost/asio/strand.hpp>

#include <cds/container/basket_queue.h>
#include <cds/gc/dhp.h>

#include <glog/logging.h>

#include "yb/gutil/ref_counted.h"

#include "yb/rpc/inbound_call.h"
#include "yb/rpc/scheduler.h"
#include "yb/rpc/service_if.h"

#include "yb/gutil/strings/substitute.h"
#include "yb/util/flag_tags.h"
#include "yb/util/lockfree.h"
#include "yb/util/logging.h"
#include "yb/util/metrics.h"
#include "yb/util/scope_exit.h"
#include "yb/util/status.h"
#include "yb/util/thread.h"
#include "yb/util/trace.h"

using namespace std::literals;
using namespace std::placeholders;
using std::shared_ptr;
using strings::Substitute;

DEFINE_int64(max_time_in_queue_ms, 6000,
             "Fail calls that get stuck in the queue longer than the specified amount of time "
                 "(in ms)");
TAG_FLAG(max_time_in_queue_ms, advanced);
TAG_FLAG(max_time_in_queue_ms, runtime);
DEFINE_int64(backpressure_recovery_period_ms, 600000,
             "Once we hit a backpressure/service-overflow we will consider dropping stale requests "
             "for this duration (in ms)");
TAG_FLAG(backpressure_recovery_period_ms, advanced);
TAG_FLAG(backpressure_recovery_period_ms, runtime);
DEFINE_test_flag(bool, enable_backpressure_mode_for_testing, false,
            "For testing purposes. Enables the rpc's to be considered timed out in the queue even "
            "when we have not had any backpressure in the recent past.");

METRIC_DEFINE_coarse_histogram(server, rpc_incoming_queue_time,
                        "RPC Queue Time",
                        yb::MetricUnit::kMicroseconds,
                        "Number of microseconds incoming RPC requests spend in the worker queue");

METRIC_DEFINE_counter(server, rpcs_timed_out_in_queue,
                      "RPC Queue Timeouts",
                      yb::MetricUnit::kRequests,
                      "Number of RPCs whose timeout elapsed while waiting "
                      "in the service queue, and thus were not processed. "
                      "Does not include calls that were expired before we tried to execute them.");

METRIC_DEFINE_counter(server, rpcs_timed_out_early_in_queue,
                      "RPC Queue Timeouts",
                      yb::MetricUnit::kRequests,
                      "Number of RPCs whose timeout elapsed while waiting "
                      "in the service queue, and thus were not processed. "
                      "Timeout for those calls were detected before the calls tried to execute.");

METRIC_DEFINE_counter(server, rpcs_queue_overflow,
                      "RPC Queue Overflows",
                      yb::MetricUnit::kRequests,
                      "Number of RPCs dropped because the service queue "
                      "was full.");

namespace yb {
namespace rpc {

namespace {

const CoarseDuration kTimeoutCheckGranularity = 100ms;
const char* const kTimedOutInQueue = "Call waited in the queue past deadline";

} // namespace

class ServicePoolImpl final : public InboundCallHandler {
 public:
  ServicePoolImpl(size_t max_tasks,
                  ThreadPool* thread_pool,
                  Scheduler* scheduler,
                  ServiceIfPtr service,
                  const scoped_refptr<MetricEntity>& entity)
      : max_queued_calls_(max_tasks),
        thread_pool_(*thread_pool),
        scheduler_(*scheduler),
        service_(std::move(service)),
        incoming_queue_time_(METRIC_rpc_incoming_queue_time.Instantiate(entity)),
        rpcs_timed_out_in_queue_(METRIC_rpcs_timed_out_in_queue.Instantiate(entity)),
        rpcs_timed_out_early_in_queue_(
            METRIC_rpcs_timed_out_early_in_queue.Instantiate(entity)),
        rpcs_queue_overflow_(METRIC_rpcs_queue_overflow.Instantiate(entity)),
        check_timeout_strand_(scheduler->io_service()),
        log_prefix_(Format("$0: ", service_->service_name())) {

          // Create per service counter for rpcs_in_queue_.
          auto id = Format("rpcs_in_queue_$0", service_->service_name());
          EscapeMetricNameForPrometheus(&id);
          string description = id + " metric for ServicePoolImpl";
          rpcs_in_queue_ = entity->FindOrCreateGauge(
              std::unique_ptr<GaugePrototype<int64_t>>(new OwningGaugePrototype<int64_t>(
                  entity->prototype().name(), std::move(id),
                  description, MetricUnit::kRequests, description, MetricLevel::kInfo)),
              static_cast<int64>(0) /* initial_value */);

          LOG_WITH_PREFIX(INFO) << "yb::rpc::ServicePoolImpl created at " << this;
  }

  ~ServicePoolImpl() {
    StartShutdown();
    CompleteShutdown();
  }

  void CompleteShutdown() {
    shutdown_complete_latch_.Wait();
    while (scheduled_tasks_.load(std::memory_order_acquire) != 0) {
      std::this_thread::sleep_for(10ms);
    }
  }

  void StartShutdown() {
    bool closing_state = false;
    if (closing_.compare_exchange_strong(closing_state, true)) {
      service_->Shutdown();

      auto check_timeout_task = check_timeout_task_.load(std::memory_order_acquire);
      if (check_timeout_task != kUninitializedScheduledTaskId) {
        scheduler_.Abort(check_timeout_task);
      }

      check_timeout_strand_.dispatch([this] {
        std::weak_ptr<InboundCall> inbound_call_wrapper;
        while (pre_check_timeout_queue_.pop(inbound_call_wrapper)) {}
        shutdown_complete_latch_.CountDown();
      });
    }
  }

  void Enqueue(const InboundCallPtr& call) {
    TRACE_TO(call->trace(), "Inserting onto call queue");

    auto task = call->BindTask(this);
    if (!task) {
      Overflow(call, "service", queued_calls_.load(std::memory_order_relaxed));
      return;
    }

    auto call_deadline = call->GetClientDeadline();
    if (call_deadline != CoarseTimePoint::max()) {
      pre_check_timeout_queue_.push(call);
      ScheduleCheckTimeout(call_deadline);
    }

    thread_pool_.Enqueue(task);
  }

  const Counter* RpcsTimedOutInQueueMetricForTests() const {
    return rpcs_timed_out_early_in_queue_.get();
  }

  const Counter* RpcsQueueOverflowMetric() const {
    return rpcs_queue_overflow_.get();
  }

  std::string service_name() const {
    return service_->service_name();
  }

  ServiceIfPtr TEST_get_service() const {
    return service_;
  }

  void Overflow(const InboundCallPtr& call, const char* type, size_t limit) {
    const auto err_msg =
        Format("$0 request on $1 from $2 dropped due to backpressure. "
                   "The $3 queue is full, it has $4 items.",
            call->method_name().ToBuffer(),
            service_->service_name(),
            call->remote_address(),
            type,
            limit);
    YB_LOG_EVERY_N_SECS(WARNING, 3) << LogPrefix() << err_msg;
    const auto response_status = STATUS(ServiceUnavailable, err_msg);
    rpcs_queue_overflow_->Increment();
    call->RespondFailure(ErrorStatusPB::ERROR_SERVER_TOO_BUSY, response_status);
    last_backpressure_at_.store(
        CoarseMonoClock::Now().time_since_epoch(), std::memory_order_release);
  }

  void Failure(const InboundCallPtr& call, const Status& status) override {
    if (!call->TryStartProcessing()) {
      return;
    }

    if (status.IsServiceUnavailable()) {
      Overflow(call, "global", thread_pool_.options().queue_limit);
      return;
    }
    YB_LOG_EVERY_N_SECS(WARNING, 1)
        << LogPrefix()
        << call->method_name() << " request on " << service_->service_name() << " from "
        << call->remote_address() << " dropped because of: " << status.ToString();
    const auto response_status = STATUS(ServiceUnavailable, "Service is shutting down");
    call->RespondFailure(ErrorStatusPB::FATAL_SERVER_SHUTTING_DOWN, response_status);
  }

  void FillEndpoints(const RpcServicePtr& service, RpcEndpointMap* map) {
    service_->FillEndpoints(service, map);
  }

  void Handle(InboundCallPtr incoming) override {
    incoming->RecordHandlingStarted(incoming_queue_time_);
    ADOPT_TRACE(incoming->trace());

    const char* error_message;
    if (PREDICT_FALSE(incoming->ClientTimedOut())) {
      error_message = kTimedOutInQueue;
    } else if (PREDICT_FALSE(ShouldDropRequestDuringHighLoad(incoming))) {
      error_message = "The server is overloaded. Call waited in the queue past max_time_in_queue.";
    } else {
      TRACE_TO(incoming->trace(), "Handling call $0", yb::ToString(incoming->method_name()));

      if (incoming->TryStartProcessing()) {
        service_->Handle(std::move(incoming));
      }
      return;
    }

    TRACE_TO(incoming->trace(), error_message);
    VLOG_WITH_PREFIX(4)
        << "Timing out call " << incoming->ToString() << " due to: " << error_message;

    // Respond as a failure, even though the client will probably ignore
    // the response anyway.
    TimedOut(incoming.get(), error_message, rpcs_timed_out_in_queue_.get());
  }

 private:
  void TimedOut(InboundCall* call, const char* error_message, Counter* metric) {
    if (call->RespondTimedOutIfPending(error_message)) {
      metric->Increment();
    }
  }

  bool ShouldDropRequestDuringHighLoad(const InboundCallPtr& incoming) {
    CoarseTimePoint last_backpressure_at(last_backpressure_at_.load(std::memory_order_acquire));

    // For testing purposes.
    if (GetAtomicFlag(&FLAGS_TEST_enable_backpressure_mode_for_testing)) {
      last_backpressure_at = CoarseMonoClock::Now();
    }

    // Test for a sentinel value, to avoid reading the clock.
    if (last_backpressure_at == CoarseTimePoint()) {
      return false;
    }

    auto now = CoarseMonoClock::Now();
    if (now > last_backpressure_at + FLAGS_backpressure_recovery_period_ms * 1ms) {
      last_backpressure_at_.store(CoarseTimePoint().time_since_epoch(), std::memory_order_release);
      return false;
    }

    return incoming->GetTimeInQueue().ToMilliseconds() > FLAGS_max_time_in_queue_ms;
  }

  void CheckTimeout(ScheduledTaskId task_id, CoarseTimePoint time, const Status& status) {
    auto se = ScopeExit([this, task_id, time] {
      auto expected_duration = time.time_since_epoch();
      next_check_timeout_.compare_exchange_strong(
          expected_duration, CoarseTimePoint::max().time_since_epoch(),
          std::memory_order_acq_rel);
      auto expected_task_id = task_id;
      check_timeout_task_.compare_exchange_strong(
          expected_task_id, kUninitializedScheduledTaskId, std::memory_order_acq_rel);
      scheduled_tasks_.fetch_sub(1, std::memory_order_acq_rel);
    });
    if (!status.ok()) {
      return;
    }

    auto now = CoarseMonoClock::now();
    {
      std::weak_ptr<InboundCall> weak_inbound_call;
      while (pre_check_timeout_queue_.pop(weak_inbound_call)) {
        auto inbound_call = weak_inbound_call.lock();
        if (!inbound_call) {
          continue;
        }
        if (now > inbound_call->GetClientDeadline()) {
          TimedOut(inbound_call.get(), kTimedOutInQueue, rpcs_timed_out_early_in_queue_.get());
        } else {
          check_timeout_queue_.emplace(inbound_call);
        }
      }
    }

    while (!check_timeout_queue_.empty() && now > check_timeout_queue_.top().time) {
      auto call = check_timeout_queue_.top().call.lock();
      if (call) {
        TimedOut(call.get(), kTimedOutInQueue, rpcs_timed_out_early_in_queue_.get());
      }
      check_timeout_queue_.pop();
    }

    if (!check_timeout_queue_.empty()) {
      ScheduleCheckTimeout(check_timeout_queue_.top().time);
    }
  }

  void ScheduleCheckTimeout(CoarseTimePoint time) {
    if (closing_.load(std::memory_order_acquire)) {
      return;
    }
    CoarseDuration next_check_timeout = next_check_timeout_.load(std::memory_order_acquire);
    time += kTimeoutCheckGranularity;
    while (CoarseTimePoint(next_check_timeout) > time) {
      if (next_check_timeout_.compare_exchange_weak(
              next_check_timeout, time.time_since_epoch(), std::memory_order_acq_rel)) {
        check_timeout_strand_.dispatch([this, time] {
          auto check_timeout_task = check_timeout_task_.load(std::memory_order_acquire);
          if (check_timeout_task != kUninitializedScheduledTaskId) {
            scheduler_.Abort(check_timeout_task);
          }
          scheduled_tasks_.fetch_add(1, std::memory_order_acq_rel);
          auto task_id = scheduler_.Schedule(
              [this, time](ScheduledTaskId task_id, const Status& status) {
                check_timeout_strand_.dispatch([this, time, task_id, status] {
                  CheckTimeout(task_id, time, status);
                });
              },
              ToSteady(time));
          check_timeout_task_.store(task_id, std::memory_order_release);
        });
        break;
      }
    }
  }

  const std::string& LogPrefix() const {
    return log_prefix_;
  }

  bool CallQueued() override {
    auto queued_calls = queued_calls_.fetch_add(1, std::memory_order_acq_rel);
    if (queued_calls < 0) {
      YB_LOG_EVERY_N_SECS(DFATAL, 5) << "Negative number of queued calls: " << queued_calls;
    }

    if (queued_calls >= max_queued_calls_) {
      queued_calls_.fetch_sub(1, std::memory_order_relaxed);
      return false;
    }

    rpcs_in_queue_->Increment();
    return true;
  }

  void CallDequeued() override {
    queued_calls_.fetch_sub(1, std::memory_order_relaxed);
    rpcs_in_queue_->Decrement();
  }

  const size_t max_queued_calls_;
  ThreadPool& thread_pool_;
  Scheduler& scheduler_;
  ServiceIfPtr service_;
  scoped_refptr<Histogram> incoming_queue_time_;
  scoped_refptr<Counter> rpcs_timed_out_in_queue_;
  scoped_refptr<Counter> rpcs_timed_out_early_in_queue_;
  scoped_refptr<Counter> rpcs_queue_overflow_;
  scoped_refptr<AtomicGauge<int64_t>> rpcs_in_queue_;
  // Have to use CoarseDuration here, since CoarseTimePoint does not work with clang + libstdc++
  std::atomic<CoarseDuration> last_backpressure_at_{CoarseTimePoint().time_since_epoch()};
  std::atomic<int64_t> queued_calls_{0};

  // It is too expensive to update timeout priority queue when each call is received.
  // So we are doing the following trick.
  // All calls are added to pre_check_timeout_queue_, w/o priority.
  // Then before timeout check we move calls from this queue to priority queue.
  typedef cds::container::BasketQueue<cds::gc::DHP, std::weak_ptr<InboundCall>>
      PreCheckTimeoutQueue;
  PreCheckTimeoutQueue pre_check_timeout_queue_;

  // Used to track scheduled time, to avoid unnecessary rescheduling.
  std::atomic<CoarseDuration> next_check_timeout_{CoarseTimePoint::max().time_since_epoch()};

  // Last scheduled task, required to abort scheduled task during reschedule.
  std::atomic<ScheduledTaskId> check_timeout_task_{kUninitializedScheduledTaskId};

  std::atomic<int> scheduled_tasks_{0};

  // Timeout checking synchronization.
  IoService::strand check_timeout_strand_;

  struct QueuedCheckDeadline {
    CoarseTimePoint time;
    // We use weak pointer to avoid retaining call that was already processed.
    std::weak_ptr<InboundCall> call;

    explicit QueuedCheckDeadline(const InboundCallPtr& inp)
        : time(inp->GetClientDeadline()), call(inp) {
    }
  };

  // Priority queue puts the geatest value on top, so we invert comparison.
  friend bool operator<(const QueuedCheckDeadline& lhs, const QueuedCheckDeadline& rhs) {
    return lhs.time > rhs.time;
  }

  std::priority_queue<QueuedCheckDeadline> check_timeout_queue_;

  std::atomic<bool> closing_ = {false};
  CountDownLatch shutdown_complete_latch_{1};
  std::string log_prefix_;
};

ServicePool::ServicePool(size_t max_tasks,
                         ThreadPool* thread_pool,
                         Scheduler* scheduler,
                         ServiceIfPtr service,
                         const scoped_refptr<MetricEntity>& metric_entity)
    : impl_(new ServicePoolImpl(
        max_tasks, thread_pool, scheduler, std::move(service), metric_entity)) {
}

ServicePool::~ServicePool() {
}

void ServicePool::StartShutdown() {
  impl_->StartShutdown();
}

void ServicePool::CompleteShutdown() {
  impl_->CompleteShutdown();
}

void ServicePool::QueueInboundCall(InboundCallPtr call) {
  impl_->Enqueue(std::move(call));
}

void ServicePool::Handle(InboundCallPtr call) {
  impl_->Handle(std::move(call));
}

void ServicePool::FillEndpoints(RpcEndpointMap* map) {
  impl_->FillEndpoints(RpcServicePtr(this), map);
}

const Counter* ServicePool::RpcsTimedOutInQueueMetricForTests() const {
  return impl_->RpcsTimedOutInQueueMetricForTests();
}

const Counter* ServicePool::RpcsQueueOverflowMetric() const {
  return impl_->RpcsQueueOverflowMetric();
}

std::string ServicePool::service_name() const {
  return impl_->service_name();
}

ServiceIfPtr ServicePool::TEST_get_service() const {
  return impl_->TEST_get_service();
}

} // namespace rpc
} // namespace yb

ext: cc


text:
//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "armnnTfParser/ITfParser.hpp"
#include "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(TensorflowParser)

struct UnsupportedMaximumFixture
    : public armnnUtils::ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    UnsupportedMaximumFixture()
    {
        m_Prototext = R"(
            node {
                name: "graphInput"
                op: "Placeholder"
                attr {
                    key: "dtype"
                    value {
                        type: DT_FLOAT
                    }
                }
                attr {
                    key: "shape"
                    value {
                        shape {
                        }
                    }
                }
            }
            node {
                name: "Maximum"
                op: "Maximum"
                input: "graphInput"
                attr {
                    key: "dtype"
                    value {
                        type: DT_FLOAT
                    }
                }
            }
        )";
    }
};

BOOST_FIXTURE_TEST_CASE(UnsupportedMaximum, UnsupportedMaximumFixture)
{
    BOOST_CHECK_THROW(
        SetupSingleInputSingleOutput({ 1, 1 }, "graphInput", "Maximum"),
        armnn::ParseException);
}

struct SupportedMaximumFixture
    : public armnnUtils::ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    SupportedMaximumFixture(const std::string & maxInput0,
                            const std::string & maxInput1,
                            const std::string & mulInput0,
                            const std::string & mulInput1)
    {
        m_Prototext = R"(
            node {
                name: "graphInput"
                op: "Placeholder"
                attr {
                    key: "dtype"
                    value { type: DT_FLOAT }
                }
                attr {
                    key: "shape"
                    value { shape { } }
                }
            }
            node {
                name: "Alpha"
                op: "Const"
                attr {
                    key: "dtype"
                    value { type: DT_FLOAT }
                }
                attr {
                    key: "value"
                    value {
                        tensor {
                            dtype: DT_FLOAT
                            tensor_shape {
                                dim { size: 1 }
                            }
                            float_val: 0.1
                        }
                    }
                }
            }
            node {
                name: "Mul"
                op: "Mul"
                input: ")" + mulInput0 + R"("
                input: ")" + mulInput1 + R"("
                attr {
                    key: "T"
                    value { type: DT_FLOAT }
                }
            }
            node {
                name: "Maximum"
                op: "Maximum"
                input: ")" + maxInput0 + R"("
                input: ")" + maxInput1 + R"("
                attr {
                    key: "T"
                    value { type: DT_FLOAT }
                }
            }
        )";
        SetupSingleInputSingleOutput({ 1, 2 }, "graphInput", "Maximum");
    }
};

struct LeakyRelu_Max_MulAT_T_Fixture : public SupportedMaximumFixture
{
    LeakyRelu_Max_MulAT_T_Fixture()
    : SupportedMaximumFixture("Mul","graphInput","Alpha","graphInput") {}
};

BOOST_FIXTURE_TEST_CASE(LeakyRelu_Max_MulAT_T, LeakyRelu_Max_MulAT_T_Fixture)
{
    RunTest<2>(std::vector<float>({-5.0, 3.0}), {-0.5, 3.0});
}

struct LeakyRelu_Max_T_MulAT_Fixture : public SupportedMaximumFixture
{
    LeakyRelu_Max_T_MulAT_Fixture()
    : SupportedMaximumFixture("graphInput","Mul","Alpha","graphInput") {}
};

BOOST_FIXTURE_TEST_CASE(LeakyRelu_Max_T_MulAT, LeakyRelu_Max_T_MulAT_Fixture)
{
    RunTest<2>(std::vector<float>({-10.0, 3.0}), {-1.0, 3.0});
}

struct LeakyRelu_Max_MulTA_T_Fixture : public SupportedMaximumFixture
{
    LeakyRelu_Max_MulTA_T_Fixture()
    : SupportedMaximumFixture("Mul", "graphInput","graphInput","Alpha") {}
};

BOOST_FIXTURE_TEST_CASE(LeakyRelu_Max_MulTA_T, LeakyRelu_Max_MulTA_T_Fixture)
{
    RunTest<2>(std::vector<float>({-5.0, 3.0}), {-0.5, 3.0});
}

struct LeakyRelu_Max_T_MulTA_Fixture : public SupportedMaximumFixture
{
    LeakyRelu_Max_T_MulTA_Fixture()
    : SupportedMaximumFixture("graphInput", "Mul", "graphInput", "Alpha") {}
};

BOOST_FIXTURE_TEST_CASE(LeakyRelu_Max_T_MulTA, LeakyRelu_Max_T_MulTA_Fixture)
{
    RunTest<2>(std::vector<float>({-10.0, 13.0}), {-1.0, 13.0});
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: VinayKarnam/armnn

max_issues_repo_name: VinayKarnam/armnn

max_forks_repo_name: VinayKarnam/armnn


text:
#include <armadillo>
#include <boost/program_options.hpp>
#include <HSMM.hpp>
#include <iostream>
#include <json.hpp>
#include <memory>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace std;
using json = nlohmann::json;
namespace po = boost::program_options;

mat fieldToMat(int njoints, field<mat> &samples) {
    mat ret(njoints, samples.n_elem);
    for(int i = 0; i < samples.n_elem; i++)
        ret.col(i) = samples(i);
    return ret;
}

vec pmfFromGaussian(double mean, double var, int size, int min_duration) {
    vec pmf(size);
    for (int t = min_duration; t < min_duration + size; t++) {
        int idx = t - min_duration;
        double tmp = ((t - mean) * (t - mean)) / var;
        pmf(idx) = exp(-0.5 * tmp);
    }
    pmf = pmf * (1.0 / sum(pmf));
    return pmf;
}

int main(int argc, char *argv[]) {
    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Produce help message")
        ("params,p", po::value<string>(), "JSON input params (optional)")
        ("output,o", po::value<string>(), "Filename to store the obs")
        ("vit,v", po::value<string>(), "Filename to store the viterbi output")
        ("ms", po::value<string>(), "state marginals file name")
        ("mr", po::value<string>(), "runlength marginals file name")
        ("md", po::value<string>(), "duration marginals file name ")
        ("imd", po::value<string>(), "implicit duration marginals file name."
                " This means it is computed from the runlength and state")
        ("polybasisfun", po::value<int>()->default_value(1), "Order of the"
                " poly basis")
        ("rbfbasisfun", po::value<int>()->default_value(3), "Number of radial"
                " basis functions to use between 0 and 1. 0,1 are excluded.")
        ("delta", po::value<double>(), "delta between sample locations")
        ("print_ll_vit", "If set then the ll from each hs for"
                " every ground truth segment is printed. Intended for "
                "debugging purposes");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    int min_duration = 30;
    mat transition = ones<mat>(2, 2);
    transition.diag().zeros();
    int nstates = transition.n_rows;
    int ndurations = 30;
    vec pi = {0.5, 0.5};
    mat durations(nstates, ndurations);
    durations.row(0) = conv_to<rowvec>::from(pmfFromGaussian(
                40, 16, ndurations, min_duration));
    durations.row(1) = conv_to<rowvec>::from(pmfFromGaussian(
                50, 16, ndurations, min_duration));
    int njoints = 1;
    json input_params;
    if (vm.count("params")) {
        string params = vm["params"].as<string>();
        ifstream input_params_file(params);
        input_params_file >> input_params;
        min_duration = input_params["min_duration"];
        nstates = input_params["nstates"];
        ndurations = input_params["ndurations"];
        transition = eye<mat>(nstates, nstates);
        pi = eye<vec>(nstates, 1);
        durations = eye<mat>(nstates, ndurations);
        njoints = input_params["emission_params"][0]["num_joints"];
    }

    // Setting a combination of polynomial and rbf basis functions.
    auto poly = make_shared<ScalarPolyBasis>(vm["polybasisfun"].as<int>());
    auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({poly}));
    int nrbf = vm["rbfbasisfun"].as<int>();
    if (nrbf > 0) {
        vec centers = linspace<vec>(0, 1.0, nrbf + 2);
        centers = centers.subvec(1, centers.n_elem - 2);
        auto rbf = shared_ptr<ScalarGaussBasis>(new ScalarGaussBasis(centers,
                    0.25));
        comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({rbf, poly}));
    }
    int n_basis_functions = comb->dim();
    int nparameters = n_basis_functions * njoints;

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w = zeros<vec>(nparameters);
        mat Sigma_w = eye<mat>(nparameters, nparameters);
        mat Sigma_y = 0.00001*eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP poly(comb, promp, njoints);
        promps.push_back(poly);
    }

    // Creating the ProMP emission.
    shared_ptr<AbstractEmissionOnlineSetting> ptr_emission(new ProMPsEmission(
                promps));

    OnlineHSMM promp_hsmm(ptr_emission, transition, pi, durations,
            min_duration);
    if (vm.count("params"))
        promp_hsmm.from_stream(input_params);

    int nseq = 1;
    int nsegments = 10;
    field<ivec> hidden_states, hidden_durations;
    field<field<mat>> multiple_toy_obs = promp_hsmm.sampleMultipleSequences(
            nseq, nsegments, hidden_states, hidden_durations);
    cout << "Generated states and durations for the first sequence" << endl;
    imat viterbi = join_horiz(hidden_states(0), hidden_durations(0));
    cout << viterbi << endl;

    cout << "Model Parameters" << endl;
    json params = promp_hsmm.to_stream();
    cout << params.dump(4) << endl;

    mat obs = fieldToMat(njoints, multiple_toy_obs(0));
    if (vm.count("output"))
        obs.save(vm["output"].as<string>(), raw_ascii);
    if (vm.count("vit"))
        viterbi.save(vm["vit"].as<string>(), raw_ascii);

    // Note that delta only affects the online filtering, not the generation.
    if (vm.count("delta"))
        std::static_pointer_cast<ProMPsEmission>(
                ptr_emission)->setDelta(vm["delta"].as<double>());

    if (vm.count("print_ll_vit")) {
        int idx = 0;
        for(int i = 0; i < viterbi.n_rows; i++) {
            int dur = viterbi(i, 1);
            int hs = viterbi(i, 0);
            cout << "Segment #" << i << ": (" << hs << "," << dur << ")" <<
                    endl;
            const field<mat>& segment = multiple_toy_obs(0).rows(idx,
                    idx + dur - 1);
            idx += dur;
            for(int i = 0; i < nstates; i++) {
                double ll = promp_hsmm.emission_->loglikelihood(i, segment);
                double lld = ll + log(promp_hsmm.duration_(i, dur - min_duration));
                cout << "State " << i << ": " << ll << " with dur: " << lld <<
                        endl;
            }
        }
    }

    mat state_marginals_over_time(nstates, obs.n_cols);
    mat runlength_marginals_over_time(min_duration + ndurations,
            obs.n_cols);
    mat duration_marginals_over_time(ndurations, obs.n_cols);
    mat implicit_duration_marginals_over_time(ndurations, obs.n_cols);
    for(int c = 0; c < obs.n_cols; c++) {
        promp_hsmm.addNewObservation(obs.col(c));
        vec s_marginal = promp_hsmm.getStateMarginal();
        state_marginals_over_time.col(c) = s_marginal;
        vec r_marginal = promp_hsmm.getRunlengthMarginal();
        runlength_marginals_over_time.col(c) = r_marginal;
        vec d_marginal = promp_hsmm.getDurationMarginal();
        duration_marginals_over_time.col(c) = d_marginal;
        if (vm.count("imd")) {
            vec indirect_d_marginal = promp_hsmm.getImplicitDurationMarginal();
            implicit_duration_marginals_over_time.col(c) = indirect_d_marginal;
        }
        if (vm.count("print_ll_vit")) {
            cout << "Obs idx: " << c << endl;
            promp_hsmm.printTopKFromPosterior(40);
        }
    }

    // Saving the marginals if required.
    if (vm.count("ms"))
        state_marginals_over_time.save(vm["ms"].as<string>(), raw_ascii);
    if (vm.count("mr"))
        runlength_marginals_over_time.save(vm["mr"].as<string>(), raw_ascii);
    if (vm.count("md"))
        duration_marginals_over_time.save(vm["md"].as<string>(), raw_ascii);
    if (vm.count("imd"))
        implicit_duration_marginals_over_time.save(vm["imd"].as<string>(),
                raw_ascii);
    return 0;
}

max_stars_repo_name: DiegoAE/BOSD

max_issues_repo_name: DiegoAE/BOSD

max_forks_repo_name: DiegoAE/BOSD


text:
/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: t; c-basic-offset: 4 -*- vim:set ts=4 sw=4 sts=4 noet: */
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "gui/phsp.h"

#include <cassert>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <sstream>
#include <string>

#include <boost/uuid/uuid_io.hpp>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
#include <wx/wx.h>
#pragma GCC diagnostic pop

#include "gui/configuration.h"
#include "gui/document.h"
#include "gui/filename.h"
#include "gui/formula.h"
#include "gui/simulation.h"

namespace flint {
namespace gui {

namespace {

class Writer {
public:
	Writer(const Simulation *sim, const wxString &file);

	~Writer();

	bool operator()();

private:
	const Simulation *sim_;
	FILE *fp_;
};

Writer::Writer(const Simulation *sim, const wxString &file)
	: sim_(sim)
	, fp_(wxFopen(file, "wb"))
{
	if (!fp_)
		wxLogError("failed to open %s", file);
}

Writer::~Writer()
{
	if (fp_)
		std::fclose(fp_);
}

void PrintLnWithIndent(const char *s, int indent, FILE *fp)
{
	for (int i=0;i<indent;i++)
		std::fputc(' ', fp);
	std::fputs(s, fp);
	std::fputc('\n', fp);
}

void WriteSubtreeOfParameterSet(const ParamMap &param_map,
								std::uintptr_t n,
								int indent,
								FILE *fp)
{
	auto *node = reinterpret_cast<ParamTreeNode *>(n);
	assert(node);
	const auto *parameter = dynamic_cast<const ParamTreeParameter *>(node);
	if (parameter) {
		auto xml = parameter->GetXml(indent);
		std::fputs(xml.ToAscii(), fp); // TODO: UTF-8?
	} else if (dynamic_cast<ParamTreeProduct *>(node)) {
		PrintLnWithIndent("<product>", indent, fp);
		auto p = param_map.equal_range(n);
		for (auto it=p.first;it!=p.second;++it)
			WriteSubtreeOfParameterSet(param_map, it->second, indent+2, fp);
		PrintLnWithIndent("</product>", indent, fp);
	} else if (dynamic_cast<ParamTreeZip *>(node)) {
		PrintLnWithIndent("<zip>", indent, fp);
		auto p = param_map.equal_range(n);
		for (auto it=p.first;it!=p.second;++it)
			WriteSubtreeOfParameterSet(param_map, it->second, indent+2, fp);
		PrintLnWithIndent("</zip>", indent, fp);
	} else {
		assert(false);
	}
}

void WriteParameterSet(const ParamMap &param_map, FILE *fp)
{
	if (param_map.empty()) {
		std::fprintf(fp, "      <parameter name='defaultValue'>\n");
		std::fprintf(fp, "        <range type='enum'>0</range>\n");
		std::fprintf(fp, "      </parameter>\n");
		return;
	}
	auto p = param_map.equal_range(0);
	assert(p.first != param_map.end());
	for (auto it=p.first;it!=p.second;++it)
		WriteSubtreeOfParameterSet(param_map, it->second, 6, fp);
}

bool Writer::operator()()
{
	if (!fp_)
		return false;

	std::ostringstream oss, es;
	std::fprintf(fp_, "<?xml version='1.0' encoding='UTF-8'?>\n");
	std::fprintf(fp_, "<phsp xmlns='http://www.physiodesigner.org/2013/ns/phsp/1.0' xmlns:m='http://www.w3.org/1998/Math/MathML' version='1.0'>\n");
	for (const auto &p : sim_->entries) {
		const auto *doc = p.first;
		const auto *config = p.second;
		const auto &path = doc->path();
		auto path_u = path.utf8_str();
		std::fprintf(fp_, "  <model format='%s' iref='%s'>\n",
					 doc->GetFormat(), path_u.data());
		std::fprintf(fp_, "    <target-set>\n");
		for (const auto &target : config->GetTargets(doc)) {
			const auto &column = target.first;
			const auto &value = target.second;
			std::unique_ptr<Formula> formula(Formula::FromUtf8(value.c_str(), es));
			if (!formula) {
				wxLogError("failed to parse parameter value: %s", es.str());
				return false;
			}
			switch (doc->format()) {
			case file::kCellml:
				std::fprintf(fp_,
							 "      <target component='%s' variable='%s'>\n",
							 column.track_name().c_str(),
							 column.name().c_str());
				break;
			case file::kSbml:
				if (std::strncmp(column.name().c_str(), "sbml:", 5) == 0) {
					std::fprintf(fp_,
								 "      <target %s='%s'>\n",
								 (column.type() == lo::Type::S) ? "parameter-id" : "species-id", // TODO: optional reaction-id
								 &column.name().c_str()[5]);
				} else {
					wxLogError("unexpected column name: %s", column.name());
					return false;
				}
				break;
			default:
				{
					boost::uuids::uuid uuid;
					std::memcpy(&uuid, column.uuid().c_str(), 16);
					auto u = boost::uuids::to_string(uuid);
					std::fprintf(fp_, "      <target module-id='%s' physical-quantity-id='%d'>\n",
								 u.c_str(), column.id());
				}
				break;
			}
			formula->WriteMathML("m", oss);
			std::fprintf(fp_, "        <value><m:math>%s</m:math></value>\n",
						 oss.str().c_str());
			oss.str("");
			std::fprintf(fp_, "      </target>\n");
		}
		std::fprintf(fp_, "    </target-set>\n");
		std::fprintf(fp_, "    <parameter-set>\n");
		WriteParameterSet(config->param_map, fp_);
		std::fprintf(fp_, "    </parameter-set>\n");
		std::fprintf(fp_, "  </model>\n");
	}
	std::fprintf(fp_, "</phsp>\n");
	return true;
}

}

bool WritePhsp(const Simulation *sim, const wxFileName &filename)
{
	Writer writer(sim, filename.GetFullPath());
	return writer();
}

}
}

max_stars_repo_name: Abhisheknishant/Flint

max_issues_repo_name: Abhisheknishant/Flint

max_forks_repo_name: Abhisheknishant/Flint


text:
#include "engaging.h"

#include <fstream>
#include <cstdlib>  // For srand() and rand()

#include <Eigen/SVD>
#include <RobotUtilities/utilities.h>
#include <RobotUtilities/TimerLinux.h>

#include <solvehfvc.h>

#define PI 3.14159265

using namespace RUT;
using std::cout;
using std::cerr;
using std::endl;
using std::string;
using Eigen::Vector2d;
using Eigen::Matrix2d;

bool EngagingTaskServer::initEngagingTaskServer() {
  ROS_INFO_STREAM("Engaging server is starting");
  if (_ros_handle_p == nullptr) {
    ROS_ERROR_STREAM("[EngagingTaskServer] You must call .init() before .initEngagingTaskServer().");
    exit(1);
  }

  std::vector<double> scale_force_vector;
  _ros_handle_p->getParam("/constraint_estimation/scale_force_vector", scale_force_vector);
  if (!_ros_handle_p->hasParam("/constraint_estimation/scale_force_vector"))
    ROS_WARN_STREAM("Parameter [/constraint_estimation/scale_force_vector] not found!");

  _force_scale_matrix_inv = Matrix6d::Zero();
  for (int i = 0; i < 6; ++i) _force_scale_matrix_inv(i,i) = 1.0/scale_force_vector[i];

  double pool_duration = 0;
  _ros_handle_p->param(string("/constraint_estimation/pool_duration"), pool_duration, 0.5);
  if (!_ros_handle_p->hasParam("/constraint_estimation/pool_duration"))
      ROS_WARN_STREAM("Parameter [/constraint_estimation/pool_duration] not found");
  _pool_size = (int)round(pool_duration*_main_loop_rate);

  _ros_handle_p->param(string("/task/v_singular_value_threshold"),
      _v_singular_value_threshold, 0.1);
  _ros_handle_p->param(string("/task/f_singular_value_threshold"),
      _f_singular_value_threshold, 0.1);
  if (!_ros_handle_p->hasParam("/task/v_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter [/task/v_singular_value_threshold] not found");
  if (!_ros_handle_p->hasParam("/task/f_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter [/task/f_singular_value_threshold] not found");

  _ros_handle_p->param(std::string("/task/time_step"), _kTimeStepSec, 0.1);
  if (!_ros_handle_p->hasParam("/task/time_step"))
    ROS_WARN_STREAM("Parameter [/task/time_step] not found");
  _ros_handle_p->param(std::string("/task/goal_veloicty_meter"), _kGoalVelocityM, 0.01);
  if (!_ros_handle_p->hasParam("/task/goal_veloicty_meter"))
    ROS_WARN_STREAM("Parameter [/task/goal_veloicty_meter] not found");
  _ros_handle_p->param(std::string("/task/max_normal_force"), _kNormalForceMax, 4.0);
  if (!_ros_handle_p->hasParam("/task/max_normal_force"))
    ROS_WARN_STREAM("Parameter [/task/max_normal_force] not found");
  _ros_handle_p->param(std::string("/task/engaging_force"), _kEngagingForce, 1.0);
  if (!_ros_handle_p->hasParam("/task/engaging_force"))
    ROS_WARN_STREAM("Parameter [/task/engaging_force] not found");

  _ros_handle_p->param(std::string("/task/num_timesteps_file_path"), _timesteps_file_path, std::string("/tmp"));
  if (!_ros_handle_p->hasParam("/task/num_timesteps_file_path"))
    ROS_WARN_STREAM("Parameter [/task/num_timesteps_file_path] not found");

  std::vector<double> goal_W_vector;
  _ros_handle_p->getParam("/task/goal_direction_W1", goal_W_vector);
  if (!_ros_handle_p->hasParam("/task/goal_direction_W1"))
    ROS_WARN_STREAM("Parameter [/task/goal_direction_W1] not found!");
  _goal_W1(0) = goal_W_vector[0];
  _goal_W1(1) = goal_W_vector[1];
  _goal_W1(2) = goal_W_vector[2];
  _goal_W1.normalize();

  _ros_handle_p->getParam("/task/goal_direction_W2", goal_W_vector);
  if (!_ros_handle_p->hasParam("/task/goal_direction_W2"))
    ROS_WARN_STREAM("Parameter [/task/goal_direction_W2] not found!");
  _goal_W2(0) = goal_W_vector[0];
  _goal_W2(1) = goal_W_vector[1];
  _goal_W2(2) = goal_W_vector[2];
  _goal_W2.normalize();

  return true;
}

bool EngagingTaskServer::hostServices() {
  // --------------------------------------------------------
  // Establish Services
  // --------------------------------------------------------
  ros::ServiceServer reset_service            = _ros_handle_p->advertiseService("reset", &EngagingTaskServer::SrvReset, (RobotBridge*)this);
  ros::ServiceServer move_tool_service        = _ros_handle_p->advertiseService("move_tool", &EngagingTaskServer::SrvMoveTool, (RobotBridge*)this);
  ros::ServiceServer move_until_touch_service = _ros_handle_p->advertiseService("move_until_touch", &EngagingTaskServer::SrvMoveUntilTouch, (RobotBridge*)this);
  ros::ServiceServer get_pose_service         = _ros_handle_p->advertiseService("get_pose", &EngagingTaskServer::SrvGetPose, (RobotBridge*)this);
  ros::ServiceServer execute_task_service     = _ros_handle_p->advertiseService("execute_task", &EngagingTaskServer::SrvExecuteTask, this);
  ros::ServiceServer compliant_engage_service = _ros_handle_p->advertiseService("compliant_engage", &EngagingTaskServer::SrvCompliantEngage, this);

  cout << endl << "[EngagingTaskServer] Service servers are listening.." << endl;
  ros::spin();

  ROS_INFO_STREAM(endl << "[EngagingTaskServer] Service servers stopped." << endl);
  return true;
}

bool EngagingTaskServer::SrvCompliantEngage(std_srvs::Empty::Request  &req,
    std_srvs::Empty::Response &res) {
  _controller.reset();

  // read number of time steps
  std::ifstream fp;
  fp.open(_timesteps_file_path);
  if (!fp) {
    cerr << "Unable to open file for fp'.";
        exit(1); // terminate with error
  }
  int num_timesteps;
  fp >> num_timesteps;
  cout << "Number of timesteps: " << num_timesteps << endl;
  fp.close();

  for (int fr = 0; fr < num_timesteps; ++fr) {
    /**
     * Execute the hybrid action (6D)
     */
    // Tool frame velocity
    Matrix6d R_a = Matrix6d::Identity();

    // World frame velocity, set pose
    double pose_set[7];
    _robot.getPose(pose_set);

    Matrix4d SE3_WT_fb = posemm2SE3(pose_set);
    Matrix6d Adj_WT = SE32Adj(SE3_WT_fb);

    Vector6d force_W;
    if (fr < 0.5*num_timesteps)
      force_W << _goal_W1(0), _goal_W1(1), _goal_W1(2), 0, 0, 0;
    else
      force_W << _goal_W2(0), _goal_W2(1), _goal_W2(2), 0, 0, 0;
    force_W.normalize();
    force_W *= _kEngagingForce;
    Vector6d force_T = Adj_WT.transpose() * force_W;

    printf("F in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_W[0],
        force_W[1], force_W[2],force_W[3],force_W[4],force_W[5]);
    // cout << "Press Enter to begin motion: \n";
    // getchar();
    cout << "motion begins:" << endl;
    _controller.ExecuteHFVC(6, 0, R_a, pose_set, force_T.data(),
        HS_CONTINUOUS, _main_loop_rate, _kTimeStepSec);
  }
  return true;
}

bool EngagingTaskServer::SrvExecuteTask(std_srvs::Empty::Request  &req,
    std_srvs::Empty::Response &res) {
  _controller.reset();

  if (_controller._f_queue.size() < 0.5 * _pool_size) {
    cout << "Run update() for " << _pool_size << " frames:" << endl;
    // first, run update for 1s to populate the data deques
    ros::Rate pub_rate(_main_loop_rate);
    for (int i = 0; i < _main_loop_rate; ++i) {
      bool b_is_safe = _controller.update();
      pub_rate.sleep();
    }
    cout << "Done." << endl;
  }

  // read number of time steps
  std::ifstream fp;
  fp.open(_timesteps_file_path);
  if (!fp) {
    cerr << "Unable to open file for fp'.";
        exit(1); // terminate with error
  }
  int num_timesteps;
  fp >> num_timesteps;
  cout << "Number of timesteps: " << num_timesteps << endl;
  fp.close();

  Timer timer;
  std::srand(std::time(0));
  MatrixXd f_data, v_data;
  for (int fr = 0; fr < num_timesteps; ++fr) {
    cout << "time step " << fr << endl;
    timer.tic();
    /**
     * Estimate Natural Constraints
     */
    // get the weighted data
    f_data = MatrixXd::Zero(6, _controller._f_queue.size());
    v_data = MatrixXd::Zero(6, _controller._v_queue.size());
    for (int i = 0; i < _controller._f_queue.size(); ++i)
      f_data.col(i) = _controller._f_queue[i] * _controller._f_weights[i];
    for (int i = 0; i < _controller._v_queue.size(); ++i)
      v_data.col(i) = _controller._v_queue[i] * _controller._v_weights[i];

    // SVD on velocity data
    Eigen::JacobiSVD<MatrixXd> svd_v(v_data.transpose(), Eigen::ComputeThinV);
    VectorXd sigma_v = svd_v.singularValues();
    int DimV = 0;
    for (int i = 0; i < 6; ++i)
      if (sigma_v(i) > _v_singular_value_threshold) DimV ++;

    cout << "DimV: " << DimV << endl;
    // get a basis for row space of velocity data
    MatrixXd rowspace_v = svd_v.matrixV().leftCols(DimV);

    // filter out force data that:
    //    1. has a small weight
    std::vector<int> f_id;
    for (int i = 0; i < f_data.cols(); ++i) {
      double length = f_data.col(i).norm();
      if (length > 1.5) { // weighted length in newton
        f_id.push_back(i);
      }
    }
    int f_data_length = f_id.size();
    cout << "f_data_length: " << f_data_length << endl;
    MatrixXd f_data_filtered = MatrixXd::Zero(6, f_data_length);
    for (int i = 0; i < f_data_length; ++i)
      f_data_filtered.col(i) = f_data.col(f_id[i]);

    int DimF;
    MatrixXd Nf, Nf_null;
    MatrixXd f_data_selected;

    if (f_data_length < 6) {
      DimF = 0;
      Nf = MatrixXd(0, 6);
      Nf_null = MatrixXd::Identity(6, 6);
      f_data_selected = MatrixXd(6, 0);
    } else {
      // SVD on filtered force data, get the prime direction
      Eigen::JacobiSVD<MatrixXd> svd_f(f_data_filtered.transpose(), Eigen::ComputeThinV);
      VectorXd sigma_f = svd_f.singularValues();
      // check dimensions, estimate natural constraints
      double threshold = std::max(_f_singular_value_threshold, 0.1*sigma_f(0));
      DimF = 0;
      for (int i = 0; i < 6; ++i)
        if (sigma_f(i) > threshold) DimF ++;
      cout << "DimF: " << DimF << endl;

      // Sample DimF force directions
      MatrixXd f_data_normalized = f_data_filtered;
      for (int i = 0; i < f_data_length; ++i) f_data_normalized.col(i).normalize();
      int kNFSamples = (int)pow(double(f_data_length), 0.7);
      f_data_selected = MatrixXd(6, DimF);
      if (DimF == 1) {
        f_data_selected = f_data_filtered.rowwise().mean();
        f_data_selected.normalize();
      } else {
        MatrixXd f_data_selected_new(6, DimF);
        double f_distance = 0;
        assert(f_data_length < 32767);
        for (int i = 0; i < kNFSamples; ++i) {
          // 1. sample
          for (int s = 0; s < DimF; s++) {
            int r = (rand() % f_data_length);
            f_data_selected_new.col(s) = f_data_normalized.col(r);
          }
          // 2. compute distance
          double f_distance_new = 0;
          for (int ii = 0; ii < DimF-1; ++ii)
            for (int jj = ii+1; jj < DimF; ++jj)
              f_distance_new += (f_data_selected_new.col(ii) -
                  f_data_selected_new.col(jj)).norm();
          // 3. Update data
          if (f_distance_new > f_distance) {
            f_data_selected = f_data_selected_new;
            f_distance = f_distance_new;
          }
        } // end sampling
      }
      // Get Nf
      Nf = f_data_selected.transpose() * _force_scale_matrix_inv;
      Eigen::JacobiSVD<MatrixXd> svd_Nf(Nf, Eigen::ComputeFullV);
      Nf_null = svd_Nf.matrixV().leftCols(6 - DimF);
    }

    // compute goal
    double pose_fb[7], pose_set[7];
    _robot.getPose(pose_fb);
    Matrix3d R_WT = quat2SO3(pose_fb[3], pose_fb[4], pose_fb[5], pose_fb[6]);

    Vector3d goal_T;
    if (fr < 0.5*num_timesteps)
      goal_T = R_WT.transpose() * _goal_W1;
    else
      goal_T = R_WT.transpose() * _goal_W2;
    MatrixXd G0 = MatrixXd::Zero(1, 6);
    G0.block<1, 3>(0, 0) = goal_T.transpose();
    /**
     * Project Goal to the Null space of Nf
     */
    MatrixXd G = G0*Nf_null;
    double G_norm = G.norm();
    if (G_norm < 1e-5) {
      cerr << "[Engaging] Nf is conflicting with G." << endl;
      getchar();
    }
    G = G/G_norm;
    VectorXd b_G = VectorXd::Zero(1);
    b_G(0) = _kGoalVelocityM;

    /**
     * Do Hybrid Servoing (3D)
     */
    HFVC action;
    const int kDimActualized      = 6;
    const int kDimUnActualized    = 0;
    const int kDimSlidingFriction = 0;
    const int kNumSeeds           = 5;
    const int kDimLambda          = DimF;
    const int kPrintLevel         = 1;

    VectorXd F = VectorXd::Zero(6);
    MatrixXd Aeq(0, DimF + 6); // dummy
    VectorXd beq(0); // dummy
    MatrixXd A = MatrixXd::Zero(DimF, DimF + 6);
    VectorXd b_A = VectorXd::Zero(DimF);
    if (DimF > 0) {
      A.leftCols(DimF) = MatrixXd::Identity(DimF,DimF);
      for (int i = 0; i < DimF; ++i) b_A(i) = _kNormalForceMax;
      // trick:
      // one big force, others are small force
      if (DimF > 1) {
        A(0, 0) = -1;
        b_A(0) = -_kEngagingForce;
        // Vector6d goal_dir = G.row(0).transpose();
        // Vector6d rand_dir = Vector6d::Random().normalized();
        // while (goal_dir.dot(rand_dir) < 0) {
        //   rand_dir = Vector6d::Random().normalized();
        // }
        // rand_dir.tail(3) *= 25; // scale between N and Nm
        // MatrixXd Atemp = A;
        // VectorXd b_ATemp = b_A;
        // A = MatrixXd::Zero(DimF + 1, DimF + 6);
        // A.topRows(DimF) = Atemp;
        // A.block<1, 6>(DimF, kDimLambda) = rand_dir.transpose();
        // b_A = VectorXd::Zero(DimF + 1);
        // b_A.head(DimF) = b_ATemp;
        // b_A(DimF) = -_kEngagingForce;
      }
    }

    cout << "Nf: " << Nf.format(MatlabFmt) << endl;
    // cout << "G: " << G.format(MatlabFmt) << endl;
    // cout << "b_G: " << b_G.format(MatlabFmt) << endl;
    // cout << "F: " << F.format(MatlabFmt) << endl;
    // cout << "Aeq: " << Aeq.format(MatlabFmt) << endl;
    // cout << "beq: " << beq.format(MatlabFmt) << endl;
    // cout << "A: " << A.format(MatlabFmt) << endl;
    // cout << "b_A: " << b_A.format(MatlabFmt) << endl;

    solvehfvc(Nf, G, b_G, F, Aeq, beq, A, b_A,
      kDimActualized, kDimUnActualized,
      kDimSlidingFriction, kDimLambda,
      kNumSeeds, kPrintLevel,
      &action);

    double computation_time_ms = timer.toc();

    /**
     * Execute the hybrid action (6D)
     */
    Vector6d v_Tr = Vector6d::Zero();
    for (int i = 0; i < action.n_av; ++i)  v_Tr(i+action.n_af) = action.w_av(i);

    // Tool frame velocity
    Matrix6d R_a_inv = action.R_a.inverse();
    Vector6d v_T = R_a_inv*v_Tr;

    // limit the magnitude of v_T
    const double scale_rot_to_tran = 0.5; // 1m = 2rad
    Vector6d v_T_scaled = v_T;
    v_T_scaled.tail(3) *= scale_rot_to_tran;
    if (v_T_scaled.norm() > _kGoalVelocityM) {
      double scale_safe = _kGoalVelocityM/v_T_scaled.norm();
      v_T *= scale_safe;
    }

    // World frame velocity, set pose
    _robot.getPose(pose_fb);
    Matrix4d SE3_WT_fb = posemm2SE3(pose_fb);
    Matrix6d Adj_WT = SE32Adj(SE3_WT_fb);
    Vector6d v_W = Adj_WT*v_T;
    Matrix4d SE3_WT_command;
    SE3_WT_command = SE3_WT_fb + wedge6(v_W)*SE3_WT_fb*_kTimeStepSec;
    SE32Posemm(SE3_WT_command, pose_set);

    // Tool frame force
    Vector6d force_Tr_set = Vector6d::Zero();
    for (int i = 0; i < action.n_af; ++i)  force_Tr_set[i] = action.eta_af(i);
    Vector6d force_T = R_a_inv*force_Tr_set;
    Matrix6d Adj_TW = SE32Adj(SE3Inv(SE3_WT_fb));
    Vector6d force_W = Adj_TW.transpose() * force_T;

    double force_fb[6];
    int safe_flag = _robot.getWrench(force_fb);

    printf("F in tool: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_T[0],
        force_T[1], force_T[2],force_T[3],force_T[4],force_T[5]);
    printf("F fb: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_fb[0],
        force_fb[1], force_fb[2],force_fb[3],force_fb[4],force_fb[5]);
    printf("V in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", v_W[0], v_W[1],
        v_W[2],v_W[3],v_W[4],v_W[5]);
    printf("F in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_W[0],
        force_W[1], force_W[2],force_W[3],force_W[4],force_W[5]);
    cout << "Computation Time: " << computation_time_ms << endl;
    if (safe_flag == 2) {
      ROS_ERROR_STREAM("Force feedback stall!!!");
      return true;
    }

    if ((safe_flag != 0)||(fr == num_timesteps-1)) {
      // print to file
      std::ofstream fp;
      // f_queue
      fp.open(_task_data_file_path + "f_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_queue.size(); ++i) {
        stream_array_in(fp, _controller._f_queue[i].data(), 3);
        fp << endl;
      }
      fp.close();
      // v_queue
      fp.open(_task_data_file_path + "v_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._v_queue.size(); ++i) {
        stream_array_in(fp, _controller._v_queue[i].data(), 3);
        fp << endl;
      }
      fp.close();
      // f_weights
      fp.open(_task_data_file_path + "f_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_weights.size(); ++i)
        fp << _controller._f_weights[i] << endl;
      fp.close();

      // f_prob
      fp.open(_task_data_file_path + "f_prob.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_probability.size(); ++i)
        fp << _controller._f_probability[i] << endl;
      fp.close();

      // v_weights
      fp.open(_task_data_file_path + "v_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._v_weights.size(); ++i)
        fp << _controller._v_weights[i] << endl;
      fp.close();

      // f_data_filtered
      fp.open(_task_data_file_path + "f_data_filtered.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_filtered.transpose() << endl;
      fp.close();
      // f_data_selected
      fp.open(_task_data_file_path + "f_data_selected.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_selected.transpose() << endl;
      fp.close();
      // others
      fp.open(_task_data_file_path + "process.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << DimV << endl << DimF << endl;
      stream_array_in(fp, v_T.data(), 3);
      fp << endl;
      stream_array_in(fp, force_T.data(), 3);
      fp.close();
    }

    if (std::isnan(force_Tr_set[0])) {
      cout << "================== NaN =====================" << endl;
      cout << "Press Enter to continue.." << endl;
      getchar();
    }

    if (safe_flag == 3) {
      ROS_ERROR_STREAM("Force feedback violated safety limit!!!");
      if (fr < 0.5*num_timesteps) {
        pose_fb[0] -= _goal_W1[0]*2.0;
        pose_fb[1] -= _goal_W1[1]*2.0;
        pose_fb[2] -= _goal_W1[2]*2.0;
      } else {
        pose_fb[0] -= _goal_W2[0]*2.0;
        pose_fb[1] -= _goal_W2[1]*2.0;
        pose_fb[2] -= _goal_W2[2]*2.0;
      }
      _robot.setPose(pose_fb);
    } else {
      // cout << "Current pose: " << pose_fb[0] << ", " << pose_fb[1] << ", " << pose_fb[2] << endl;
      // cout << "Set pose: " << pose_set[0] << ", " << pose_set[1] << ", " << pose_set[2] << endl;
      // cout << "Press Enter to begin motion: \n";
      // getchar();
      cout << "motion begins:" << endl;
      _controller.ExecuteHFVC(action.n_af, action.n_av,
          action.R_a, pose_set, force_Tr_set.data(),
          HS_CONTINUOUS, _main_loop_rate, _kTimeStepSec);

      cout << endl;
    }

  } // end for
  return true;
}


text:
/*
 *          Copyright Andrey Semashev 2007 - 2014.
 * 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)
 */
/*!
 * \file   debug_output_backend.hpp
 * \author Andrey Semashev
 * \date   07.11.2008
 *
 * The header contains a logging sink backend that outputs log records to the debugger.
 */

#ifndef BOOST_LOG_SINKS_DEBUG_OUTPUT_BACKEND_HPP_INCLUDED_
#define BOOST_LOG_SINKS_DEBUG_OUTPUT_BACKEND_HPP_INCLUDED_

#include <string>
#include <boost/log/detail/config.hpp>

#ifdef BOOST_HAS_PRAGMA_ONCE
#pragma once
#endif

#ifndef BOOST_LOG_WITHOUT_DEBUG_OUTPUT

#include <boost/log/sinks/basic_sink_backend.hpp>
#include <boost/log/sinks/frontend_requirements.hpp>
#include <boost/log/attributes/attribute_value_set.hpp>
#include <boost/log/core/record_view.hpp>
#include <boost/log/detail/header.hpp>

namespace boost {

BOOST_LOG_OPEN_NAMESPACE

namespace sinks {

/*!
 * \brief An implementation of a logging sink backend that outputs to the debugger
 *
 * The sink uses Windows API in order to write log records as debug messages, if the
 * application process is run under debugger. The sink backend also provides a specific
 * filter that allows to check whether the debugger is available and thus elide unnecessary
 * formatting.
 */
template< typename CharT >
class basic_debug_output_backend :
    public basic_formatted_sink_backend< CharT, concurrent_feeding >
{
    //! Base type
    typedef basic_formatted_sink_backend< CharT, concurrent_feeding > base_type;

public:
    //! Character type
    typedef typename base_type::char_type char_type;
    //! String type to be used as a message text holder
    typedef typename base_type::string_type string_type;

public:
    /*!
     * Constructor. Initializes the sink backend.
     */
    BOOST_LOG_API basic_debug_output_backend();
    /*!
     * Destructor
     */
    BOOST_LOG_API ~basic_debug_output_backend();

    /*!
     * The method passes the formatted message to debugger
     */
    BOOST_LOG_API void consume(record_view const& rec, string_type const& formatted_message);
};

#ifdef BOOST_LOG_USE_CHAR
typedef basic_debug_output_backend< char > debug_output_backend;      //!< Convenience typedef for narrow-character logging
#endif
#ifdef BOOST_LOG_USE_WCHAR_T
typedef basic_debug_output_backend< wchar_t > wdebug_output_backend;  //!< Convenience typedef for wide-character logging
#endif

} // namespace sinks

BOOST_LOG_CLOSE_NAMESPACE // namespace log

} // namespace boost

#include <boost/log/detail/footer.hpp>

#endif // BOOST_LOG_WITHOUT_DEBUG_OUTPUT

#endif // BOOST_LOG_SINKS_DEBUG_OUTPUT_BACKEND_HPP_INCLUDED_

ext: hpp

max_stars_repo_name: bowlofstew/omim

max_issues_repo_name: bowlofstew/omim

max_forks_repo_name: bowlofstew/omim


text:
#include <opencv2/imgcodecs.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <boost/multi_array.hpp>
#include <opencv2/imgproc.hpp>
#include <cmath>
#include <vector>
#include <iostream>
#include <stack>
#include <queue>
#define oneighthPI (3.141592654/8.)

using namespace cv;
using namespace std;
class Point0{
    public:
	int x;
	int y;
	Point0(int x,int y);
};
class matrics
{
    public:
	int rows;
	int cols;
	Mat bgr;
	Mat gray;
	Mat GauBlurMat;
	typedef boost::multi_array<double,3> Esn0;
	typedef boost::multi_array<double,2> thinMat;
	thinMat In;
	thinMat Infinal;
	Esn0 es0;
	enum {strength,tangent};
	enum operators {oper2x2,sobel};
	matrics(const char *filename);
	Mat doubleArray2Mat(thinMat);
	thinMat triTo2d(Esn0 es0);
	void gausblur(int i);
	void enhance(operators op);
	void nonmaximaSupress();
	void doubleTholdNlink(double th,double tl);
};


text:
#include <iostream>
#include <algorithm>
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <tuple>
#include <stdbool.h>
#include <bitset>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;
using namespace std;

int main(void) {
    int n;
    cin >> n;
    string s;
    cin >> s;

    int r = 0, b = 0;
    for(const auto& c : s) {
        switch(c) {
            case 'R':
                r++;
                break;
            case 'B':
                b++;
                break;
        }
    }
    if (r > b) {
        cout << "Yes" << endl;
    } else {
        cout << "No" << endl;
    }
    return 0;
}

max_stars_repo_name: kamiyaowl/atcoder

max_issues_repo_name: kamiyaowl/atcoder

max_forks_repo_name: kamiyaowl/atcoder


text:
/*
Copyright (c) 2013, David C Horton

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <algorithm>
#include <regex>

#include <boost/algorithm/string.hpp>

#include <arpa/inet.h>

#include "sip-transports.hpp"
#include "controller.hpp"
#include "drachtio.h"

namespace {
    /* needed to be able to live in a boost unordered container */
    size_t hash_value( const drachtio::SipTransport& d) {
        std::size_t seed = 0;
        boost::hash_combine(seed, d.getTport());
        return seed;
    }
}

namespace drachtio {

  SipTransport::SipTransport(const string& contact, const string& localNet, const string& externalIp) :
    m_strContact(contact), m_strExternalIp(externalIp), m_strLocalNet(localNet), m_tp(NULL) {
    init() ;
  }
  SipTransport::SipTransport(const string& contact, const string& localNet) :
    m_strContact(contact), m_strLocalNet(localNet), m_tp(NULL) {
    init() ;
  }
  SipTransport::SipTransport(const string& contact) : m_strContact(contact), m_tp(NULL) {
    init() ;
  }
  SipTransport::SipTransport(const std::shared_ptr<drachtio::SipTransport> other) :
    m_strContact(other->m_strContact), m_strLocalNet(other->m_strLocalNet), m_strExternalIp(other->m_strExternalIp),
    m_dnsNames(other->m_dnsNames), m_tp(NULL) {
    init() ;
  }

  SipTransport::~SipTransport() {
  }

  void SipTransport::init() {

    if( !parseSipUri(m_strContact, m_contactScheme, m_contactUserpart, m_contactHostpart, m_contactPort, m_contactParams) ) {
        cerr << "SipTransport::init - contact: " << m_strContact << endl ;
        throw std::runtime_error(string("SipTransport::init - invalid contact ") + m_strContact);
    }

    if( !m_strExternalIp.empty() ) {
      try {
        std::regex re("^([0-9\\.]*)$");
        std::smatch mr;
        if (!std::regex_search(m_strExternalIp, mr, re)) {
          cerr << "SipTransport::init - invalid format for externalIp, must be ipv4 dot decimal address: " << m_strExternalIp << endl ;
          throw std::runtime_error("SipTransport::init - invalid format for externalIp, must be ipv4 dot decimal address");
        }
      } catch( std::regex_error& e) {
        DR_LOG(log_error) << "SipTransport::init - regex error " << e.what();
      }
    }

    bool hasNetmask = false;
    string network, bits;
    uint32_t netbits;

    if( !m_strLocalNet.empty() ) {
      hasNetmask = true;

      try {
        std::regex re("^([0-9\\.]*)\\/(.*)$");
        std::smatch mr;
        if (!std::regex_search(m_strLocalNet, mr, re)) {
          cerr << "SipTransport::init - invalid format for localNet, must be CIDR format: " << m_strLocalNet << endl ;
          throw std::runtime_error("SipTransport::init - invalid format for localNet, must be CIDR format");
        }
        network = mr[1] ;
        bits = mr[2];
      } catch (std::regex_error& e) {
        DR_LOG(log_error) << "SipTransport::init - regex error 2 " << e.what();
      }
    }

    if (hasNetmask) {
      struct sockaddr_in range;
      inet_pton(AF_INET, network.c_str(), &(range.sin_addr));
      m_range = htonl(range.sin_addr.s_addr) ;

      uint32_t netbits = ::atoi( bits.c_str() ) ;
      m_netmask = ~(~uint32_t(0) >> netbits);
    }
  }

  bool SipTransport::shouldAdvertisePublic( const char* address ) const {
    if( !hasExternalIp() ) return false ;

    return !isInNetwork(address) ;
  }

  bool SipTransport::isInNetwork(const char* address) const {
    if (!m_netmask) return false;

    struct sockaddr_in addr;
    inet_pton(AF_INET, address, &(addr.sin_addr));

    uint32_t ip = htonl(addr.sin_addr.s_addr) ;

    return (ip & m_netmask) == (m_range & m_netmask) ;
  }

  void SipTransport::getDescription(string& s, bool shortVersion) {
    s = "" ;
    if( hasTport() ) {
      s += getProtocol() ;
      s += "/" ;
      s += getHost() ;
      s += ":" ;
      s += getPort() ;

      if( shortVersion ) return ;

      s += " ";
    }
    s += "(";
    s += getContact() ;
    s += ", external-ip: " ;
    s += getExternalIp() ;
    s += ", local-net: " ;
    s += getLocalNet() ;
    s += ")" ;

    if( m_dnsNames.size() ) {
      s += " dns names: " ;
      int i = 0 ;
      for( vector<string>::const_iterator it = m_dnsNames.begin(); it != m_dnsNames.end(); ++it, i++ ) {
        if( i++ ) {
          s+= ", " ;
        }
        s += *it ;
      }
    }
  }

  void SipTransport::getHostport(string& s) {
    assert(hasTport()) ;
    s = "" ;
    s += getProtocol() ;
    s += "/" ;
    s += hasExternalIp() ? getExternalIp() : getHost() ;
    s += ":" ;
    s += getPort() ;
  }

  void SipTransport::getBindableContactUri(string& contact) {
    contact = m_contactScheme ;
    contact.append(":");

    //host
    if (hasExternalIp()) {
      contact.append(m_strExternalIp) ;
    }
    else {
      contact.append(m_contactHostpart) ;
    }

    //port
    if (!m_contactPort.empty()) {
      contact.append(":") ;
      contact.append(m_contactPort) ;
    }

    //parameters
    for (vector< pair<string,string> >::const_iterator it = m_contactParams.begin(); it != m_contactParams.end(); ++it) {
      contact.append(";");
      contact.append(it->first) ;
      if( !it->second.empty() ) {
        contact.append("=");
        contact.append(it->second) ;
      }
    }

    if (hasExternalIp()) {
      contact.append(";maddr=");
      contact.append(m_contactHostpart);
    }
    DR_LOG(log_debug) << "SipTransport::getBindableContactUri: " << contact;
  }

  void SipTransport::getContactUri(string& contact, bool useExternalIp) {
    contact = m_contactScheme ;
    contact.append(":");
    if( !m_contactUserpart.empty() ) {
      contact.append(m_contactUserpart) ;
      contact.append("@") ;
    }
    if( m_tp ) {
      contact.append(useExternalIp && !m_strExternalIp.empty() ? m_strExternalIp : getHost()) ;
      const char* szPort = getPort() ;
      const char* szProto = getProtocol() ;
      if( szPort && strlen(szPort) > 0 ) {
        contact.append(":") ;
        contact.append(szPort);
      }
    }
    else {
      contact.append(useExternalIp && !m_strExternalIp.empty() ? m_strExternalIp : m_contactHostpart) ;
      if( !m_contactPort.empty() ) {
        contact.append(":") ;
        contact.append(m_contactPort);
      }
      if( m_contactParams.size() > 0 ) {
        for (vector< pair<string,string> >::const_iterator it = m_contactParams.begin(); it != m_contactParams.end(); ++it) {
          pair<string,string> kv = *it ;
          contact.append(";") ;
          contact.append(kv.first);
          if( !kv.second.empty() ) {
            contact.append("=") ;
            contact.append(kv.second) ;
          }
        }
      }
    }

    DR_LOG(log_debug) << "SipTransport::getContactUri - created Contact header: " << contact;
  }
  sip_via_t* SipTransport::makeVia(su_home_t * h, const char* szRemoteHost) {
    bool isInSubnet = szRemoteHost ? this->isInNetwork(szRemoteHost) : false;
    string host = this->getHost();
    if (this->hasExternalIp() && !isInSubnet) {
      host = this->getExternalIp();
    }

    string proto = this->getProtocol();
    if (0 == proto.length()) proto = "UDP";
    boost::to_upper(proto);

    string transport = string("SIP/2.0/") + proto;
    DR_LOG(log_debug) << "SipTransport::makeVia - host " << host << ", port " << this->getPort() << ", transport " << transport ;

    return sip_via_create(h, host.c_str(), this->getPort(), transport.c_str());
  }

  bool SipTransport::isIpV6(void) {
    return hasTport() && NULL != strstr( getHost(), "[") && NULL != strstr( getHost(), "]") ;
  }

  bool SipTransport::isLocalhost(void) {
    return hasTport() && (0 == strcmp(getHost(), "127.0.0.1") || 0 == strcmp(getHost(), "[::1]"));
  }

  bool SipTransport::isLocal(const char* szHost) {
    if( 0 == strcmp(getHost(), szHost) ) {
      return true ;
    }
    else if( 0 == m_contactHostpart.compare(szHost) ) {
      return true ;
    }
    else if( hasExternalIp() && 0 == m_strExternalIp.compare(szHost) ) {
      return true ;
    }
    for( vector<string>::const_iterator it = m_dnsNames.begin(); it != m_dnsNames.end(); ++it ) {
      if( 0 == (*it).compare(szHost) ) {
        return true ;
      }
    }
    return false ;
  }

  uint32_t SipTransport::getOctetMatchCount(const string& address) {
    uint32_t count = 0 ;
    string them[4], mine[4];
    try {
      std::regex re("^(\\d+)\\.(\\d+)\\.(\\d+)\\.(\\d+)");
      std::smatch mr;
      if (std::regex_search(address, mr, re) && mr.size() > 1) {
        them[0] = mr[1] ;
        them[1] = mr[2] ;
        them[2] = mr[3] ;
        them[3] = mr[4] ;

        string host = this->getHost();
        if (std::regex_search(host, mr, re) && mr.size() > 1) {
          mine[0] = mr[1] ;
          mine[1] = mr[2] ;
          mine[2] = mr[3] ;
          mine[3] = mr[4] ;

          for(int i = 0; i < 4; i++) {
            if(0 != them[i].compare(mine[i])) return count;
            count++;
          }
        }
      }
    } catch (std::regex_error& e) {
      DR_LOG(log_error) << "SipTransport::getOctetMatchCount - regex error " << e.what();
    }

    return count;
  }

  /** static methods */

  SipTransport::mapTport2SipTransport SipTransport::m_mapTport2SipTransport  ;
  std::shared_ptr<SipTransport> SipTransport::m_masterTransport ;

  /**
   * iterate all tport_t* that have been created, and any that are "unassigned" associate
   * them with the provided SipTransport
   *
   * @param config - SipTransport that was used to create the currently unassigned tport_t*
   */
  void SipTransport::addTransports(std::shared_ptr<SipTransport> config, unsigned int mtu) {

    tport_t* tp = nta_agent_tports(theOneAndOnlyController->getAgent());
    m_masterTransport = std::make_shared<SipTransport>( config ) ;
    m_masterTransport->setTport(tp) ;

    while (NULL != (tp = tport_next(tp) )) {
      const tp_name_t* tpn = tport_name(tp) ;
      string desc ;
      getTransportDescription( tp, desc );

      if (0 == strcmp(tpn->tpn_proto, "udp") && mtu > 0) {
        tport_set_params(tp, TPTAG_MTU(mtu), TAG_END());
      }

      mapTport2SipTransport::const_iterator it = m_mapTport2SipTransport.find(tp) ;
      if (it == m_mapTport2SipTransport.end()) {
        DR_LOG(log_info) << "SipTransport::addTransports - creating transport: " << hex << tp << ": " << desc ;

        std::shared_ptr<SipTransport> p = std::make_shared<SipTransport>( config ) ;
        p->setTport(tp);
        p->setTportName(tpn) ;
        m_mapTport2SipTransport.insert(mapTport2SipTransport::value_type(tp, p)) ;

        if (p->getLocalNet().empty()) {
          string network, bits;
          string host = p->hasExternalIp() ? p->getExternalIp() : p->getHost();
          if(0 == host.compare("127.0.0.1")) {
            network = "127.0.0.1";
            bits = "32";
          }
          else if(0 == host.find("192.168.0.")) {
            network = "192.168.0.0";
            bits = "24";
          }
          else if(0 == host.find("172.16.")) {
            network = "172.16.0.0";
            bits = "16";
          }
          else if(0 == host.find("10.")) {
            network = "10.0.0.0";
            bits = "8";
          }

          if (!network.empty()) {
            struct sockaddr_in range;
            inet_pton(AF_INET, network.c_str(), &(range.sin_addr));
            p->setRange(htonl(range.sin_addr.s_addr)) ;

            uint32_t netbits = ::atoi( bits.c_str() ) ;
            p->setNetmask(~(~uint32_t(0) >> netbits));
            p->setNetwork(network);
            p->setLocalNet(network, bits);
          }
        }
      }
    }
  }

  std::shared_ptr<SipTransport> SipTransport::findTransport(tport_t* tp) {
    mapTport2SipTransport::const_iterator it = m_mapTport2SipTransport.find(tp) ;
    if( it == m_mapTport2SipTransport.end() ) {
      tport_t* tpp = tport_parent(tp);
      DR_LOG(log_debug) << "SipTransport::findTransport - could not find transport: " << hex << tp << " searching for parent " <<  tpp ;
      it = m_mapTport2SipTransport.find(tport_parent(tpp));
    }
    assert( it != m_mapTport2SipTransport.end() ) ;
    return it->second ;
  }

  std::shared_ptr<SipTransport> SipTransport::findAppropriateTransport(const char* remoteHost, const char* proto) {
    string scheme, userpart, hostpart, port ;
    vector< pair<string,string> > vecParam ;
    string host = remoteHost ;

    DR_LOG(log_debug) << "SipTransport::findAppropriateTransport: searching for a transport to reach " << proto << "/" << remoteHost ;

    if( parseSipUri(host, scheme, userpart, hostpart, port, vecParam) ) {
      host = hostpart ;
      DR_LOG(log_debug) << "SipTransport::findAppropriateTransport: host parsed as " << host;
    }

    string desc ;
    bool wantsIpV6 = (NULL != strstr( remoteHost, "[") && NULL != strstr( remoteHost, "]")) ;

    // create vector of candidates, remove those with wrong transport or protocol
    // then sort from most to least desirable:
    // - transports within the subnet of the remote host
    // - transports that have an external address
    // - ...all others, and finally..
    // - transports bound to localhost
    std::vector< std::shared_ptr<SipTransport> > candidates;
    std::pair< tport_t*, boost::shared_ptr<SipTransport> > myPair;

    for (auto pair : m_mapTport2SipTransport) {
      candidates.push_back(pair.second);
    }

    // filter by transport
    auto it = std::remove_if(candidates.begin(), candidates.end(), [wantsIpV6](const std::shared_ptr<SipTransport>& p) {
      return (wantsIpV6 && !p->isIpV6()) || (!wantsIpV6 && p->isIpV6());
    });
    candidates.erase(it, candidates.end());
    DR_LOG(log_debug) <<  "SipTransport::findAppropriateTransport - after filtering for transport we have " << candidates.size() << " candidates";

    // filter by protocol
    it = std::remove_if(candidates.begin(), candidates.end(), [proto](const std::shared_ptr<SipTransport>& p) {
      if (!proto) return false;
      return 0 != strcmp(p->getProtocol(), proto);
    });
    candidates.erase(it, candidates.end());
    DR_LOG(log_debug) <<  "SipTransport::findAppropriateTransport - after filtering for protocol we have " << candidates.size() << " candidates";

    sort(candidates.begin(), candidates.end(), [host](const std::shared_ptr<SipTransport>& pA, const std::shared_ptr<SipTransport>& pB) {

      if (pA->isInNetwork(host.c_str())) {
        return true;
      }
      if (pB->isInNetwork(host.c_str())) {
        return false;
      }

      uint32_t a = pA->getOctetMatchCount(host);
      uint32_t b = pB->getOctetMatchCount(host);
      if (a > b) return true;
      if (a < b) return false;

      if (pA->hasExternalIp()) return true;
      if (pB->hasExternalIp()) return false;

      if (pA->isLocalhost()) return false;
      if (pB->isLocalhost()) return true;

      return true;
    });

#ifdef DEBUG
    DR_LOG(log_debug) <<  "SipTransport::findAppropriateTransport - in priority order, here are the candidates for sending to  " << host;
    BOOST_FOREACH(std::shared_ptr<SipTransport>& p, candidates) {
      string desc;
      p->getDescription(desc);
      DR_LOG(log_debug) << "SipTransport::findAppropriateTransport -   " << desc;
    }
#endif

    if (candidates.empty()) {
      m_masterTransport->getDescription(desc) ;
      DR_LOG(log_debug) << "SipTransport::findAppropriateTransport: - returning master transport " << hex << m_masterTransport->getTport() <<
        " as we found no better matches: " << desc ;
      return m_masterTransport ;
    }

    std::shared_ptr<SipTransport> p = candidates[0];
    p->getDescription(desc);
    DR_LOG(log_debug) << "SipTransport::findAppropriateTransport: - returning the best match " << hex << p->getTport() << ": " << desc ;
    return p ;
  }

  void SipTransport::getAllExternalIps( vector<string>& vec ) {
    for (mapTport2SipTransport::const_iterator it = m_mapTport2SipTransport.begin(); m_mapTport2SipTransport.end() != it; ++it ) {
      std::shared_ptr<SipTransport> p = it->second ;
      if( p->hasExternalIp() ) {
        vec.push_back(p->getExternalIp()) ;
      }
      for( vector<string>::const_iterator itDns = p->getDnsNames().begin(); itDns != p->getDnsNames().end(); ++itDns ) {
        vec.push_back(*itDns) ;
      }
    }
  }
  void SipTransport::getAllExternalContacts( vector< pair<string, string> >& vec ) {
    for (mapTport2SipTransport::const_iterator it = m_mapTport2SipTransport.begin(); m_mapTport2SipTransport.end() != it; ++it ) {
      std::shared_ptr<SipTransport> p = it->second ;
      string port = p->getPort() != NULL ? p->getPort() : "5060";
      if( p->hasExternalIp() ) {
        vec.push_back( make_pair(p->getExternalIp(), port)) ;
      }
      for( vector<string>::const_iterator itDns = p->getDnsNames().begin(); itDns != p->getDnsNames().end(); ++itDns ) {
        vec.push_back( make_pair(*itDns, port)) ;
      }
    }
  }
  void SipTransport::getAllHostports( vector<string>& vec ) {
    for (mapTport2SipTransport::const_iterator it = m_mapTport2SipTransport.begin(); m_mapTport2SipTransport.end() != it; ++it ) {
      std::shared_ptr<SipTransport> p = it->second ;
      string desc ;
      p->getHostport(desc);
      vec.push_back(desc) ;
    }
  }

  bool SipTransport::isLocalAddress(const char* szHost, tport_t* tp) {
    if( tp ) {
     std::shared_ptr<SipTransport> p = SipTransport::findTransport(tp) ;
     return p->isLocal(szHost);
    }

    // search all
    for (mapTport2SipTransport::const_iterator it = m_mapTport2SipTransport.begin(); m_mapTport2SipTransport.end() != it; ++it ) {
      std::shared_ptr<SipTransport> p = it->second ;
      if( p->isLocal(szHost) ) {
        return true ;
      }
      for( vector<string>::const_iterator itDns = p->getDnsNames().begin(); itDns != p->getDnsNames().end(); ++itDns ) {
        if( 0 == (*itDns).compare(szHost) ) {
          return true;
        }
      }
    }

    return false;
  }

  void SipTransport::logTransports() {
    DR_LOG(log_info) << "SipTransport::logTransports - there are : " << dec <<  m_mapTport2SipTransport.size() << " transports";

    for (mapTport2SipTransport::const_iterator it = m_mapTport2SipTransport.begin(); m_mapTport2SipTransport.end() != it; ++it ) {
      std::shared_ptr<SipTransport> p = it->second ;
      string desc ;

      p->getDescription(desc, false) ;

      if (0 == strcmp(p->getProtocol(), "udp")) {
        unsigned int mtuSize = 0;
        const tport_t* tp = p->getTport();
        tport_get_params(tp, TPTAG_MTU_REF(mtuSize), TAG_END());
        DR_LOG(log_info) << "SipTransport::logTransports - " << desc << ", mtu size: " << mtuSize;
      }
      else {
        DR_LOG(log_info) << "SipTransport::logTransports - " << desc ;
      }
    }
  }

}

max_stars_repo_name: danias/drachtio-server

max_issues_repo_name: danias/drachtio-server

max_forks_repo_name: danias/drachtio-server