text:
#include "amcChannelDescriptor.hpp"
#include "testUtils.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/assign/list_of.hpp>
#include <iostream>

BOOST_AUTO_TEST_SUITE( amcChannelDescriptorTests )

BOOST_AUTO_TEST_CASE( notEnoughPortNumbers )
{
  const std::vector<int> ports = boost::assign::list_of(0x01)(0x02)(0x03);
  BOOST_CHECK_THROW( amcChannelDescriptor acd( ports ), std::runtime_error );
}

BOOST_AUTO_TEST_CASE( tooManyPortNumbers )
{
  const std::vector<int> ports = boost::assign::list_of(0x01)(0x02)(0x03)(0x04)(0x05);
  BOOST_CHECK_THROW( amcChannelDescriptor acd( ports ), std::runtime_error );
}

BOOST_AUTO_TEST_CASE( portNumberNegative )
{
  const std::vector<int> ports = boost::assign::list_of(0x01)(-10)(0x03)(0x04);
  BOOST_CHECK_THROW( amcChannelDescriptor acd( ports ), std::out_of_range );
}

BOOST_AUTO_TEST_CASE( portNumberTooLarge )
{
  const std::vector<int> ports = boost::assign::list_of(0x01)(32)(0x03)(0x04);
  BOOST_CHECK_THROW( amcChannelDescriptor acd( ports ), std::out_of_range );
}

BOOST_AUTO_TEST_CASE( constructorGetBinaryData )
{
  const std::vector<int> ports = boost::assign::list_of(0x01)(0x02)(0x03)(0x04);
  amcChannelDescriptor acd( ports );
  std::vector<uint8_t> manResult = boost::assign::list_of(0x41)(0x0c)(0xf2);
  std::vector<uint8_t> autoResult = acd.getBinaryData();
  BOOST_CHECK_EQUAL_COLLECTIONS( autoResult.cbegin(), autoResult.cend(), manResult.cbegin(), manResult.cend() );
  if( autoResult != manResult )
  {
    std::cout << "should be: ";
    std::copy(manResult.cbegin(), manResult.cend(), std::ostream_iterator<int>(std::cout << std::hex, " "));
    std::cout << std::endl;
    std::cout << "is:        ";
    std::copy(autoResult.cbegin(), autoResult.cend(), std::ostream_iterator<int>(std::cout << std::hex, " "));
    std::cout << std::endl;
  }
}

BOOST_AUTO_TEST_CASE( binarySize )
{
  const std::vector<int> ports = boost::assign::list_of(0x01)(0x02)(0x03)(0x04);
  amcChannelDescriptor acd( ports );
  BOOST_CHECK_EQUAL( acd.getBinaryData().size(), 3 );
}

BOOST_AUTO_TEST_CASE( size )
{
  const std::vector<int> ports = boost::assign::list_of(0x01)(0x02)(0x03)(0x04);
  amcChannelDescriptor acd( ports );
  BOOST_CHECK_EQUAL( acd.size(), 3 );
}

BOOST_AUTO_TEST_SUITE_END()


text:
//  (C) Copyright 2015 Boost.test team
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>

struct my_struct {
  my_struct(int var_) : var(var_)
  {
    if(var_ < 0) throw std::runtime_error("negative value not allowed");
  }
  int var;
};

BOOST_AUTO_TEST_CASE( test )
{
  my_struct instance(-2);
  // ...
}

BOOST_AUTO_TEST_CASE( test2 )
{
  BOOST_TEST(true);
}
//]

ext: cpp


text:
// Copyright (C) 2001-2003
// William E. Kempf
// Copyright (C) 2007 Anthony Williams
// Copyright (C) 2013 Andrey Semashev
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <cstddef>
#include <new>
#include <iostream>
#include <boost/aligned_storage.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/thread/thread.hpp>
#include <boost/sync/detail/config.hpp>
#include <boost/sync/thread_specific/thread_specific_ptr.hpp>
#include <boost/sync/mutexes/mutex.hpp>
#include <boost/sync/locks/lock_guard.hpp>
#include "utils.hpp"

#if defined(BOOST_SYNC_DETAIL_PLATFORM_WINAPI)
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#else
#include <pthread.h>
#endif

boost::sync::mutex check_mutex;
boost::sync::mutex tss_mutex;
int tss_instances = 0;
int tss_total = 0;

struct tss_value_t
{
    tss_value_t()
    {
        boost::sync::lock_guard<boost::sync::mutex> lock(tss_mutex);
        ++tss_instances;
        ++tss_total;
        value = 0;
    }

    ~tss_value_t()
    {
        boost::sync::lock_guard<boost::sync::mutex> lock(tss_mutex);
        --tss_instances;
    }

    int value;
};

boost::sync::thread_specific_ptr<tss_value_t> tss_value;

void test_tss_thread()
{
    tss_value.reset(new tss_value_t());
    for (int i=0; i<1000; ++i)
    {
        int& n = tss_value->value;
        if (n != i)
        {
            // Don't call BOOST_CHECK_EQUAL directly, as it's not thread-safe.
            boost::sync::lock_guard<boost::sync::mutex> lock(check_mutex);
            BOOST_CHECK_EQUAL(n, i);
        }
        ++n;
    }
}

#if defined(BOOST_SYNC_DETAIL_PLATFORM_WINAPI)

typedef HANDLE native_thread_t;

DWORD WINAPI test_tss_thread_native(LPVOID /*lpParameter*/)
{
    test_tss_thread();
    return 0;
}

native_thread_t create_native_thread(void)
{
    native_thread_t const res=CreateThread
    (
        0, //security attributes (0 = not inheritable)
        0, //stack size (0 = default)
        &test_tss_thread_native, //function to execute
        0, //parameter to pass to function
        0, //creation flags (0 = run immediately)
        0  //thread id (0 = thread id not returned)
    );
    BOOST_CHECK(res!=0);
    return res;
}

void join_native_thread(native_thread_t thread)
{
    DWORD res = WaitForSingleObject(thread, INFINITE);
    BOOST_CHECK(res == WAIT_OBJECT_0);

    res = CloseHandle(thread);
    BOOST_CHECK(SUCCEEDED(res));
}

#else // defined(BOOST_SYNC_DETAIL_PLATFORM_WINAPI)

typedef pthread_t native_thread_t;

extern "C"
{
    void* test_tss_thread_native(void*)
    {
        test_tss_thread();
        return 0;
    }
}

native_thread_t create_native_thread()
{
    native_thread_t thread_handle;

    int const res = pthread_create(&thread_handle, 0, &test_tss_thread_native, 0);
    BOOST_CHECK(!res);
    return thread_handle;
}

void join_native_thread(native_thread_t thread)
{
    void* result=0;
    int const res = pthread_join(thread, &result);
    BOOST_CHECK(!res);
}

#endif // defined(BOOST_SYNC_DETAIL_PLATFORM_WINAPI)

void do_test_tss()
{
    tss_instances = 0;
    tss_total = 0;

    const int NUMTHREADS = 5;
    boost::thread_group threads;
    try
    {
        for (int i = 0; i < NUMTHREADS; ++i)
            threads.create_thread(&test_tss_thread);
        threads.join_all();
    }
    catch(...)
    {
        threads.interrupt_all();
        threads.join_all();
        throw;
    }

    std::cout
        << "tss_instances = " << tss_instances
        << "; tss_total = " << tss_total
        << "\n";
    std::cout.flush();

    BOOST_CHECK_EQUAL(tss_instances, 0);
    BOOST_CHECK_EQUAL(tss_total, 5);

    tss_instances = 0;
    tss_total = 0;

    native_thread_t thread1 = create_native_thread();
    native_thread_t thread2 = create_native_thread();
    native_thread_t thread3 = create_native_thread();
    native_thread_t thread4 = create_native_thread();
    native_thread_t thread5 = create_native_thread();

    join_native_thread(thread5);
    join_native_thread(thread4);
    join_native_thread(thread3);
    join_native_thread(thread2);
    join_native_thread(thread1);

    std::cout
        << "tss_instances = " << tss_instances
        << "; tss_total = " << tss_total
        << "\n";
    std::cout.flush();

    // The following is not really an error. TSS cleanup support still is available for boost threads.
    // Also this usually will be triggered only when bound to the static version of thread lib.
    // 2006-10-02 Roland Schwarz
    //BOOST_CHECK_EQUAL(tss_instances, 0);
    BOOST_CHECK_MESSAGE(tss_instances == 0, "Support of automatic tss cleanup for native threading API not available");
    BOOST_CHECK_EQUAL(tss_total, 5);
}

BOOST_AUTO_TEST_CASE(test_tss)
{
    timed_test(&do_test_tss, 2);
}

bool tss_cleanup_called = false;

struct Dummy
{
};

void tss_custom_cleanup(Dummy* d)
{
    delete d;
    tss_cleanup_called=true;
}

boost::sync::thread_specific_ptr<Dummy> tss_with_cleanup(tss_custom_cleanup);

void tss_thread_with_custom_cleanup()
{
    tss_with_cleanup.reset(new Dummy);
}

void do_test_tss_with_custom_cleanup()
{
    boost::thread t(tss_thread_with_custom_cleanup);
    try
    {
        t.join();
    }
    catch(...)
    {
        t.interrupt();
        t.join();
        throw;
    }

    BOOST_CHECK(tss_cleanup_called);
}

BOOST_AUTO_TEST_CASE(test_tss_with_custom_cleanup)
{
    timed_test(&do_test_tss_with_custom_cleanup, 2);
}

Dummy* tss_object = new Dummy;

void tss_thread_with_custom_cleanup_and_release()
{
    tss_with_cleanup.reset(tss_object);
    tss_with_cleanup.release();
}

void do_test_tss_does_no_cleanup_after_release()
{
    tss_cleanup_called = false;
    boost::thread t(tss_thread_with_custom_cleanup_and_release);
    try
    {
        t.join();
    }
    catch(...)
    {
        t.interrupt();
        t.join();
        throw;
    }

    BOOST_CHECK(!tss_cleanup_called);
    if(!tss_cleanup_called)
    {
        delete tss_object;
    }
}

struct dummy_class_tracks_deletions
{
    static unsigned deletions;

    ~dummy_class_tracks_deletions()
    {
        ++deletions;
    }
};

unsigned dummy_class_tracks_deletions::deletions = 0;

boost::sync::thread_specific_ptr<dummy_class_tracks_deletions> tss_with_null_cleanup(NULL);

void tss_thread_with_null_cleanup(dummy_class_tracks_deletions* delete_tracker)
{
    tss_with_null_cleanup.reset(delete_tracker);
}

void do_test_tss_does_no_cleanup_with_null_cleanup_function()
{
    dummy_class_tracks_deletions* delete_tracker = new dummy_class_tracks_deletions;
    boost::thread t(tss_thread_with_null_cleanup, delete_tracker);
    try
    {
        t.join();
    }
    catch(...)
    {
        t.interrupt();
        t.join();
        throw;
    }

    BOOST_CHECK(!dummy_class_tracks_deletions::deletions);
    if(!dummy_class_tracks_deletions::deletions)
    {
        delete delete_tracker;
    }
}

BOOST_AUTO_TEST_CASE(test_tss_does_no_cleanup_after_release)
{
    timed_test(&do_test_tss_does_no_cleanup_after_release, 2);
}

BOOST_AUTO_TEST_CASE(test_tss_does_no_cleanup_with_null_cleanup_function)
{
    timed_test(&do_test_tss_does_no_cleanup_with_null_cleanup_function, 2);
}

void thread_with_local_tss_ptr()
{
    {
        boost::sync::thread_specific_ptr<Dummy> local_tss(tss_custom_cleanup);

        local_tss.reset(new Dummy);
    }
    BOOST_CHECK(tss_cleanup_called);
    tss_cleanup_called = false;
}

BOOST_AUTO_TEST_CASE(test_tss_does_not_call_cleanup_after_ptr_destroyed)
{
    boost::thread t(thread_with_local_tss_ptr);
    t.join();
    BOOST_CHECK(!tss_cleanup_called);
}

BOOST_AUTO_TEST_CASE(test_tss_cleanup_not_called_for_null_pointer)
{
    boost::sync::thread_specific_ptr<Dummy> local_tss(tss_custom_cleanup);
    local_tss.reset(new Dummy);
    tss_cleanup_called = false;
    local_tss.reset(0);
    BOOST_CHECK(tss_cleanup_called);
    tss_cleanup_called = false;
    local_tss.reset(new Dummy);
    BOOST_CHECK(!tss_cleanup_called);
}

BOOST_AUTO_TEST_CASE(test_tss_at_the_same_adress)
{
    typedef boost::sync::thread_specific_ptr<Dummy> ptr_t;
    enum { size = sizeof(ptr_t) };
    boost::aligned_storage< size > storage;
    ptr_t* ptr = static_cast< ptr_t* >(storage.address());

    // Create the first pointer
    tss_cleanup_called = false;

    new (ptr) ptr_t(tss_custom_cleanup);
    BOOST_CHECK(!tss_cleanup_called);
    BOOST_CHECK(ptr->get() == NULL);

    ptr->reset(new Dummy);
    BOOST_CHECK(!tss_cleanup_called);
    BOOST_CHECK(ptr->get() != NULL);

    ptr->~ptr_t();
    BOOST_CHECK(tss_cleanup_called);

    // Create the second pointer at the same address
    tss_cleanup_called = false;

    new (ptr) ptr_t(tss_custom_cleanup);
    BOOST_CHECK(!tss_cleanup_called);
    BOOST_CHECK(ptr->get() == NULL);

    ptr->~ptr_t();
    BOOST_CHECK(!tss_cleanup_called);
}

max_stars_repo_name: ballisticwhisper/boost

max_issues_repo_name: ballisticwhisper/boost

max_forks_repo_name: ballisticwhisper/boost


text:
//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright David Abrahams, Vicente Botet, Ion Gaztanaga 2009.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/move for documentation.
//
//////////////////////////////////////////////////////////////////////////////
#include <boost/move/move.hpp>
#include "../example/movable.hpp"
#include <boost/static_assert.hpp>

movable function(movable m)
{
   return movable(boost::move(m));
}

movable functionr(BOOST_RV_REF(movable) m)
{
   return movable(boost::move(m));
}

movable function2(movable m)
{
   return boost::move(m);
}

BOOST_RV_REF(movable) function2r(BOOST_RV_REF(movable) m)
{
   return boost::move(m);
}

movable move_return_function2 ()
{
    return movable();
}

movable move_return_function ()
{
    movable m;
    return (boost::move(m));
}

#include <boost/move/move.hpp>
#include "../example/movable.hpp"

//Catch by value
void function_value(movable)
{}

//Catch by reference
void function_ref(const movable &)
{}

//Catch by reference
void function_ref(BOOST_RV_REF(movable))
{}

void limitations_test()
{
   movable m;
   movable cm;

   //move returns rv ref
   BOOST_RV_REF(movable) r = boost::move(m);
   //Conversion operator applied
   BOOST_RV_REF(movable) q = movable();
   //Dangerous for a container if const overload is disabled
   BOOST_RV_REF(movable) s = m;
   (void)r;
   (void)q;
   (void)s;

   //Catch by value, ok
   m = movable();

   //Does not compile
   //_value(m);
   //Compiles
   function_value(movable());

   //Does not compile
   //function_ref(movable());
   //Compiles
   function_ref(boost::move(m));
}

struct copyable
{};

int main()
{
   BOOST_STATIC_ASSERT((boost::has_nothrow_move<movable>::value == true));
   BOOST_STATIC_ASSERT((boost::has_nothrow_move<copyable>::value == false));
   {
      movable m;
      movable m2(boost::move(m));
      movable m3(function(movable(boost::move(m2))));
      movable m4(function(boost::move(m3)));
	}
   {
      movable m;
      movable m2(boost::move(m));
      movable m3(functionr(movable(boost::move(m2))));
      movable m4(functionr(boost::move(m3)));
	}
   {
      movable m;
      movable m2(boost::move(m));
      movable m3(function2(movable(boost::move(m2))));
      movable m4(function2(boost::move(m3)));
	}
   {
      movable m;
      movable m2(boost::move(m));
      movable m3(function2r(movable(boost::move(m2))));
      movable m4(function2r(boost::move(m3)));
	}
   {
      movable m;
      movable m2(boost::move(m));
      movable m3(move_return_function());
	}
   {
      movable m;
      movable m2(boost::move(m));
      movable m3(move_return_function2());
	}
   limitations_test();
   return 0;
}

max_stars_repo_name: ghisguth/tasks

max_issues_repo_name: ghisguth/tasks

max_forks_repo_name: ghisguth/tasks


text:
#pragma once
#ifdef ELLE_WINDOWS
# include <winsock2.h>
#endif
#include <boost/thread.hpp>

namespace elle
{
  namespace reactor
  {
    namespace backend
    {
      namespace pthread
      {
        class Manager;
        class Thread;

        /** Thread is a cooperative thread.
         *
         * A Thread runs a callback in a separate thread of execution in
         * a cooperative way. That is, no code will be executed until
         * someone calls the step() method, at which point only this
         * thread will execute until it calls its yields() method or its
         * main routine ends.
         */
        class Thread
        {
          /*---------.
          | Typedefs |
          `---------*/
          public:
            /// Ourselves.
            using Self = Thread;

          /*-------------.
          | Construction |
          `-------------*/
          public:
            /** Construct a Thread.
             *
             * \param manager The manager.
             * \param name This name for debugging purpose.
             * \param action The main routine this thread will run.
             */
            Thread(Manager& manager,
                   const std::string& name,
                   const Action& action);
            /** Dispose of a thread.
             *
             * A thread may only be destroyed once it finished
             * executing.
             */
            ~Thread();
          private:
            /** Constructor for the first root Thread.
             *
             * This thread represents the main thread - the one that
             * instantiated the manager.
             */
            Thread(Manager& manager);

          /*------.
          | State |
          `------*/
          public:
            /// Pretty name.
            std::string name() const;
            /// Current status.
            Status status() const;
            /// Whether our status is 'done'.
            bool done() const;
          private:
            std::string _name;
            Status _status;

          /*----------.
          | Switching |
          `----------*/
          public:
            /** Start or resume execution.
             *
             * Start execution by running the action or resume it at the
             * point where yield was called. May only be called on a
             * waiting or starting thread. Switch status to
             * running. Make this thread the current thread.
             */
            void step();
            /** Give execution back to our caller.
             *
             * Suspend our execution and give it back to the thread that
             * called our step method. May only be called on the current
             * thread (whose status is thus running). Switch status to
             * waiting. Make the caller the current thread.
             */
            void yield();

          /*--------.
          | Details |
          `--------*/
          private:
            /// Helper to bounce pthreads on Thread::_run.
            friend void* starter(void*);
            /// Let the manager use our private constructor.
            friend class Manager;
            /// Main routine of this thread of execution.
            void _run();
            /// Owning manager.
            Manager& _manager;
            /// Routine callback provided by the user.
            Action _action;
            /// Underlying pthread.
            boost::thread _thread;
            /// The condition used to block/resume execution.
            std::condition_variable _cond;
            /// The mutex protecting the condition.
            std::mutex _mutex;
            /// The thread that stepped us.
            Thread* _caller;
        };

        /** Pool of thread that can switch execution.
         *
         * All thread are affiliated with a Manager, and can only switch
         * execution to threads in the same manager. The context which
         * instantiated the Manager is a valid thread (the root thread).
         */
        class Manager
        {
          /*-------------.
          | Construction |
          `-------------*/
          public:
            /** Construct a manager.
             *
             * Makes the current execution context the current thread.
             */
            Manager();
            /** Dispose of a manager.
             *
             * All managed thread must be disposed of beforehand.
             */
            ~Manager();

          /*------.
          | State |
          `------*/
          public:
            /// The currently running thread.
            Thread* current() const;

          /*--------.
          | Details |
          `--------*/
          private:
            /// Let threads manipulate the current thread and the root thread.
            friend class Thread;
            /// Root thread, which instantiated the Manager.
            Thread _self;
            /// Current thread.
            Thread* _current;
            /// The condition used to block/resume execution.
            std::condition_variable _cond;
            /// The mutex protecting the condition.
            std::mutex _mutex;
        };
      }
    }
  }
}

ext: hh

max_issues_repo_name: mefyl/elle

max_forks_repo_name: mefyl/elle


text:
///
/// if force € [0, FORCE_F_1_2_THRESH] then state 1 : lower cup
/// if force € (FORCE_F_1_2_THRESH, FORCE_F_2_3_THRESH] then state 2 : do nothing
/// if force € (FORCE_F_2_3_THRESH, MAX] then state 3 : raise cup
///
/// Fallback velocity functions
///	Vel.x				                                  Vel.y
/// -180 to -90	  y =  0.004444444*x + 0.8			  -180 to   0	y =  0.004444444*x + 0.4
///  -90 to  90	  y = -0.004444444*x 			           0 to 180	y = -0.004444444*x + 0.4
///   90 to 180	  y =  0.004444444*x - 0.8
///
///
///

#include <ros/console.h>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
#include "mutex"
#include "angles/angles.h"
#include "std_msgs/Int32.h"
#include "audio_emergency/AudioMessage.h"

#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"

#include <hri_package/Sens_Force.h>
#include <kinova_msgs/PoseVelocity.h>

#include <fstream>
#include <vector>
#include <iterator>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>

#define FORCE_F_1_2_THRESH 0.05
#define FORCE_F_2_3_THRESH 0.5
#define NUMBER_OF_ARM_SUB_STATES 10
#define UPPER_FEED_ANGLE_THRESH 180.00 // was 140
#define FORCE_SAFETY 5
#define SUB_SAMPLED_SIZE 10

#define ARC_DOWN 1
#define ARC_UP 2
#define TRANSLATE_BACK 3
#define RAISE_CUP 4
#define LOWER_CUP 5
#define TRANSLATE_UP 6
#define TRANSLATE_DOWN 7
#define TRANSLATE_FRONT 8
#define END_EFF_FRAME "j2s7s300_end_effector"
#define BASE_FRAME "j2s7s300_link_base"
#define SENSOR_FRAME "forcesensor"

#define MAX_STEPS 200
#define VEL_LIN_MAX 0.04
#define VEL_ANG_MAX 0.4
#define VEL_CMD_DURATION 0.8

#define ACTION_DOWN 0
#define ACTION_STAY 1
#define ACTION_UP 2

double lower_angle_thresh = 85;
float thresh_lin = 0.01;
float thresh_ang = 0.05;
double rotation_step;
bool emergency = false;

int step_count;

std::string result;
std::mutex lock_pose;
std::mutex lock_status;
std::mutex lock_proc;
std::mutex lock_force;
std::mutex lock_emerg;
double force_f, force_b;
bool processing=false;
ros::Publisher cmd_pos;
ros::Publisher cmd_vel;
geometry_msgs::PoseStamped current_pose, initial_pose;

void getRPYFromQuaternionMSG(geometry_msgs::Quaternion orientation, double& roll,double& pitch, double& yaw)
{
  tf::Quaternion quat;
  // Suppress a weird warning that tells me to normalise the quats even though I do that.
  if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error) ) ros::console::notifyLoggerLevelsChanged();
  tf::quaternionMsgToTF(orientation,quat);
  if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) ros::console::notifyLoggerLevelsChanged();
  quat.normalize();

  tf::Matrix3x3 mat(quat);

  mat.getRPY(roll, pitch,yaw);

}

void waitForActionCompleted()
{
  std::string temp_res="";
  while(ros::ok())
  {
    ros::spinOnce();
    lock_status.lock();
    temp_res = result;
    lock_status.unlock();

    if (temp_res=="Stopped")
    {
      ROS_INFO_STREAM("Status : " << temp_res);
      //Wait a little extra for arm to settle due to inertia.
      ros::Time start = ros::Time::now();
      while(ros::Time::now()-start < ros::Duration(0.2)) ros::spinOnce();
      break;
    }
  }
}

void setPoseForDirection(int direction, geometry_msgs::PoseStamped& start_pose,double  distance)
{
  geometry_msgs::Quaternion quat;
  switch(direction)
  {
  /// END EFFECTOR FRAME  Z-forward-blue, Y-up-green, X-left-red
  case ARC_DOWN :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(rotation_step),angles::from_degrees(0),angles::from_degrees(0)); //Turn down
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=0.01;  // Slightly up
    start_pose.pose.position.z=-0.01; // Slightly back
    start_pose.pose.orientation = quat;
    break;
  case ARC_UP :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(-rotation_step),angles::from_degrees(0),angles::from_degrees(0));
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=-0.01;
    start_pose.pose.position.z=0.01;
    start_pose.pose.orientation = quat;
    break;
  case TRANSLATE_BACK :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(0),angles::from_degrees(0),angles::from_degrees(0));
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=0;
    start_pose.pose.position.z=-distance;
    start_pose.pose.orientation = quat;
    break;
  case TRANSLATE_FRONT :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(0),angles::from_degrees(0),angles::from_degrees(0));
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=0;
    start_pose.pose.position.z=distance;
    start_pose.pose.orientation = quat;
    break;

  /// SENSORFRAME  X-forward-red, Y-left-green, Z-up-blue
  }
}

geometry_msgs::TwistStamped getTwistForDirection(int direction)
{
  geometry_msgs::TwistStamped twist;
  twist.header.stamp=ros::Time::now();
  twist.header.frame_id="j2s7s300_link_7";
  switch(direction)
  {
    case ARC_DOWN:
      ROS_INFO("Arc down");
      twist.twist.linear.z=VEL_LIN_MAX;
      twist.twist.angular.x=VEL_ANG_MAX;
    break;

    case ARC_UP:
      ROS_INFO("Arc up");
      twist.twist.linear.z=-VEL_LIN_MAX;
      twist.twist.angular.x=-VEL_ANG_MAX;
    break;

    case TRANSLATE_BACK:
      ROS_INFO("Translate back");
      twist.twist.linear.x=-VEL_LIN_MAX;
    break;

    case RAISE_CUP:
      ROS_INFO("Raise Cup");
      twist.twist.angular.x=VEL_ANG_MAX;
    break;

    case LOWER_CUP:
      ROS_INFO("Lower Cup");
      twist.twist.angular.x=-VEL_ANG_MAX;
    break;

    case TRANSLATE_UP:
      ROS_INFO("Translate up");
      twist.twist.linear.z=VEL_LIN_MAX;
    break;

    case TRANSLATE_DOWN:
      ROS_INFO("Translate down");
      twist.twist.linear.z=-VEL_LIN_MAX;
    break;

  }
  return twist;
}

bool checkUpperAngleThreshold();
bool checkLowerAngleThreshold();

void publishTwistForDuration(geometry_msgs::TwistStamped twist_msg, double duration)
{
  ros::Time time_start = ros::Time::now();
  while (ros::Time::now() - time_start < ros::Duration(duration))
  {
    ros::spinOnce();
    cmd_vel.publish(twist_msg);
  }
}

void positionControlDriveForDirection(int direction, double distance)
{
  geometry_msgs::PoseStamped start_pose, pose_in_base;
  tf::TransformListener tf_listener;
  //Wait for TF buffer to fill up
  ros::Duration(0.25).sleep();
  ros::Rate loop_rate(10);

  while(ros::ok())
  {
    try
    {
      lock_pose.lock();
      start_pose = current_pose;
      lock_pose.unlock();

      setPoseForDirection(direction, start_pose, distance);

      tf_listener.transformPose(BASE_FRAME,start_pose,pose_in_base);
      break;

    }
    catch (tf::ExtrapolationException e)
    {
     /* ROS_INFO_STREAM("Waiting for transform "
                      //<< e.what()
                      );*/
    }
    ros::spinOnce();
    loop_rate.sleep();
  }

  cmd_pos.publish(pose_in_base);
}

bool isForceSafe()
{
  double local_force_f;
  lock_force.lock();
  local_force_f=force_f;
  lock_force.unlock();

  if (local_force_f >= FORCE_SAFETY)
  {
    ROS_WARN_STREAM("PAIN THRESHOLD BREACHED!!");
    return false;
  }
  else
    return true;
}

bool isAudioSafe()
{
  lock_emerg.lock();
  bool emerg = emergency;
  lock_emerg.unlock();

  if (emerg) ROS_WARN_STREAM("AUDIO EMERGENCY DETECTED!!");
  return !emerg;
}

void fallback(bool emerg=false);

void driveToRollGoalWithVelocity(int direction)
{
  if(direction == RAISE_CUP)
    ROS_INFO_STREAM("Raise cup");
  else if (direction == LOWER_CUP)
    ROS_INFO_STREAM("Lower cup");
  else
  {
    ROS_ERROR_STREAM("Wrong direction given!!");
    return;
  }

  geometry_msgs::PoseStamped start_pose, goal_pose, temp_pose;

  lock_pose.lock();
  start_pose=current_pose;
  lock_pose.unlock();

  double temp_rol, temp_pit, temp_yaw;
  double goal_rol, goal_pit, goal_yaw;
  double start_rol, start_pit, start_yaw;
  double del_rol;

  goal_pose = start_pose;

  tf::Quaternion q_rot = tf::createQuaternionFromRPY(angles::from_degrees( direction==RAISE_CUP?rotation_step:-rotation_step),angles::from_degrees(0),angles::from_degrees(0)); // Rotate about x by 20 degrees
  tf::Quaternion q_start; tf::quaternionMsgToTF(start_pose.pose.orientation, q_start);
  tf::Quaternion q_goal = q_start*q_rot;

  tf::quaternionTFToMsg(q_goal,goal_pose.pose.orientation);

  ros::Rate loop_rate(100);
  //ros::Time start = ros::Time::now();
  while(ros::ok())
  {
    lock_pose.lock();
    temp_pose = current_pose;
    lock_pose.unlock();

    getRPYFromQuaternionMSG(goal_pose.pose.orientation, goal_rol, goal_pit, goal_yaw);
    getRPYFromQuaternionMSG(temp_pose.pose.orientation, temp_rol, temp_pit, temp_yaw);
    getRPYFromQuaternionMSG(start_pose.pose.orientation, start_rol, start_pit, start_yaw);

    if (goal_rol < 0 ) goal_rol = goal_rol + angles::from_degrees(360);
    if (temp_rol < 0 ) temp_rol = temp_rol + angles::from_degrees(360);

    del_rol = goal_rol - temp_rol;

    if (std::fabs(del_rol)>=thresh_ang)
    {
      float linear_vel = 0.02;
      geometry_msgs::TwistStamped twist_msg;
      twist_msg.twist.linear.x = 0;
      twist_msg.twist.linear.y = 0;
      // Comment out next line ONLY if using end-effector mode ie: Mode 1 on jostick is active and 4th blue LED on far right is active.
//      twist_msg.twist.linear.z = direction==RAISE_CUP ? linear_vel : -linear_vel;
      twist_msg.twist.angular.x= del_rol>0?VEL_ANG_MAX:-VEL_ANG_MAX;
      twist_msg.twist.angular.y = 0;
      twist_msg.twist.angular.z= 0;

      if(isForceSafe() && isAudioSafe())
      {
        cmd_vel.publish(twist_msg);
      }
      else
      {
        ROS_WARN("ABORTING SEQUENCE!!!");
        fallback(true);
        ros::shutdown();
      }
    }
    else
    {
      //ROS_INFO_STREAM("Goal reached, breaking loop");
      break;
    }

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

}

void moveCup(int direction, double duration=VEL_CMD_DURATION, double distance=0.1)
{
  if (direction==TRANSLATE_BACK || direction==TRANSLATE_FRONT)
  {
    positionControlDriveForDirection(direction, distance);
    return;
  }
  else
  {
    geometry_msgs::TwistStamped twist_msg = getTwistForDirection(direction);
    publishTwistForDuration(twist_msg, duration);

  }
  return;
}

void poseGrabber(geometry_msgs::PoseStamped pose)
{
  lock_pose.lock();
  current_pose=pose;
  lock_pose.unlock();
}

void forceGrabber(const hri_package::Sens_Force::ConstPtr msg)
{
  lock_force.lock();
  force_f=msg->forceF;
  force_b=msg->forceB;
  lock_force.unlock();
}

void statusGrabber(std_msgs::String::ConstPtr status)
{
  lock_status.lock();
  result=status->data;
  lock_status.unlock();
}

void waitForPoseDataAvailable()
{
  // This loop to wait until subscriber starts returning valid poses.
  ros::Rate loop_rate(9);
  geometry_msgs::PoseStamped temp_pose;
  while(ros::ok())
  {
    lock_pose.lock();
    temp_pose = current_pose;
    lock_pose.unlock();

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

    if (temp_pose.pose.position.x != 0) break;
  }
  ROS_INFO("Pose data available");
}

bool checkUpperAngleThreshold()
{
  geometry_msgs::PoseStamped temp_pose;

  lock_pose.lock();
  temp_pose=current_pose;
  lock_pose.unlock();

  double temp_roll, temp_pitch, temp_yaw;
  getRPYFromQuaternionMSG(temp_pose.pose.orientation,temp_roll, temp_pitch, temp_yaw);

  if ( angles::to_degrees(temp_roll) > UPPER_FEED_ANGLE_THRESH )
  {
    ROS_WARN_STREAM("MAX UPPER FEED ANGLE REACHED");
    return false;
  }
  else
  {
   return true;
  }
}

bool checkLowerAngleThreshold()
{
  geometry_msgs::PoseStamped temp_pose;

  lock_pose.lock();
  temp_pose=current_pose;
  lock_pose.unlock();

  double temp_roll, temp_pitch, temp_yaw;
  getRPYFromQuaternionMSG(temp_pose.pose.orientation,temp_roll, temp_pitch, temp_yaw);

  if ((int)angles::to_degrees(temp_roll) - (int)angles::to_degrees(lower_angle_thresh) < rotation_step/2 && temp_roll > 0)
  {
    //ROS_WARN_STREAM("MAX LOWER ANGLE REACHED. LIMIT: " << angles::to_degrees(lower_angle_thresh) << " CURRENT: " << angles::to_degrees(temp_roll));
    return false;
  }
  else
  {
//   ROS_INFO_STREAM("Current roll: " << sangles::to_degrees(temp_roll));
   return true;
  }
}

double getCurrentRoll()
{
  lock_pose.lock();
  geometry_msgs::PoseStamped temp_pose = current_pose;
  lock_pose.unlock();

  double temp_rol, temp_pitch, temp_yaw;
  getRPYFromQuaternionMSG(temp_pose.pose.orientation, temp_rol, temp_pitch, temp_yaw);

  return temp_rol<0?temp_rol+angles::from_degrees(360):temp_rol;
}

void callFallbackTimer(double duration)
{

  ros::Time start = ros::Time::now();
  ros::Duration run_for(duration);
  ros::Rate loop_rate(10);
  ROS_WARN_STREAM("Starting fallback timeout for: " << duration << "s.");
  while(ros::Time::now() - start <= run_for)
  {
    lock_force.lock();
    double force = force_f;
    lock_force.unlock();

    if (force>= FORCE_F_2_3_THRESH)
    {
      ROS_INFO_STREAM("Force detected, breaking out of timer");
      return;
    }

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

  ROS_WARN_STREAM("Time out! Initialising fallback");
 // moveCup(TRANSLATE_BACK);
  fallback();
  sleep(1);
  ros::shutdown();
}

void fallback(bool emerg)
{
  lock_pose.lock();
  geometry_msgs::PoseStamped  start_pose=current_pose;
  lock_pose.unlock();

  double roll, pitch, yaw;
  getRPYFromQuaternionMSG(start_pose.pose.orientation, roll, pitch, yaw);

  int yaw_degrees = angles::to_degrees(yaw);

  // ROS_INFO_STREAM("Current yaw float: " << angles::to_degrees(yaw) << " int " << yaw_degrees);

  geometry_msgs::TwistStamped twist_cmd;

  // y = mx + c
  double m = 0.004444444;
  double velx_c = 0.8;
  double vely_c = 0.4;

  /// Fallback velocity functions
  ///	Vel.x				                                  Vel.y
  /// -180 to -90	  y =  0.004444444*x + 0.8			  -180 to   0	y =  0.004444444*x + 0.4
  ///  -90 to  90	  y = -0.004444444*x 			           0 to 180	y = -0.004444444*x + 0.4
  ///   90 to 180	  y =  0.004444444*x - 0.8

  if (yaw_degrees >= -180 && yaw_degrees < -90)
  {
    //ROS_INFO_STREAM("Sector 1");
    twist_cmd.twist.linear.x = m * yaw_degrees + velx_c;
    twist_cmd.twist.linear.y = m * yaw_degrees + vely_c;
  }
  else if (yaw_degrees >= -90 &&  yaw_degrees <0)
  {
    //ROS_INFO_STREAM("Sector 2");
    twist_cmd.twist.linear.x = -m * yaw_degrees;
    twist_cmd.twist.linear.y = m * yaw_degrees + vely_c;
  }
  else if (yaw_degrees >= 0 &&  yaw_degrees <90)
  {
    //ROS_INFO_STREAM("Sector 3");
    twist_cmd.twist.linear.x = -m * yaw_degrees;
    twist_cmd.twist.linear.y = -m * yaw_degrees + vely_c;
  }
  else if (yaw_degrees >= 90 &&  yaw_degrees <=180)
  {
    //ROS_INFO_STREAM("Sector 4");
    twist_cmd.twist.linear.x = m * yaw_degrees - velx_c;
    twist_cmd.twist.linear.y = -m * yaw_degrees + vely_c;
  }
  //ROS_INFO_STREAM("Publishing twist : ");
  //std::cout << (twist_cmd) << std::endl;
  if (emerg==true) twist_cmd.twist.angular.x = -1;
  publishTwistForDuration(twist_cmd,1);
}

class CSVReader
{
    std::string fileName;
    std::string delimeter;

public:
    CSVReader(std::string filename, std::string delm = ",") :
        fileName(filename), delimeter(delm)
    { }

    // Function to fetch data from a CSV File
    std::vector<std::vector<std::string> > getData();
};

std::vector<std::vector<std::string> > CSVReader::getData()
{
    std::ifstream file(fileName);

    std::vector<std::vector<std::string> > dataList;

    std::string line = "";
    // Iterate through each line and split the content using delimeter
    while (getline(file, line))
    {
        std::vector<std::string> vec;
        boost::algorithm::split(vec, line, boost::is_any_of(delimeter));
        dataList.push_back(vec);
    }
    // Close the File
    file.close();

    return dataList;
}

std::vector<std::vector<float>> getGMRFromCsv(std::string file)
{

   CSVReader reader(file);

   std::vector<std::vector<std::string> > dataList = reader.getData();

   int RR = dataList.size();
   int CC = dataList[0].size();
    std::vector<std::vector<float>> gmr(RR);
    for ( int i = 0 ; i < RR ; i++ )
       gmr[i].resize(CC);

  ROS_INFO_STREAM("Fetching GMR data from " << file );
  for (int i=0; i<dataList.size(); i++)
   {
     std::vector<std::string> vec = dataList[i];
     for(int j=0; j< vec.size();j++ )
     {
       gmr[i][j]= std::stof(vec[j]);
     }
   }
    return gmr;
}

/**
 * @brief Calculate the delta pose vector i.e.: the difference of each pose from the first pose
 * @param gmr
 * @return pose_delta
 */
std::vector<std::vector<float>> getPoseDeltaFromGMR(std::vector<std::vector<float>> gmr)
{

   int RR = gmr.size();
   int CC = 6; // 6 columns for deltas in x,y,z,roll,pitch,yaw
   std::vector<std::vector<float>> pose_delta(RR);
   for ( int i = 0 ; i < RR ; i++ )
      pose_delta[i].resize(CC);

   geometry_msgs::PoseStamped first_pose;
   first_pose.pose.position.x = gmr[0][1]; //columen 0 is index number, dropping it
   first_pose.pose.position.y = gmr[0][2];
   first_pose.pose.position.z = gmr[0][3];
   first_pose.pose.orientation.x = gmr[0][4];
   first_pose.pose.orientation.y = gmr[0][5];
   first_pose.pose.orientation.z = gmr[0][6];
   first_pose.pose.orientation.w = gmr[0][7];
   double roll_first, pitch_first, yaw_first;
   getRPYFromQuaternionMSG(first_pose.pose.orientation, roll_first, pitch_first, yaw_first);

// First row/pose is always 0 since it is the reference point.
   for (int i=0; i<RR; i++)
   {
     geometry_msgs::PoseStamped current_pose;
     current_pose.pose.position.x = gmr[i][1];  //columen 0 is index number, dropping it
     current_pose.pose.position.y = gmr[i][2];
     current_pose.pose.position.z = gmr[i][3];
     current_pose.pose.orientation.x = gmr[i][4];
     current_pose.pose.orientation.y = gmr[i][5];
     current_pose.pose.orientation.z = gmr[i][6];
     current_pose.pose.orientation.w = gmr[i][7];

     double roll_curr, pitch_curr, yaw_curr;

     getRPYFromQuaternionMSG(current_pose.pose.orientation,roll_curr, pitch_curr, yaw_curr);

     double delta_roll, delta_pitch, delta_yaw;
     delta_roll =  roll_curr - roll_first ;
     delta_pitch = pitch_curr - pitch_first ;
     delta_yaw = yaw_curr - yaw_first;

     pose_delta[i][0] = current_pose.pose.position.x - first_pose.pose.position.x;
     pose_delta[i][1] = current_pose.pose.position.y - first_pose.pose.position.y;
     pose_delta[i][2] = current_pose.pose.position.z - first_pose.pose.position.z;
     pose_delta[i][3] = delta_roll; // Forward feeding approach changes only roll. Change in pitch and yaw <1° and hence ignored. Direct addition of single euler angle works between quaternions. NOT multiple angles.
//     pose_delta[i][4] = delta_pitch;    // Even if there are pitches and yaws due to human error, they need to be suppressed to avoid_discomfort to the user/spillage_.
//     pose_delta[i][5] = delta_yaw;
   }

   return pose_delta;
}

// Take the actual starting pose from current run, consider the index of the GMR trajectory required, add the delta to take the starting pose to new index
geometry_msgs::PoseStamped getNewPoseAtIndex(geometry_msgs::PoseStamped starting_pose, int new_index_in_trajectory, std::vector<std::vector<float>> pose_delta_subsampled )
{
  geometry_msgs::PoseStamped new_pose;
  new_pose.pose.position.x = starting_pose.pose.position.x + pose_delta_subsampled[new_index_in_trajectory][0];
  new_pose.pose.position.y = starting_pose.pose.position.y + pose_delta_subsampled[new_index_in_trajectory][1];
  new_pose.pose.position.z = starting_pose.pose.position.z + pose_delta_subsampled[new_index_in_trajectory][2];

  double starting_roll, starting_pitch, starting_yaw, new_roll, new_pitch, new_yaw;
  getRPYFromQuaternionMSG(starting_pose.pose.orientation, starting_roll, starting_pitch, starting_yaw);
  new_roll = starting_roll + pose_delta_subsampled[new_index_in_trajectory][3];
  new_pitch = starting_pitch + pose_delta_subsampled[new_index_in_trajectory][4];
  new_yaw = starting_yaw+ pose_delta_subsampled[new_index_in_trajectory][5];

  tf::Quaternion q_new = tf::createQuaternionFromRPY(new_roll, new_pitch, new_yaw);
  q_new.normalize();
  tf::quaternionTFToMsg(q_new,new_pose.pose.orientation);

  return new_pose;
}

void audioGrabber(audio_emergency::AudioMessage::ConstPtr msg)
{
  lock_emerg.lock();
  if (msg->result == "talk") emergency = true;
  lock_emerg.unlock();
}

void safePauseFor(float duration)
{
  ros::Time start = ros::Time::now();
  ros::Rate loop_rate(10);
  while(ros::Time::now()-start < ros::Duration(duration))
  {
    ros::spinOnce();
    if (!isAudioSafe() || !isForceSafe())
    {
      ROS_WARN_STREAM("ABORTING SEQUENCE!!");
      fallback(true);
      ros::shutdown();
    }
    loop_rate.sleep();
  }
}

void printPose(geometry_msgs::PoseStamped pose, std::string text)
{
  double roll, pitch, yaw;
  getRPYFromQuaternionMSG(pose.pose.orientation, roll, pitch, yaw);
  ROS_INFO_STREAM(text << pose.pose.position.x << " "
                  << pose.pose.position.y << " "
                  << pose.pose.position.z << " "
                  << angles::to_degrees(roll) << " "
                  << angles::to_degrees(pitch) << " "
                  << angles::to_degrees(yaw) << " " );
}

void printGMRRow(std::vector<std::vector<float>> gmr, int row, std::string text)
{

    double roll, pitch, yaw;
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = gmr[row][1];
    pose.pose.position.y = gmr[row][2];
    pose.pose.position.z = gmr[row][3];
    pose.pose.orientation.x = gmr[row][4];
    pose.pose.orientation.y = gmr[row][5];
    pose.pose.orientation.z = gmr[row][6];
    pose.pose.orientation.w = gmr[row][7];

    getRPYFromQuaternionMSG(pose.pose.orientation, roll, pitch, yaw);

    ROS_INFO_STREAM(text << pose.pose.position.x << " "
                    << pose.pose.position.y << " "
                    << pose.pose.position.z << " "
                    << angles::to_degrees(roll) << " "
                    << angles::to_degrees(pitch) << " "
                    << angles::to_degrees(yaw) << " " );

}

std::vector<std::vector<float>> subSamplePoseDelta(std::vector<std::vector<float>> pose_delta, int resultant_length)
{
  int sampling_freq = pose_delta.size()/resultant_length;
  std::vector<std::vector<float>> pose_delta_subsampled(resultant_length);
  // Resize the subsampled set to have the number of rows as required by resultant length but number of columns same as pose_delta
  for(int i=0; i< resultant_length; i++)
  {
    pose_delta_subsampled[i].resize(pose_delta[0].size());
  }

  // Subsample pose_delta according to sampling frequency
  for (int i=0; i<resultant_length; i++)
  {
    pose_delta_subsampled[i] = pose_delta[i*sampling_freq];
  }

  return pose_delta_subsampled;
}

void driveToPoseStep(int step_count,std::vector<std::vector<float>> pose_delta_subsampled )
{
  ros::spinOnce();

  lock_pose.lock();
  geometry_msgs::PoseStamped temp_pose=current_pose;
  lock_pose.unlock();

  ROS_INFO_STREAM("Moving to WP" << step_count);
  geometry_msgs::PoseStamped pose_new = getNewPoseAtIndex(initial_pose, step_count, pose_delta_subsampled);
  printPose(temp_pose , "Cur pose: ");
  printPose(pose_new, "New pose: ");
  cmd_pos.publish(pose_new);
  waitForActionCompleted();
  ROS_INFO_STREAM("Done");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "feeder_gmm");
  ros::NodeHandle nh;

  std::vector<std::vector<float>> gmr = getGMRFromCsv(argv[1]);
  std::vector<std::vector<float>> pose_delta = getPoseDeltaFromGMR(gmr);
  std::vector<std::vector<float>> pose_delta_subsampled = subSamplePoseDelta(pose_delta, SUB_SAMPLED_SIZE);

  ROS_INFO_STREAM("Size of GMR : " << gmr.size() << " pose_delta : " << pose_delta.size() << " pose_delta_subsampled : " << pose_delta_subsampled.size());

  tf::TransformListener tf_listener;

  ros::Subscriber sub = nh.subscribe("/force_values", 1000, forceGrabber);
  cmd_vel = nh.advertise<geometry_msgs::TwistStamped>("/RobotControl/VelocityControl", 1000);
  cmd_pos = nh.advertise<geometry_msgs::PoseStamped>("/RobotControl/PoseControl", 1000);
  ros::Subscriber tool_pose = nh.subscribe("/j2s7s300_driver/out/tool_pose",1000, poseGrabber );
  ros::Subscriber control_status = nh.subscribe("/RobotControl/Status",1000, statusGrabber );
  ros::Publisher arm_pose_pub = nh.advertise<std_msgs::Int32>("/arm_state", 1000);
  ros::Subscriber audio_emerg = nh.subscribe("/audio_emergency",1000, audioGrabber );

  waitForPoseDataAvailable();

  double local_force_f;
  bool print_once_only=true;

  lock_pose.lock();
  initial_pose=current_pose;
  lock_pose.unlock();

 //TODO : Implement the driver below

  step_count = 0;
  int prev_step_count = 0;

  ros::Rate loop_rate(10);
  while(ros::ok())
  {
    ros::spinOnce();

    lock_force.lock();
    local_force_f=force_f;
    lock_force.unlock();

    if (local_force_f >= 0  && local_force_f <= FORCE_F_1_2_THRESH && step_count >0)
    {
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO_STREAM("STEP_DOWN for " << local_force_f << "N from step " << step_count);
      prev_step_count = step_count--;
      driveToPoseStep(step_count,pose_delta_subsampled);
      print_once_only=true;
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO(" ");

      if (prev_step_count > step_count && step_count==0)
      {
        callFallbackTimer(3);
      }
    }

    else if (local_force_f >= FORCE_F_2_3_THRESH && local_force_f <= FORCE_SAFETY && step_count < SUB_SAMPLED_SIZE-1)
    {
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO_STREAM("STEP_UP for " << local_force_f << "N from step " << step_count);
      prev_step_count =  step_count++;
      driveToPoseStep(step_count,pose_delta_subsampled);
      print_once_only=true;
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO(" ");
    }

    else if (print_once_only)
    {
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO_STREAM("Do nothing.");
      print_once_only=false;
      ROS_WARN_STREAM("Step : " << prev_step_count << " -> " << step_count  << " @ roll : " << angles::to_degrees(getCurrentRoll()));
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO(" ");

    }

    loop_rate.sleep();
    std_msgs::Int32 arm_pose_msg;
    arm_pose_msg.data=step_count;
    arm_pose_pub.publish(arm_pose_msg);
    ros::spinOnce();

    if (!isAudioSafe())
    {
      ROS_WARN_STREAM("ABORTING SEQUENCE!!");
      fallback(true);
      ros::shutdown();
    }
  }
  return 0;
}

max_stars_repo_name: Tejas-Shastha/tejas

max_issues_repo_name: Tejas-Shastha/tejas

max_forks_repo_name: Tejas-Shastha/tejas


text:
// Boost.Signals2 library

// Copyright Frank Mori Hess 2007-2008.
// Copyright Timmo Stange 2007.
// Copyright Douglas Gregor 2001-2004. Use, modification and
// distribution is subject to the Boost Software License, Version
// 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// For more information, see http://www.boost.org

#ifndef BOOST_SIGNALS2_SLOT_BASE_HPP
#define BOOST_SIGNALS2_SLOT_BASE_HPP

#include <boost/any.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/signals2/detail/foreign_ptr.hpp>
#include <boost/signals2/expired_slot.hpp>
#include <boost/signals2/signal_base.hpp>
#include <boost/throw_exception.hpp>
#include <boost/mpl/not.hpp> // work around missing include in Boost.Variant headers
#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/variant.hpp>
#include <vector>

namespace boost
{
  namespace signals2
  {
    namespace detail
    {
      class tracked_objects_visitor;

      typedef boost::variant<boost::weak_ptr<void>, detail::foreign_void_weak_ptr > void_weak_ptr_variant;
      typedef boost::variant<boost::shared_ptr<void>, detail::foreign_void_shared_ptr > void_shared_ptr_variant;
      class lock_weak_ptr_visitor
      {
      public:
        typedef void_shared_ptr_variant result_type;
        template<typename WeakPtr>
        result_type operator()(const WeakPtr &wp) const
        {
          return wp.lock();
        }
      };
      class expired_weak_ptr_visitor
      {
      public:
        typedef bool result_type;
        template<typename WeakPtr>
        bool operator()(const WeakPtr &wp) const
        {
          return wp.expired();
        }
      };
    }

    class slot_base
    {
    public:
      typedef std::vector<detail::void_weak_ptr_variant> tracked_container_type;
      typedef std::vector<detail::void_shared_ptr_variant> locked_container_type;

      const tracked_container_type& tracked_objects() const {return _tracked_objects;}
      locked_container_type lock() const
      {
        locked_container_type locked_objects;
        tracked_container_type::const_iterator it;
        for(it = tracked_objects().begin(); it != tracked_objects().end(); ++it)
        {
          locked_objects.push_back(apply_visitor(detail::lock_weak_ptr_visitor(), *it));
          if(apply_visitor(detail::expired_weak_ptr_visitor(), *it))
          {
            boost::throw_exception(expired_slot());
          }
        }
        return locked_objects;
      }
      bool expired() const
      {
        tracked_container_type::const_iterator it;
        for(it = tracked_objects().begin(); it != tracked_objects().end(); ++it)
        {
          if(apply_visitor(detail::expired_weak_ptr_visitor(), *it)) return true;
        }
        return false;
      }
    protected:
      friend class detail::tracked_objects_visitor;

      void track_signal(const signal_base &signal)
      {
        _tracked_objects.push_back(signal.lock_pimpl());
      }

      tracked_container_type _tracked_objects;
    };
  }
} // end namespace boost

#endif // BOOST_SIGNALS2_SLOT_BASE_HPP

ext: hpp

max_stars_repo_name: jonstewart/boost-svn

max_issues_repo_name: jonstewart/boost-svn

max_forks_repo_name: jonstewart/boost-svn


text:
#ifndef STAN_MATH_PRIM_SCAL_FUN_ROUND_HPP
#define STAN_MATH_PRIM_SCAL_FUN_ROUND_HPP

#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <stan/math/prim/scal/fun/is_nan.hpp>
#include <boost/math/special_functions/round.hpp>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the closest integer to the specified argument, with
 * halfway cases rounded away from zero.
 *
 * @param x Argument.
 * @return The rounded value of the argument.
 */
inline double round(double x) {
  if (is_nan(x))
    return std::numeric_limits<double>::quiet_NaN();
  return boost::math::round(x, boost_policy_t());
}

/**
 * Return the closest integer to the specified argument, with
 * halfway cases rounded away from zero.
 *
 * @param x Argument.
 * @return The rounded value of the argument.
 */
inline double round(int x) { return round(static_cast<double>(x)); }

}  // namespace math
}  // namespace stan
#endif

max_stars_repo_name: sakrejda/math

max_issues_repo_name: sakrejda/math

max_forks_repo_name: sakrejda/math


text:
/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

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

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>

#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <cmath>
#include <ctime>
#include <time.h>

#include "pseudo_inverse.h"

using namespace std;
using namespace Eigen;
using namespace KDL;

// take it from yaml file
int n_rows;
int n_cols;
int quality_index;
string relative_path_file;
string file_name;
std::vector<double> Quality;
int n_c_max = 19; // number of contact point, assumed that 1 contact point for each phalanx

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

	ros::init(argc, argv, "Hand_Jacobian");	// ROS node
	ros::NodeHandle nh;

  nh.param<int>("n_rows_file",n_rows,108);
  nh.param<int>("n_cols_file",n_cols,86);
  nh.param<int>("quality_index",quality_index,0);
  nh.param<std::string>("file_name", relative_path_file, "/db/box_db_2.csv" );

  std::string path = ros::package::getPath("grasp_learning");
  file_name = path + relative_path_file ;
  ifstream file(file_name);

  std::cout << "file: " << file_name.c_str() << " is " << (file.is_open() == true ? "already" : "not") << " open" << std::endl;
  if(!file.is_open())
  return 0;

  ////////////////////////////////////////////////////////////////////////////////////
	 /////////////////////////////////////////////////////////////////////////

  //	TAKE DATA_BASE : load a file .csv and put the values into Eigen::MatrixXd

  ////////////////////////////////////////////////////////////////////////////////////
	 /////////////////////////////////////////////////////////////////////////

  int count_row = 0;
  int count_col = 0;

  bool count_cols_only_one = true;

  for(std::string line; getline ( file, line, '\n' ); ) // ciclo sulla riga
  {
    count_row++;
    std::istringstream iss_line(line);

    if(count_cols_only_one)
    {

      for(std::string value; getline(iss_line, value, ',' ); )
          count_col++;

      count_cols_only_one = false;
    }
  }

  cout << "ROWS : " << count_row << endl; // 40
  cout << "COLS : " << count_col << endl; // 189

  Eigen::MatrixXd data_set(count_row,count_col);

	int i = 0;
	int j = 0;

	for(std::string line; getline( file, line, '\n' ); ) // ciclo sulla riga
	{

    std::istringstream iss_line(line);
    for(std::string value; getline(iss_line, value, ',' ); )
    {
    	data_set(i,j) = stod(value);
    	j++;
    }

    //cout << endl;
    j=0;
    i++;
	}

	file.close();

//	cout << " DATA_SET" << data_set << endl;

///////////////////////////// 		END		///////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

//								TAKE SOFT_HAND

////////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

  KDL::Tree hand_tree;

  std::string robot_desc_string;
  nh.param("robot_description", robot_desc_string, string());  // robot description is the name in the launch file
  if (!kdl_parser::treeFromString(robot_desc_string, hand_tree))
  { ROS_ERROR("Failed to construct kdl tree"); return false;}

  KDL::Chain chains_hand_finger[5];
  KDL::Jacobian hand_jacob[5];

  //std::string root_name = "right_hand_softhand_base";
  //std::string root_name = "right_hand_palm_link";
  std::string root_name = "world";
  std::string end_chain_name[5];
  end_chain_name[0] = "right_hand_thumb_distal_link";
  end_chain_name[1] = "right_hand_index_distal_link";
  end_chain_name[2] = "right_hand_middle_distal_link";
  end_chain_name[3] = "right_hand_ring_distal_link";
  end_chain_name[4] = "right_hand_little_distal_link";

  hand_tree.getChain(root_name, end_chain_name[0], chains_hand_finger[0]);      //thumb
  hand_tree.getChain(root_name, end_chain_name[1], chains_hand_finger[1]);      //index
  hand_tree.getChain(root_name, end_chain_name[2], chains_hand_finger[2]);      //middle
  hand_tree.getChain(root_name, end_chain_name[3], chains_hand_finger[3]);      //ring
  hand_tree.getChain(root_name, end_chain_name[4], chains_hand_finger[4]);      //little

  int nq_hand = hand_tree.getNrOfJoints();  // 34
  int ns_hand = hand_tree.getNrOfSegments(); // 39

  cout << "number_of_joint_in_hand : " << nq_hand << endl;
  cout << "number_of_segment_in_hand : " << ns_hand << endl;

///////////////////////////// 		END		  ///////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

//			CALCULATE THE GRASP_MATRIX AND HAND_JACOBIAN FOR EACH ROW
//	I supposed to have 19 joint variables and a single point of contact for each phalanx

////////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

	unsigned int nj_0 = chains_hand_finger[0].getNrOfJoints();  // 5
  unsigned int nj_1 = chains_hand_finger[1].getNrOfJoints();  // 7
  unsigned int nj_2 = chains_hand_finger[2].getNrOfJoints();  // 7
  unsigned int nj_3 = chains_hand_finger[3].getNrOfJoints();  // 7
  unsigned int nj_4 = chains_hand_finger[4].getNrOfJoints();  // 7

  KDL::JntArray q_thumb = JntArray(nj_0);    // thumb
  KDL::JntArray q_index = JntArray(nj_1);    // forefinger
  KDL::JntArray q_middle = JntArray(nj_2);   // middlefinger
  KDL::JntArray q_ring = JntArray(nj_3);     // ringfinger
  KDL::JntArray q_little = JntArray(nj_4);   // littlefinger

  cout << "nj_0 : " << nj_0 << endl;
  cout << "nj_1 : " << nj_1 << endl;
  cout << "nj_2 : " << nj_2 << endl;
  cout << "nj_3 : " << nj_3 << endl;
  cout << "nj_4 : " << nj_4 << endl;

  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_0;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_1;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_2;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_3;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_4;

  // constructs the kdl solvers in non-realtime
  jnt_to_jac_solver_0.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[0]));
  jnt_to_jac_solver_1.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[1]));
  jnt_to_jac_solver_2.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[2]));
  jnt_to_jac_solver_3.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[3]));
  jnt_to_jac_solver_4.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[4]));

  // resizes the joint state vectors in non-realtime
  q_thumb.resize(chains_hand_finger[0].getNrOfJoints());
  q_index.resize(chains_hand_finger[1].getNrOfJoints());
  q_middle.resize(chains_hand_finger[2].getNrOfJoints());
  q_ring.resize(chains_hand_finger[3].getNrOfJoints());
  q_little.resize(chains_hand_finger[4].getNrOfJoints());

  hand_jacob[0].resize(chains_hand_finger[0].getNrOfJoints());
  hand_jacob[1].resize(chains_hand_finger[1].getNrOfJoints());
  hand_jacob[2].resize(chains_hand_finger[2].getNrOfJoints());
 	hand_jacob[3].resize(chains_hand_finger[3].getNrOfJoints());
  hand_jacob[4].resize(chains_hand_finger[4].getNrOfJoints());

	for(int r = 0; r < n_rows ; r++)  // for each row calculation the grasp matrix and hand jacobian
	{
		//take the value of joints from data
		// initialize jntarray then be able to calculate the Jacobian to that point of contact

		q_thumb(0) = data_set(r,10);

		q_thumb(1) = data_set(r,11) / 2;
		q_thumb(2) = data_set(r,11) / 2;

		q_thumb(3) = data_set(r,12) / 2;
		q_thumb(4) = data_set(r,12) / 2;

		q_index(0) = data_set(r,13);

		q_index(1) = data_set(r,14) / 2;
		q_index(2) = data_set(r,14) / 2;

		q_index(3) = data_set(r,15) / 2;
		q_index(4) = data_set(r,15) / 2;

		q_index(5) = data_set(r,16) / 2;
		q_index(6) = data_set(r,16) / 2;

		q_middle(0) = data_set(r,17);

		q_middle(1) = data_set(r,18) / 2;
		q_middle(2) = data_set(r,18) / 2;

		q_middle(3) = data_set(r,19) / 2;
		q_middle(4) = data_set(r,19) / 2;

		q_middle(5) = data_set(r,20) / 2;
		q_middle(6) = data_set(r,20) / 2;

		q_ring(0) = data_set(r,21);

		q_ring(1) = data_set(r,22) / 2;
		q_ring(2) = data_set(r,22) / 2;

		q_ring(3) = data_set(r,23) / 2;
		q_ring(4) = data_set(r,23) / 2;

		q_ring(5) = data_set(r,24) / 2;
		q_ring(6) = data_set(r,24) / 2;

		q_little(0) = data_set(r,25);

		q_little(1) = data_set(r,26) / 2;
		q_little(2) = data_set(r,26) / 2;

		q_little(3) = data_set(r,27) / 2;
		q_little(4) = data_set(r,27) / 2;

		q_little(5) = data_set(r,28) / 2;
		q_little(6) = data_set(r,28) / 2;

		Eigen::MatrixXd Contacts(n_c_max,3); // I build a matrix of contact points for convenience only
										 // each row is a contact point
										 // row 0, 1, 2 for thumb
										 // row 3, 4, 5, 6 for index
										 // row 7, 8, 9, 10 for middle
										 // row 11, 12, 13, 14 for ring
										 // row 15, 16, 17, 18 for little
										 // n_c = 19 ( number of contact)

	 int start = 29; // the first value for the position of the joint variables
	 int gap_point_coordinate = 0;

		for(int i = 0; i < n_c_max; i++)
		{
			for(int j = 0; j < 3; j++ )
				Contacts(i,j) = data_set(r, start + gap_point_coordinate + j);  // check if it is correct

			gap_point_coordinate += 3;
		}

    cout << "Contacts : " << Contacts << endl;

    //for each contact point
		std::vector<Eigen::MatrixXd> Grasp_Matrix_ ;
    std::vector<Eigen::MatrixXd> Hand_Jacobian_ ;

    int k = 1;

    for(int i = 0 ; i < n_c_max ; i++) //calc the grasp_matrix and hand_jacobian for each contact point
    {

      if(Contacts(i,0) != 9999) //9999 similar to NaN in dataset
      {

        Eigen::MatrixXd Grasp_Matrix(6,6);
 				Eigen::MatrixXd Skew_Matrix(3,3);
  			Eigen::MatrixXd Rotation(3,3);

  			Rotation <<  MatrixXd::Identity(3,3);

        //check if the values of the skew matrix are expressed in the correct reference system
      	Skew_Matrix(0,0) = Skew_Matrix(1,1) = Skew_Matrix(2,2) = 0;
        Skew_Matrix(0,1) = - Contacts(i,2); // -rz
        Skew_Matrix(0,2) = Contacts(i,1);   // ry
        Skew_Matrix(1,0) = Contacts(i,2);   // rz
        Skew_Matrix(2,0) = - Contacts(i,1); // -ry
        Skew_Matrix(1,2) = - Contacts(i,0); // -rx
        Skew_Matrix(2,1) = Contacts(i,0);   // rx

        Grasp_Matrix.block<3,3>(0,0) = Rotation;
        Grasp_Matrix.block<3,3>(3,3) = Rotation;
        Grasp_Matrix.block<3,3>(0,3) = Skew_Matrix * Rotation;
        Grasp_Matrix.block<3,3>(3,0) = MatrixXd::Zero(3,3);

        cout << "Grasp_Matrix" << endl;
        cout << Grasp_Matrix << endl;

        Grasp_Matrix_.push_back(Grasp_Matrix);

        jnt_to_jac_solver_0->JntToJac(q_thumb, hand_jacob[0], 0);
      	jnt_to_jac_solver_1->JntToJac(q_index, hand_jacob[1], 0);
      	jnt_to_jac_solver_2->JntToJac(q_middle, hand_jacob[2], 0);
      	jnt_to_jac_solver_3->JntToJac(q_ring, hand_jacob[3], 0);
   			jnt_to_jac_solver_4->JntToJac(q_little, hand_jacob[4], 0);

   			cout << "Hand_Jacobian_ THUMB" << endl;
      	cout <<  hand_jacob[0].data << endl;
      	cout << "Hand_Jacobian_ INDEX" << endl;
    		cout <<  hand_jacob[1].data << endl;
  			cout << "Hand_Jacobian_ MIDDLE"<< endl;
   			cout <<  hand_jacob[2].data << endl;
      	cout << "Hand_Jacobian_ RING"  << endl;
    		cout <<  hand_jacob[3].data << endl;
  			cout << "Hand_Jacobian_ LITTLE"<< endl;
  			cout <<  hand_jacob[4].data << endl;

      	int which_finger = 0;
        int which_falange = 0;

        if((0 <= i) && (i <= 2)){ which_finger = 0; which_falange = i+k; k++; if(i==2) k = 1;} // control flag which_falange there may be some error
        if((3 <= i) && (i <= 6)){ which_finger = 1; which_falange = i-3+k; k++; if(i==6) k = 1;}
        if((7 <= i) && (i <= 10)){ which_finger = 2; which_falange = i-7+k; k++; if(i==10) k = 1;}
        if((11 <= i) && (i <= 14)){ which_finger = 3; which_falange = i-11+k; k++; if(i==14) k = 1;}
        if((15 <= i) && (i <= 18)){ which_finger = 4; which_falange = i-15+k; k++; if(i==18) k = 1;}

        cout << "i : " << i << endl;
        cout << "which_finger : " << which_finger << endl;
        cout << "which_falange : " << which_falange << endl;

        switch(which_finger)
        {
          case 0: // thumb
      						jnt_to_jac_solver_0->JntToJac(q_thumb, hand_jacob[0], which_falange);
      						break;

	  			case 1: // index
      						jnt_to_jac_solver_1->JntToJac(q_index, hand_jacob[1], which_falange);
      						break;

	  			case 2: // middle
							jnt_to_jac_solver_2->JntToJac(q_middle, hand_jacob[2], which_falange);
      						break;

	  			case 3: // ring
      						jnt_to_jac_solver_3->JntToJac(q_ring, hand_jacob[3], which_falange);
	  						break;

	  			case 4: // little
      						jnt_to_jac_solver_4->JntToJac(q_little, hand_jacob[4], which_falange);
	  						break;
	  		}// end switch

	  		cout << " Jacobian thumb " << hand_jacob[0].data << endl ;
	  		cout << " Jacobian index " << hand_jacob[1].data << endl ;
	  	  cout << " Jacobian middle " << hand_jacob[2].data << endl ;
			  cout << " Jacobian ring " << hand_jacob[3].data << endl ;
			  cout << " Jacobian little " << hand_jacob[4].data << endl ;

	  		Hand_Jacobian_.push_back(hand_jacob[0].data); // 5
	  		Hand_Jacobian_.push_back(hand_jacob[1].data); // 7
	 		  Hand_Jacobian_.push_back(hand_jacob[2].data); // 7
	  		Hand_Jacobian_.push_back(hand_jacob[3].data); // 7
	  		Hand_Jacobian_.push_back(hand_jacob[4].data); // 7

      }// end if  contact

      }	// end for contact

      cout << " Dim of the vectors of the matrices grasp " << Grasp_Matrix_.size() << endl;
      cout << " Dim of the hand jacobian " << Hand_Jacobian_.size() << endl;

      int n_c_eff = Grasp_Matrix_.size();

      Eigen::MatrixXd Hand_Jacobian_Contact(6*n_c_eff,nq_hand-1);
      Eigen::MatrixXd Grasp_Matrix_Contact(6,6*n_c_eff);

      int s = 0; // dimension
      int s_ = 0;

      for(int i = 0; i < n_c_eff; i++)
      {

        Grasp_Matrix_Contact.block<6,6>(0,s) = Grasp_Matrix_[i];

        cout << "i : " << i << endl;

      	Hand_Jacobian_Contact.block<6,5>(s,0) = Hand_Jacobian_[s_];
      	Hand_Jacobian_Contact.block<6,7>(s,5) = Hand_Jacobian_[s_+1];
      	Hand_Jacobian_Contact.block<6,7>(s,12) = Hand_Jacobian_[s_+2];
      	Hand_Jacobian_Contact.block<6,7>(s,19) = Hand_Jacobian_[s_+3];
      	Hand_Jacobian_Contact.block<6,7>(s,26) = Hand_Jacobian_[s_+4];

  			s_+=5;
       	s+=6;
      }

/*
        file_output << "Grasp_Matrix_Contact : " << endl;
        file_output <<  Grasp_Matrix_Contact << endl;
        file_output << "__________________________________________________" << endl;
        file_output << "Hand_Jacobian_Contact : " << endl;
        file_output << Hand_Jacobian_Contact << endl;
*/

        // GRASP JACOBIAN

      Eigen::MatrixXd GRASP_Jacobian(6,nq_hand-1);
      Eigen::MatrixXd Grasp_Matrix_pseudo(Grasp_Matrix_Contact.rows(),Grasp_Matrix_Contact.cols()) ;
      pseudo_inverse(Grasp_Matrix_Contact,Grasp_Matrix_pseudo);

      GRASP_Jacobian = Grasp_Matrix_pseudo.transpose() * Hand_Jacobian_Contact;

      cout << "GRASP_Jacobian : " << endl;
      cout << GRASP_Jacobian << endl;

      int quality_ = 9999;
      Eigen::VectorXd Singular;
      double sigma_min ;
		  double sigma_max ;

      switch(quality_index)
      {

        case 0: // "minimum_singular_value_of_G"  Q = sigma_min(G)
				      {
        	   		JacobiSVD<MatrixXd> svd0(Grasp_Matrix_Contact, ComputeThinU | ComputeThinV);
        				Singular = svd0.singularValues();

				    		Quality.push_back(Singular[Singular.size()-1]);
				      }
        		  break;

        case 1: // "Volume of the ellipsoid in the wrench space"  Q = K sqrt(det(GG.t)) = k ( sigma_0 **** sigma_d)
        		  {
                Eigen::MatrixXd G_G_t = Grasp_Matrix_Contact * Grasp_Matrix_Contact.transpose();
					      JacobiSVD<MatrixXd> svd1(G_G_t, ComputeThinU | ComputeThinV);
					      Singular = svd1.singularValues();

				        for(int i = 0 ; i < Singular.size() ; i++)
						        quality_ *= Singular[i];

					     Quality.push_back(quality_);
				      }
        		  break;

        case 2: // "Grasp isotropy index" Q = sigma_min(G) / sigma_max(G)
        		 {
        			  JacobiSVD<MatrixXd> svd2(Grasp_Matrix_Contact, ComputeThinU | ComputeThinV);
					      Singular = svd2.singularValues();
                sigma_min = Singular[Singular.size()-1];
					      sigma_max = Singular[0];

					      Quality.push_back(sigma_min/sigma_max);
				      }
				      break;

			case 3: // "Distance to singular configuration" Q = sigma_min(H) H = G.pseudo_inverse.transpose * J
				    {
					    JacobiSVD<MatrixXd> svd3(GRASP_Jacobian, ComputeThinU | ComputeThinV);
					    Singular = svd3.singularValues();

              Quality.push_back(Singular[Singular.size()-1]);
				    }
				    break;

			case 4: // "Volume of manipulability ellipsoid"
				{
					Eigen::MatrixXd H_H_t = GRASP_Jacobian * GRASP_Jacobian.transpose();

					JacobiSVD<MatrixXd> svd4(H_H_t, ComputeThinU | ComputeThinV);

					Singular = svd4.singularValues();

					for(int i = 0 ; i < Singular.size() ; i++)
						quality_ *= Singular[i];

					Quality.push_back(quality_);
				}
				break;

			case 5: // "Uniformity of transformations"
				{
					JacobiSVD<MatrixXd> svd5(GRASP_Jacobian, ComputeThinU | ComputeThinV);

					Singular = svd5.singularValues();

					sigma_min = Singular[Singular.size()-1];
					sigma_max = Singular[0];

					Quality.push_back(sigma_min/sigma_max);
				}
				break;
      }//end switch

    cout << "Quality : " << endl;

    for(int i = 0; i < Quality.size(); i++)
			cout << Quality[i] << ' ';
    cout << endl;
	}
  // end for each row

  cout << " YEAH ENJOY " << endl;
  cout << "   fine   " << endl;

	ros::spin();
	return 0;
}


text:
//==================================================================================================
/*!
  @file

  Provides macros for function overload definition and declaration

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

**/
//==================================================================================================
#ifndef BOOST_SIMD_DETAIL_DISPATCH_FUNCTION_OVERLOAD_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_DISPATCH_FUNCTION_OVERLOAD_HPP_INCLUDED

#include <boost/preprocessor/cat.hpp>
#include <boost/preprocessor/tuple/rem.hpp>
#include <boost/preprocessor/punctuation/remove_parens.hpp>
#include <boost/config.hpp>
#include <type_traits>

/*!
  @ingroup group-function

  Provides the name of the dispatching function for a given @c TAG

  @param TAG Tag of the function to dispatch
**/
#define BOOST_DISPATCH_DISPATCHING_FUNCTION(TAG) BOOST_PP_CAT(dispatching_, TAG)

/*!
  @ingroup group-function
**/
#define BOOST_DISPATCH_IMPLEMENTS(TAG, ...)                                                         \
BOOST_DISPATCH_DISPATCHING_FUNCTION(TAG)( adl_helper const&, __VA_ARGS__)                           \

/*!
  @ingroup group-function
**/
#define BOOST_DISPATCH_FALLBACK(...) dispatching( adl_helper const&, __VA_ARGS__) BOOST_NOEXCEPT

/*!
  @ingroup group-function
**/
#define BOOST_DISPATCH_OVERLOAD(TAG, TEMPLATES, ... )                                               \
template<BOOST_PP_TUPLE_REM_CTOR(TEMPLATES)>                                                        \
BOOST_PP_CAT(impl_,TAG)<__VA_ARGS__>                                                                \
BOOST_DISPATCH_IMPLEMENTS(TAG,__VA_ARGS__) BOOST_NOEXCEPT { return {}; }                            \
template<BOOST_PP_TUPLE_REM_CTOR(TEMPLATES)> struct BOOST_PP_CAT(impl_,TAG)<__VA_ARGS__>            \
/**/

/*!
  @ingroup group-function
**/
#define BOOST_DISPATCH_OVERLOAD_IF(TAG, TEMPLATES, COND, ... )                                      \
template< BOOST_PP_TUPLE_REM_CTOR(TEMPLATES)                                                        \
        , typename = typename std::enable_if<BOOST_PP_REMOVE_PARENS(COND)::value>::type             \
        >                                                                                \
BOOST_PP_CAT(impl_,TAG)<BOOST_PP_REMOVE_PARENS(COND),__VA_ARGS__>                                   \
BOOST_DISPATCH_IMPLEMENTS(TAG,__VA_ARGS__) BOOST_NOEXCEPT { return {}; }                            \
template<BOOST_PP_TUPLE_REM_CTOR(TEMPLATES)>                                                        \
struct BOOST_PP_CAT(impl_,TAG)<BOOST_PP_REMOVE_PARENS(COND),__VA_ARGS__>                            \
/**/

/*!
  @ingroup group-function
**/
#define BOOST_DISPATCH_OVERLOAD_FALLBACK( TEMPLATES, ... )                                          \
template<typename... Specifications> struct impl_fallback;                                          \
template<BOOST_PP_TUPLE_REM_CTOR(TEMPLATES)>                                                        \
impl_fallback<__VA_ARGS__>                                                                          \
dispatching( adl_helper const&, __VA_ARGS__) BOOST_NOEXCEPT { return {}; }                          \
template<BOOST_PP_TUPLE_REM_CTOR(TEMPLATES)> struct impl_fallback<__VA_ARGS__>                      \
/**/

#endif

max_stars_repo_name: xmar/pythran

max_issues_repo_name: xmar/pythran

max_forks_repo_name: xmar/pythran


text:
#include <httplib/parser/extension_list_parser.hpp>
#include <httplib/parser/detail/utility.hpp>

#include <boost/algorithm/string/predicate.hpp>

#include <algorithm>

HTTPLIB_OPEN_NAMESPACE

boost::optional<const extension_parameter_t &> extension_t::parameter(boost::string_view name) const {
    auto it = std::find_if(parameters.begin(), parameters.end(),
        [&name](const auto &v) {
            return v.name == name;
        }
    );

    if (it != parameters.end()) {
        return *it;
    } else {
        return boost::none;
    }
}

bool extension_t::equals(boost::string_view other) const {
 return boost::algorithm::iequals(name, other);
}

boost::optional<const extension_t &> extension_list_t::get(boost::string_view name) const {
    auto it = std::find_if(extensions.begin(), extensions.end(),
        [&name](const auto &v) {
            return v.equals(name);
        }
    );

    if (it != extensions.end()) {
        return *it;
    } else {
        return boost::none;
    }
}

namespace {

boost::optional<extension_parameter_t> parse_extension_parameter(boost::string_view &data) {
    boost::string_view leftover_data = data;

    extension_parameter_t result;

    if (auto name = detail::parse_token(leftover_data)) {
        result.name = std::move(*name);
    } else {
        return boost::none;
    }

    detail::skip_optional_whitespaces(leftover_data);

    if (leftover_data.empty() || leftover_data[0] != '=') {
        return boost::none;
    }

    leftover_data = leftover_data.substr(1);
    detail::skip_optional_whitespaces(leftover_data);

    if (leftover_data.empty()) {
        return boost::none;
    }

    boost::optional<std::string> value;

    if (leftover_data[0] == '"') {
        value = detail::parse_quoted_string(leftover_data);
    } else {
        value = detail::parse_token(leftover_data);
    }

    if (value) {
        data = leftover_data;
        result.value = std::move(*value);
        return result;
    } else {
        return boost::none;
    }
}

boost::optional<extension_t> parse_extension(boost::string_view &data) {
    boost::string_view leftover_data = data;

    extension_t result;

    if (auto token = detail::parse_token(leftover_data)) {
        result.name = std::move(*token);
    } else {
        return boost::none;
    }

    while (true) {
        detail::skip_optional_whitespaces(leftover_data);

        if (leftover_data.empty() || leftover_data[0] != ';') {
            data = leftover_data;
            return result;
        }

        leftover_data = leftover_data.substr(1);
        detail::skip_optional_whitespaces(leftover_data);

        if (auto parameter = parse_extension_parameter(leftover_data)) {
            result.parameters.emplace_back(std::move(*parameter));
        } else {
            return boost::none;
        }
    }
}

} // namespace

bool detail::parse_extension_list(boost::string_view data, extension_list_t &result) {
    while (true) {
        if (data.empty()) {
            return false;
        }

        if (data[0] == ',') {
            data = data.substr(1);
        } else {
            break;
        }

        detail::skip_optional_whitespaces(data);
    }

    if (auto extension = parse_extension(data)) {
        result.extensions.emplace_back(std::move(*extension));
    } else {
        return false;
    }

    while (true) {
        detail::skip_optional_whitespaces(data);

        if (data.empty()) {
            return true;
        }

        if (data[0] != ',') {
            return false;
        }

        data = data.substr(1);
        detail::skip_optional_whitespaces(data);

        if (data.empty() || data[0] == ',') {
            continue;
        }

        if (auto extension = parse_extension(data)) {
            result.extensions.emplace_back(std::move(*extension));
        } else {
            return false;
        }
    }
}

boost::optional<extension_list_t> parse_extension_list(boost::string_view data) {
    extension_list_t result;

    if (detail::parse_extension_list(data, result)) {
        return result;
    } else {
        return boost::none;
    }
}

HTTPLIB_CLOSE_NAMESPACE


text:
// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef error_model_factory_hpp
#define error_model_factory_hpp

#include <memory>
#include <string>

#include <boost/filesystem/path.hpp>

#include "snv_error_model.hpp"
#include "indel_error_model.hpp"

namespace octopus {

enum class LibraryPreparation { pcr, pcr_free, tenx };
enum class Sequencer { hiseq_2000, hiseq_2500, hiseq_4000, xten, novaseq, bgiseq_500, pacbio, pacbio_css };

struct ModelConfig
{
    LibraryPreparation library;
    Sequencer sequencer;
};

static constexpr LibraryPreparation default_library_preparation {LibraryPreparation::pcr_free};
static constexpr Sequencer default_sequencer {Sequencer::hiseq_2500};
static constexpr ModelConfig default_model_config {default_library_preparation, default_sequencer};

struct ErrorModel
{
    std::unique_ptr<IndelErrorModel> indel;
    std::unique_ptr<SnvErrorModel> snv;
};

std::unique_ptr<SnvErrorModel> make_snv_error_model(ModelConfig config = default_model_config);
std::unique_ptr<IndelErrorModel> make_indel_error_model(ModelConfig config = default_model_config);

ErrorModel make_error_model(const std::string& label);
ErrorModel make_error_model(const boost::filesystem::path& model_filename);

} // namespace octopus

#endif

max_stars_repo_name: gunjanbaid/octopus

max_issues_repo_name: gunjanbaid/octopus

max_forks_repo_name: gunjanbaid/octopus


text:
/*
 * Copyright © 2012, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All rights reserved.
 *
 * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is 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.
*/

/**
 * @file tgRigidAutoCompound.cpp
 * @brief Definition of class tgRigidAutoCompound
 * @author Ryan Adams, Drew Sabelhaus
 * @date March 2014
 * $Id$
 */

// This module
#include "tgRigidAutoCompound.h"
// Bullet Physics
#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
#include "tgCompoundRigidInfo.h"
// The C++ standard library
#include <map>
#include <cstdlib> // for random number generator
#include <sstream> // for string streams, tags.
// Boost
#include <boost/random/random_device.hpp> // used for the random compound tag hash
#include <boost/random/uniform_int_distribution.hpp> // used for the random compound tag hash

// Debugging
#include <iostream>
#include "tgUtil.h"

using namespace std;

// @todo: we want to start using this and get rid of the set-based constructor, but until we can refactor...
tgRigidAutoCompound::tgRigidAutoCompound(std::vector<tgRigidInfo*> rigids)
{
    m_rigids.insert(m_rigids.end(), rigids.begin(), rigids.end());
}

tgRigidAutoCompound::tgRigidAutoCompound(std::deque<tgRigidInfo*> rigids) : m_rigids(rigids)
{}

std::vector< tgRigidInfo* > tgRigidAutoCompound::execute() {

    // Determine the grouping of our rigids
    groupRigids();

    // Create the compounds as necessary
    createCompounds();

    // Set the rigid body for the various groups
    for(int i=0; i < m_groups.size(); i++) {
        // Note: rigids that are not connected to anything else are placed
        //into a group of their own, so they're represented here too
        setRigidInfoForGroup(m_compounded[i], m_groups[i]);
    }
    // Need to return this so we can delete it at the appropreate time
    return m_compounded;
};

// @todo: we probably don't need this any more -- this will be taken care of in the tgRigidInfo => tgModel step
// @todo: NOTE: we need to have a way to check to see if a rigid has already been instantiated -- maybe just check get
void tgRigidAutoCompound::setRigidBodyForGroup(btCollisionObject* body, std::deque<tgRigidInfo*>& group) {
    for(int i = 0; i < group.size(); i++) {
        group[i]->setCollisionObject(body);
    }
}

void tgRigidAutoCompound::setRigidInfoForGroup(tgRigidInfo* rigidInfo, std::deque<tgRigidInfo*>& group) {
    for(int i = 0; i < group.size(); i++) {
        group[i]->setRigidInfoGroup(rigidInfo);
    }
}

void tgRigidAutoCompound::groupRigids()
{
    std::deque<tgRigidInfo*> ungrouped = std::deque<tgRigidInfo*>(m_rigids); // Copy of m_rigids

    while(ungrouped.size() > 0) {
        // go through each ungrouped element and find the groups for it
        tgRigidInfo* elem = ungrouped[0]; // Note: the 0 element is removed by findGroup, so ungrouped[0] is different each iteration

        // Find all elements linked to the first item in ungrouped
        // This will also remove the elements in the group from ungrouped
        std::deque<tgRigidInfo*> group = findGroup(elem, ungrouped);

        // Add the group to the groups list
        m_groups.push_back(group);
    }
}

// Find all rigids that should be in a group with the given rigid
// @todo: This may contain an off-by-one error (the last rigid may not be grouped properly...)
std::deque<tgRigidInfo*> tgRigidAutoCompound::findGroup(tgRigidInfo* rigid, std::deque<tgRigidInfo*>& ungrouped) {

    std::deque<tgRigidInfo*> group;

    // Add the rigid to the current group
    group.push_back(rigid);

    // Remove the rigid from the ungrouped since it's now in a group
    ungrouped.erase(std::remove(ungrouped.begin(), ungrouped.end(), rigid), ungrouped.end());

    // Recursively find linked elements
    int i = 0;
    while(i < ungrouped.size()) {
        tgRigidInfo* other = ungrouped[i];
        if(rigid->sharesNodesWith(*other)) {
            std::deque<tgRigidInfo*> links = findGroup(other, ungrouped);
            group.insert(group.end(), links.begin(), links.end());
            i = 0;
        } else {
            i++;
        }
    }

    return group;
};

void tgRigidAutoCompound::createCompounds() {
    for(int i=0; i < m_groups.size(); i++) {
        std::deque<tgRigidInfo*>& group = m_groups[i];
        if(group.size() == 1) {
            // Only one, no compounding necessary
            m_compounded.push_back(group[0]);
        } else {
            tgRigidInfo* compound = createCompound(group);
            m_compounded.push_back(compound);
        }
    }
}

tgRigidInfo* tgRigidAutoCompound::createCompound(std::deque<tgRigidInfo*> rigids) {
    tgCompoundRigidInfo* c = new tgCompoundRigidInfo();
    // Add an additional tag to this compound rigid info.
    // This is of the form "compound_3qhA8L" for example.
    std::stringstream newtag;
    newtag << "compound_" << random_tag_hash();
    for(int i = 0; i < rigids.size(); i++) {
      rigids[i]->addTags(newtag.str());
      c->addRigid(*rigids[i]);
    }
    return (tgRigidInfo*)c;
}

bool tgRigidAutoCompound::rigidBelongsIn(tgRigidInfo* rigid, std::deque<tgRigidInfo*> group) {
    for(int i = 0; i < group.size(); i++) {
        tgRigidInfo* other = group[i];
        if(rigid->sharesNodesWith(*other))
            return true;
    }
    return false;
};

std::string tgRigidAutoCompound::random_tag_hash() {
  /**
   * Many thanks to StackOverflow users Ates Goral and Mehrdad Afshari
   * for a framework for this function.
   * http://stackoverflow.com/questions/440133/how-do-i-create-a-random-alpha-numeric-string-in-c
   *
   * This function should generate a random string with a uniform distribution
   * over the characters in the 'alphanum' char array below.
   */
  // Create the string (character array) to put the random characters into
  size_t length = 6;
  char s[length];

  // The random number generator to pick characters out of the array
  boost::random::random_device rng;

  // A constant variable for the chracters that will be chosen from
  static const char alphanum[] =
    "0123456789"
    "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
    "0123456789"
    "abcdefghijklmnopqrstuvwxyz";

  // A uniform distribution over the indices into the character array
  boost::random::uniform_int_distribution<> alphanum_dist(0, sizeof(alphanum) - 1);

  // Insert a random one of these characters into the array
  // Thanks to the Boost library random number generator tutorial,
  // http://www.boost.org/doc/libs/1_62_0/doc/html/boost_random/tutorial.html
  for (int i = 0; i < length; ++i) {
    //s[i] = alphanum[rand() % (sizeof(alphanum) - 1)];
    s[i] = alphanum[ alphanum_dist(rng) ];
  }

  // set the string termination character
  s[length] = 0;

  // Returning a character array is the same as returning a string.
  return s;
}

max_stars_repo_name: wvat/NTRTsim

max_issues_repo_name: wvat/NTRTsim

max_forks_repo_name: wvat/NTRTsim


text:
/**
*	@file ServerSettings.hpp
*	@brief This file contains class that contains server settings (like timeout limit).
*/

#ifndef ServerSettings_hpp
#define ServerSettings_hpp

#include <map>
#include <string>
#include <sstream>
#include <fstream>
#include <iostream>
#include <stdexcept>

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

using namespace boost::property_tree;

/**
*	@class ServerSettings
*/
class ServerSettings final
{
public:
	/**
	*	Basic constructor.
	*/
	ServerSettings();

	/**
	*	Basic destructor.
	*/
	~ServerSettings();

	/**
	*	This member function frees memory allocated for the property tree.
	*/
	void deleteCurrentTree();

	/**
	*	This member function creates new property tree.
	*/
	void createCurrentTree();

	/**
	*	This member function sets the property tree to default values.
	*/
	void setCurrentTreeToDefault();

	/**
	*	This member function checks whether information read from the json file is correct.
	*	@returns True if file is correct and false if not.
	*/
	bool validateJsonFields();

	/**
	*	This member function reads given json settings file and validates it.
	*	@param jsonPath	A string path to the settings file.
	*/
	void readJson(const std::string& jsonPath);

	/**
	*	This member function changes the contents of the json settings file if any were made during interactive command line session.
	*	@param jsonPath A string path to the settings file.
	*/
	void writeJson(const std::string& jsonPath);

	/**
	*	Debug function. Prints current property tree.
	*/
	void printCurrentTree();

	/**
	*	A getter function.
	*	@returns A string object containing the sequence to be in the sequence swapping function. Read from the json settings file.
	*	@see PSiCAlgorithm
	*/
	std::string getConnectionData26Numbers() const;

	/**
	*	A getter function.
	*	@returns A string object containing the path to the proxy server's certificate file. Read from the json settings file.
	*/
	std::string getCertificateFilePath() const;

	/**
	*	A getter function.
	*	@returns A string object containing the path to the proxy server's private key file. Read from the json settings file.
	*/
	std::string getPrivateKeyFilePath() const;

	/**
	*	A getter function.
	*	@returns The maximum number of connections the server can handle at a time. Read from the json settings file.
	*/
	unsigned getMaxNumberOfConnections() const;

	/**
	*	A getter function.
	*	@returns The maximum number of seconds a connection through the proxy can be idle. Read from the json settings file.
	*/
	unsigned getTimeoutLimit() const;

	// Setters for internal server use in order to modify
	// json values accordingly.
	/**
	*	A setter function.
	*	@param connectionData26Numbers A reference to a string object containing the new sequence to be used.
	*/
	void setConnectionData26Numbers(const std::string& connectionData26Numbers);

	/**
	*	A setter function.
	*	@param certificateFilePath A reference to a string object containing the path to the proxy server's certificate file.
	*/
	void setCertificateFilePath(const std::string& certificateFilePath);

	/**
	*	A setter function.
	*	@param privateKeyFilePath A reference to a string object containing the path to the proxy server's private key file.
	*/
	void setPrivateKeyFilePath(const std::string& privateKeyFilePath);

	/**
	*	A setter function.
	*	@param maxNumberOfConnections A reference to a string object containing the desired maximum connections to be used.
	*/
	void setMaxNumberOfConnections(const std::string& maxNumberOfConnections);

	/**
	*	A setter function.
	*	@param timeoutLimit A reference to a string object containing the desired maximum idle time of a connection.
	*/
	void setTimeoutLimit(const std::string& timeoutLimit);

private:
	ptree* currentTree = nullptr;

	const std::map<std::string, std::string> treeDefaultvalues = {
    {"connectionData26Numbers", "562847563758263a95836111484"},
		{"certificateFilePath", "./certs/domena.crt"},
		{"privateKeyFilePath", "./certs/domena.key"},
		{"maxNumberOfConnections", "100"},
		{"timeoutLimit", "60"}
	};

	struct WrongJsonDataException : std::runtime_error {
		WrongJsonDataException(const char* msg) : std::runtime_error(msg) {}
	};
};

#endif // ServerSettings_hpp


text:
/*
    Copyright (c) 2010 Séverin Lemaignan (slemaign@laas.fr)

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

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

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

#include <algorithm>
#include <boost/functional/hash.hpp>
#include <boost/foreach.hpp>

#include "core/vectors.h"

#include "macros.h"
#include "memoryview_exceptions.h"

#include "memoryview.h"
#include "edge.h"
#include "node_relation.h"
#include "node.h"
#include "graph.h"

using namespace std;
using namespace boost;

Edge::Edge(const NodeRelation& rel, double weight) :
    node1(rel.from),
    node2(rel.to),
    weight(weight),
    renderer(EdgeRenderer(hash_value(rel.from->getID() + rel.to->getID())))
{
    //    addReferenceRelation(rel);

    spring_constant = INITIAL_SPRING_CONSTANT * weight;
    nominal_length = NOMINAL_EDGE_LENGTH;

    length = 0.0;
}

void Edge::setWeight(double _weight){
    weight = _weight;
    spring_constant = max(0., INITIAL_SPRING_CONSTANT * weight);
}

void Edge::step(Graph& g, float dt){

    if(std::isnan(weight)) return;

    updateLength();

#ifndef TEXT_ONLY

    const vec2f& pos1 = node1->pos;
    const vec2f& pos2 = node2->pos;

    //update the spline point
    vec2f td = (pos2 - pos1) * 0.5;

    vec2f mid = pos1 + td;// - td.perpendicular() * pos.normal();// * 10.0;

    vec2f delta = (mid - spos);

    //dont let spos get more than half the length of the distance behind
    if(delta.length2() > td.length2()) {
        spos += delta.normal() * (delta.length() - td.length());
    }

    spos += delta * min(1.0, dt * 2.0);

    td.normalize();

    vec2f out_of_node1_distance = td * (
                (node1->selected() ? NODE_SIZE * SELECT_SIZE_FACTOR : NODE_SIZE) / 2 + 2
                );

    vec2f out_of_node2_distance = td * (
                (node2->selected() ? NODE_SIZE * SELECT_SIZE_FACTOR : NODE_SIZE) / 2 + 2
                );

    if (   node1->selected()
        || node2->selected()
        || node1->hovered()
        || node2->hovered())
    {
        renderer.selected = true;
    }
    else {
        renderer.selected = false;
    }

    //Update the age of the edge renderer
    renderer.increment_idle_time(dt);

    //renderer.update(pos1 + out_of_node1_distance , node1->renderer.col,
    //                pos2  - out_of_node2_distance , node2->renderer.col, spos);

    auto col = computeColour();
    renderer.update(pos1 + out_of_node1_distance , col,
                    pos2  - out_of_node2_distance , col, spos);

#endif
    TRACE("Edge between " << node1->getID() << " and " << node2->getID() << " updated.");

}

vec4f Edge::computeColour() const {
    vec4f col;
    if (renderer.selected) {
        col = HOVERED_COLOUR;
    }
    else {
        if (weight > 0) {
            col = vec4f((float) weight, (float) weight * 0.5, 0, 0.7);
        }
        else {
            col = vec4f(0,0, (float) -weight, 0.7);
        }
    }

    return col;
}

void Edge::render(rendering_mode mode, MemoryView& env){

#ifndef TEXT_ONLY
    if (mode == GRAPHVIZ and !std::isnan(weight)) {
        std::stringstream str;
        auto col = computeColour();
        str << "#" << setfill('0') << setw(2) << hex << (int) floor(col.x * 256) << setw(2) << (int) floor(col.y * 256) << setw(2) << (int) floor(col.z * 256);

        env.graphvizGraph << node1->getSafeID() << " -- " << node2->getSafeID() << " [label=" << fixed << setprecision( 2 ) << weight <<", color=\"" << str.str() << "\"];\n";
        return;
    }

    if (std::isnan(weight)) {
        renderer.label = "";
    }
    else {
        std::stringstream str;
        str << fixed << setprecision( 2 ) << weight;
        renderer.label = str.str();
    }
    renderer.draw(mode, env);
#endif
    //TRACE("Edge between " << node1->getID() << " and " << node2->getID() << " rendered.");

}

void Edge::updateLength() {
    //TODO: optimisation by using length2 here?
    length = (node1->pos -  node2->pos).length();
}

int Edge::getId1() const {
    return node1->getID();
}

int Edge::getId2() const {
    return node2->getID();
}

max_stars_repo_name: severin-lemaignan/memory-view

max_issues_repo_name: severin-lemaignan/memory-view

max_forks_repo_name: severin-lemaignan/memory-view


text:
#include <ctime>
#include <iostream>
#include <string>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/asio.hpp>

using boost::asio::ip::tcp;

std::string make_daytime_string()
{
    using namespace std;
    time_t now = time(0);
    return ctime(&now);
}

// 将 tcp_connection 类封装成单例模式
class tcp_connection : public boost::enable_shared_from_this<tcp_connection>
{
public:
    typedef boost::shared_ptr<tcp_connection> pointer;

    static pointer create(boost::asio::io_context& io_context)
    {
        return pointer(new tcp_connection(io_context));
    }

    tcp::socket& socket()
    {
        return _socket;
    }

    void start()
    {
        _message = make_daytime_string();

        boost::asio::async_write(_socket,boost::asio::buffer(_message),
                boost::bind(&tcp_connection::handle_write,shared_from_this(),
                    boost::asio::placeholders::error,
                    boost::asio::placeholders::bytes_transferred));
    }

private:
    tcp_connection(boost::asio::io_context& io_context)
    :_socket(io_context)
    {   }

    void handle_write(const boost::system::error_code& /*error*/,
            size_t /*bytes_transferred*/ )
    {  }

    tcp::socket _socket;
    std::string _message;
};

class tcp_server
{
public:
    tcp_server(boost::asio::io_context& io_context)
    :_acceptor(io_context,tcp::endpoint(tcp::v4(),13))
    {
        start_accept();
    }

private:
    void start_accept()
    {
        tcp_connection::pointer new_connection = tcp_connection::create(_acceptor.get_executor().context());
        _acceptor.async_accept(new_connection->socket(),
            boost::bind(&tcp_server::handle_accept,this,new_connection,
                boost::asio::placeholders::error));
    }

    void handle_accept(tcp_connection::pointer new_connection,
        const boost::system::error_code& error)
    {
        if(!error)
        {
            new_connection -> start();
        }

        start_accept();
    }

    tcp::acceptor _acceptor;
};

int main(int argc, char const *argv[])
{
    try
    {
        boost::asio::io_context io_context;
        tcp_server server(io_context);
        io_context.run();
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << '\n';
    }

    return 0;
}