<commit_before>// @(#)root/io:$Id$
// Author: Markus Frank 28/10/04

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TEmulatedMapProxy
//
// Streamer around an arbitrary container, which implements basic
// functionality and iteration.
//
// In particular this is used to implement splitting and abstract
// element access of any container. Access to compiled code is necessary
// to implement the abstract iteration sequence and functionality like
// size(), clear(), resize(). resize() may be a void operation.
//
//////////////////////////////////////////////////////////////////////////

#include "TEmulatedMapProxy.h"
#include "TClassEdit.h"
#include "TStreamerInfo.h"
#include "TError.h"

TEmulatedMapProxy::TEmulatedMapProxy(const TEmulatedMapProxy& copy)
   : TEmulatedCollectionProxy(copy)
{
   // copy constructor
   if ( !(fSTL_type == TClassEdit::kMap || fSTL_type == TClassEdit::kMultiMap) )  {
      Fatal("TEmulatedMapProxy","Class %s is not a map-type!",fName.c_str());
   }
}

TEmulatedMapProxy::TEmulatedMapProxy(const char* cl_name, Bool_t silent)
   : TEmulatedCollectionProxy(cl_name, silent)
{
   // Build a Streamer for an emulated vector whose type is 'name'.
   if ( !(fSTL_type == TClassEdit::kMap || fSTL_type == TClassEdit::kMultiMap) )  {
      Fatal("TEmulatedMapProxy","Class %s is not a map-type!",fName.c_str());
   }
}

TEmulatedMapProxy::~TEmulatedMapProxy()
{
   // Standard destructor.
}

TVirtualCollectionProxy* TEmulatedMapProxy::Generate() const
{
   // Virtual copy constructor.
   if ( !fClass ) Initialize(kFALSE);
   return new TEmulatedMapProxy(*this);
}

void* TEmulatedMapProxy::At(UInt_t idx)
{
   // Return the address of the value at index 'idx'.
   if ( fEnv && fEnv->fObject )   {
      PCont_t c = PCont_t(fEnv->fObject);
      return idx<(c->size()/fValDiff) ? ((char*)&(*c->begin())) + idx*fValDiff : 0;
   }
   Fatal("TEmulatedMapProxy","At> Logic error - no proxy object set.");
   return 0;
}

UInt_t TEmulatedMapProxy::Size() const
{
   // Return the current size of the container.
   if ( fEnv && fEnv->fObject )   {
      PCont_t c = PCont_t(fEnv->fObject);
      return fEnv->fSize = (c->size()/fValDiff);
   }
   Fatal("TEmulatedMapProxy","Size> Logic error - no proxy object set.");
   return 0;
}

void TEmulatedMapProxy::ReadMap(UInt_t nElements, TBuffer &b)
{
   // Map input streamer.
   Bool_t vsn3 = b.GetInfo() && b.GetInfo()->GetOldVersion()<=3;
   int    idx, loop, off[2] = {0, fValOffset };
   Value  *v, *val[2] = { fKey, fVal };
   StreamHelper* helper;
   float f;
   char* addr = 0;
   char* temp = (char*)At(0);
   for ( idx = 0; idx < nElements; ++idx )  {
      addr = temp + idx*fValDiff;
      for ( loop=0; loop<2; loop++)  {
         addr += off[loop];
         helper = (StreamHelper*)addr;
         v = val[loop];
         switch (v->fCase) {
         case kIsFundamental:  // Only handle primitives this way
         case kIsEnum:
            switch( int(v->fKind) )   {
            case kBool_t:    b >> helper->boolean;     break;
            case kChar_t:    b >> helper->s_char;      break;
            case kShort_t:   b >> helper->s_short;     break;
            case kInt_t:     b >> helper->s_int;       break;
            case kLong_t:    b >> helper->s_long;      break;
            case kLong64_t:  b >> helper->s_longlong;  break;
            case kFloat_t:   b >> helper->flt;         break;
            case kFloat16_t: b >> f;
               helper->flt = float(f);  break;
            case kDouble_t:  b >> helper->dbl;         break;
            case kBOOL_t:    b >> helper->boolean;     break;
            case kUChar_t:   b >> helper->u_char;      break;
            case kUShort_t:  b >> helper->u_short;     break;
            case kUInt_t:    b >> helper->u_int;       break;
            case kULong_t:   b >> helper->u_long;      break;
            case kULong64_t: b >> helper->u_longlong;  break;
            case kDouble32_t:b >> f;
               helper->dbl = double(f);  break;
            case kchar:
            case kNoType_t:
            case kOther_t:
               Error("TEmulatedMapProxy","fType %d is not supported yet!\n",v->fKind);
            }
            break;
         case kIsClass:
            b.StreamObject(helper,v->fType);
            break;
         case kBIT_ISSTRING:
            helper->read_std_string(b);
            break;
         case kIsPointer|kIsClass:
            helper->set(b.ReadObjectAny(v->fType));
            break;
         case kIsPointer|kBIT_ISSTRING:
            helper->read_std_string_pointer(b);
            break;
         case kIsPointer|kBIT_ISTSTRING|kIsClass:
            helper->read_tstring_pointer(vsn3,b);
            break;
         }
      }
   }
}

void TEmulatedMapProxy::WriteMap(UInt_t nElements, TBuffer &b)
{
   // Map output streamer.
   Value  *v, *val[2] = { fKey, fVal };
   int    off[2]      = { 0, fValOffset };
   StreamHelper* i;
   char* addr = 0;
   char* temp = (char*)At(0);
   for (int loop, idx = 0; idx < nElements; ++idx )  {
      addr = temp + idx*fValDiff;
      for ( loop = 0; loop<2; ++loop )  {
         addr += off[loop];
         i = (StreamHelper*)addr;
         v = val[loop];
         switch (v->fCase) {
         case kIsFundamental:  // Only handle primitives this way
         case kIsEnum:
            switch( int(v->fKind) )   {
            case kBool_t:    b << i->boolean;     break;
            case kChar_t:    b << i->s_char;      break;
            case kShort_t:   b << i->s_short;     break;
            case kInt_t:     b << i->s_int;       break;
            case kLong_t:    b << i->s_long;      break;
            case kLong64_t:  b << i->s_longlong;  break;
            case kFloat_t:   b << i->flt;         break;
            case kFloat16_t: b << float(i->flt);  break;
            case kDouble_t:  b << i->dbl;         break;
            case kBOOL_t:    b << i->boolean;     break;
            case kUChar_t:   b << i->u_char;      break;
            case kUShort_t:  b << i->u_short;     break;
            case kUInt_t:    b << i->u_int;       break;
            case kULong_t:   b << i->u_long;      break;
            case kULong64_t: b << i->u_longlong;  break;
            case kDouble32_t:b << float(i->dbl);  break;
            case kchar:
            case kNoType_t:
            case kOther_t:
               Error("TEmulatedMapProxy","fType %d is not supported yet!\n",v->fKind);
            }
            break;
         case kIsClass:
            b.StreamObject(i,v->fType);
            break;
         case kBIT_ISSTRING:
            TString(i->c_str()).Streamer(b);
            break;
         case kIsPointer|kIsClass:
            b.WriteObjectAny(i->ptr(),v->fType);
            break;
         case kBIT_ISSTRING|kIsPointer:
            i->write_std_string_pointer(b);
            break;
         case kBIT_ISTSTRING|kIsClass|kIsPointer:
            i->write_tstring_pointer(b);
            break;
         }
      }
   }
}

void TEmulatedMapProxy::ReadBuffer(TBuffer &b, void *obj, const TClass *onfileClass)
{
   // Read portion of the streamer.

   SetOnFileClass((TClass*)onfileClass);
   ReadBuffer(b,obj);
}

void TEmulatedMapProxy::ReadBuffer(TBuffer &b, void *obj)
{
   // Read portion of the streamer.

   TPushPop env(this,obj);
   int nElements = 0;
   b >> nElements;
   if ( fEnv->fObject )  {
      Resize(nElements,true);
   }
   if ( nElements > 0 )  {
      ReadMap(nElements, b);
   }
}

void TEmulatedMapProxy::Streamer(TBuffer &b)
{
   // TClassStreamer IO overload.
   if ( b.IsReading() ) {  //Read mode
      UInt_t nElements = 0;
      b >> nElements;
      if ( fEnv->fObject )  {
         Resize(nElements,true);
      }
      if ( nElements > 0 )  {
         ReadMap(nElements, b);
      }
   }
   else {     // Write case
      UInt_t nElements = fEnv->fObject ? Size() : 0;
      b << nElements;
      if ( nElements > 0 )  {
         WriteMap(nElements, b);
      }
   }
}
<commit_msg>two more instance of Int_t -> UInt_t<commit_after>// @(#)root/io:$Id$
// Author: Markus Frank 28/10/04

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TEmulatedMapProxy
//
// Streamer around an arbitrary container, which implements basic
// functionality and iteration.
//
// In particular this is used to implement splitting and abstract
// element access of any container. Access to compiled code is necessary
// to implement the abstract iteration sequence and functionality like
// size(), clear(), resize(). resize() may be a void operation.
//
//////////////////////////////////////////////////////////////////////////

#include "TEmulatedMapProxy.h"
#include "TClassEdit.h"
#include "TStreamerInfo.h"
#include "TError.h"

TEmulatedMapProxy::TEmulatedMapProxy(const TEmulatedMapProxy& copy)
   : TEmulatedCollectionProxy(copy)
{
   // copy constructor
   if ( !(fSTL_type == TClassEdit::kMap || fSTL_type == TClassEdit::kMultiMap) )  {
      Fatal("TEmulatedMapProxy","Class %s is not a map-type!",fName.c_str());
   }
}

TEmulatedMapProxy::TEmulatedMapProxy(const char* cl_name, Bool_t silent)
   : TEmulatedCollectionProxy(cl_name, silent)
{
   // Build a Streamer for an emulated vector whose type is 'name'.
   if ( !(fSTL_type == TClassEdit::kMap || fSTL_type == TClassEdit::kMultiMap) )  {
      Fatal("TEmulatedMapProxy","Class %s is not a map-type!",fName.c_str());
   }
}

TEmulatedMapProxy::~TEmulatedMapProxy()
{
   // Standard destructor.
}

TVirtualCollectionProxy* TEmulatedMapProxy::Generate() const
{
   // Virtual copy constructor.
   if ( !fClass ) Initialize(kFALSE);
   return new TEmulatedMapProxy(*this);
}

void* TEmulatedMapProxy::At(UInt_t idx)
{
   // Return the address of the value at index 'idx'.
   if ( fEnv && fEnv->fObject )   {
      PCont_t c = PCont_t(fEnv->fObject);
      return idx<(c->size()/fValDiff) ? ((char*)&(*c->begin())) + idx*fValDiff : 0;
   }
   Fatal("TEmulatedMapProxy","At> Logic error - no proxy object set.");
   return 0;
}

UInt_t TEmulatedMapProxy::Size() const
{
   // Return the current size of the container.
   if ( fEnv && fEnv->fObject )   {
      PCont_t c = PCont_t(fEnv->fObject);
      return fEnv->fSize = (c->size()/fValDiff);
   }
   Fatal("TEmulatedMapProxy","Size> Logic error - no proxy object set.");
   return 0;
}

void TEmulatedMapProxy::ReadMap(UInt_t nElements, TBuffer &b)
{
   // Map input streamer.
   Bool_t vsn3 = b.GetInfo() && b.GetInfo()->GetOldVersion()<=3;
   UInt_t idx, loop;
   Int_t off[2] = {0, fValOffset };
   Value  *v, *val[2] = { fKey, fVal };
   StreamHelper* helper;
   float f;
   char* addr = 0;
   char* temp = (char*)At(0);
   for ( idx = 0; idx < nElements; ++idx )  {
      addr = temp + idx*fValDiff;
      for ( loop=0; loop<2; loop++)  {
         addr += off[loop];
         helper = (StreamHelper*)addr;
         v = val[loop];
         switch (v->fCase) {
         case kIsFundamental:  // Only handle primitives this way
         case kIsEnum:
            switch( int(v->fKind) )   {
            case kBool_t:    b >> helper->boolean;     break;
            case kChar_t:    b >> helper->s_char;      break;
            case kShort_t:   b >> helper->s_short;     break;
            case kInt_t:     b >> helper->s_int;       break;
            case kLong_t:    b >> helper->s_long;      break;
            case kLong64_t:  b >> helper->s_longlong;  break;
            case kFloat_t:   b >> helper->flt;         break;
            case kFloat16_t: b >> f;
               helper->flt = float(f);  break;
            case kDouble_t:  b >> helper->dbl;         break;
            case kBOOL_t:    b >> helper->boolean;     break;
            case kUChar_t:   b >> helper->u_char;      break;
            case kUShort_t:  b >> helper->u_short;     break;
            case kUInt_t:    b >> helper->u_int;       break;
            case kULong_t:   b >> helper->u_long;      break;
            case kULong64_t: b >> helper->u_longlong;  break;
            case kDouble32_t:b >> f;
               helper->dbl = double(f);  break;
            case kchar:
            case kNoType_t:
            case kOther_t:
               Error("TEmulatedMapProxy","fType %d is not supported yet!\n",v->fKind);
            }
            break;
         case kIsClass:
            b.StreamObject(helper,v->fType);
            break;
         case kBIT_ISSTRING:
            helper->read_std_string(b);
            break;
         case kIsPointer|kIsClass:
            helper->set(b.ReadObjectAny(v->fType));
            break;
         case kIsPointer|kBIT_ISSTRING:
            helper->read_std_string_pointer(b);
            break;
         case kIsPointer|kBIT_ISTSTRING|kIsClass:
            helper->read_tstring_pointer(vsn3,b);
            break;
         }
      }
   }
}

void TEmulatedMapProxy::WriteMap(UInt_t nElements, TBuffer &b)
{
   // Map output streamer.
   Value  *v, *val[2] = { fKey, fVal };
   int    off[2]      = { 0, fValOffset };
   StreamHelper* i;
   char* addr = 0;
   char* temp = (char*)At(0);
   for (UInt_t loop, idx = 0; idx < nElements; ++idx )  {
      addr = temp + idx*fValDiff;
      for ( loop = 0; loop<2; ++loop )  {
         addr += off[loop];
         i = (StreamHelper*)addr;
         v = val[loop];
         switch (v->fCase) {
         case kIsFundamental:  // Only handle primitives this way
         case kIsEnum:
            switch( int(v->fKind) )   {
            case kBool_t:    b << i->boolean;     break;
            case kChar_t:    b << i->s_char;      break;
            case kShort_t:   b << i->s_short;     break;
            case kInt_t:     b << i->s_int;       break;
            case kLong_t:    b << i->s_long;      break;
            case kLong64_t:  b << i->s_longlong;  break;
            case kFloat_t:   b << i->flt;         break;
            case kFloat16_t: b << float(i->flt);  break;
            case kDouble_t:  b << i->dbl;         break;
            case kBOOL_t:    b << i->boolean;     break;
            case kUChar_t:   b << i->u_char;      break;
            case kUShort_t:  b << i->u_short;     break;
            case kUInt_t:    b << i->u_int;       break;
            case kULong_t:   b << i->u_long;      break;
            case kULong64_t: b << i->u_longlong;  break;
            case kDouble32_t:b << float(i->dbl);  break;
            case kchar:
            case kNoType_t:
            case kOther_t:
               Error("TEmulatedMapProxy","fType %d is not supported yet!\n",v->fKind);
            }
            break;
         case kIsClass:
            b.StreamObject(i,v->fType);
            break;
         case kBIT_ISSTRING:
            TString(i->c_str()).Streamer(b);
            break;
         case kIsPointer|kIsClass:
            b.WriteObjectAny(i->ptr(),v->fType);
            break;
         case kBIT_ISSTRING|kIsPointer:
            i->write_std_string_pointer(b);
            break;
         case kBIT_ISTSTRING|kIsClass|kIsPointer:
            i->write_tstring_pointer(b);
            break;
         }
      }
   }
}

void TEmulatedMapProxy::ReadBuffer(TBuffer &b, void *obj, const TClass *onfileClass)
{
   // Read portion of the streamer.

   SetOnFileClass((TClass*)onfileClass);
   ReadBuffer(b,obj);
}

void TEmulatedMapProxy::ReadBuffer(TBuffer &b, void *obj)
{
   // Read portion of the streamer.

   TPushPop env(this,obj);
   int nElements = 0;
   b >> nElements;
   if ( fEnv->fObject )  {
      Resize(nElements,true);
   }
   if ( nElements > 0 )  {
      ReadMap(nElements, b);
   }
}

void TEmulatedMapProxy::Streamer(TBuffer &b)
{
   // TClassStreamer IO overload.
   if ( b.IsReading() ) {  //Read mode
      UInt_t nElements = 0;
      b >> nElements;
      if ( fEnv->fObject )  {
         Resize(nElements,true);
      }
      if ( nElements > 0 )  {
         ReadMap(nElements, b);
      }
   }
   else {     // Write case
      UInt_t nElements = fEnv->fObject ? Size() : 0;
      b << nElements;
      if ( nElements > 0 )  {
         WriteMap(nElements, b);
      }
   }
}
<|endoftext|>


<commit_before>// Copyright 1998-2016 Epic Games, Inc. All Rights Reserved.

#include "UnrealEnginePythonPrivatePCH.h"

void unreal_engine_init_py_module();

#define LOCTEXT_NAMESPACE "FUnrealEnginePythonModule"

void FUnrealEnginePythonModule::StartupModule()
{
	// This code will execute after your module is loaded into memory; the exact timing is specified in the .uplugin file per-module

	Py_Initialize();
	wchar_t *argv[] = { UTF8_TO_TCHAR("UnrealEngine"), NULL };
	PySys_SetArgv(1, argv);

	unreal_engine_init_py_module();

	PyObject *py_sys = PyImport_ImportModule("sys");
	PyObject *py_sys_dict = PyModule_GetDict(py_sys);

	PyObject *py_path = PyDict_GetItemString(py_sys_dict, "path");

	char *zip_path = TCHAR_TO_UTF8(*FPaths::Combine(*FPaths::GameContentDir(), UTF8_TO_TCHAR("python35.zip")));
	PyObject *py_zip_path = PyUnicode_FromString(zip_path);
	PyList_Insert(py_path, 0, py_zip_path);

	char *scripts_path = TCHAR_TO_UTF8(*FPaths::Combine(*FPaths::GameContentDir(), UTF8_TO_TCHAR("Scripts")));
	PyObject *py_scripts_path = PyUnicode_FromString(scripts_path);
	PyList_Insert(py_path, 0, py_scripts_path);

	UE_LOG(LogPython, Log , TEXT("Python VM initialized: %s"), UTF8_TO_TCHAR(Py_GetVersion()));
	UE_LOG(LogPython, Log, TEXT("Python Scripts search path: %s"), UTF8_TO_TCHAR(scripts_path));

	if (PyImport_ImportModule("ue_site")) {
		UE_LOG(LogPython, Log, TEXT("ue_site Python module successfully imported"));
	}
	else {
		unreal_engine_py_log_error();
	}
}

void FUnrealEnginePythonModule::ShutdownModule()
{
	// This function may be called during shutdown to clean up your module.  For modules that support dynamic reloading,
	// we call this function before unloading the module.

	UE_LOG(LogPython, Log, TEXT("Goodbye Python"));
	Py_Finalize();
}

#undef LOCTEXT_NAMESPACE

IMPLEMENT_MODULE(FUnrealEnginePythonModule, UnrealEnginePython)

<commit_msg>time to intruce threads, they are required for proper qt5 integration<commit_after>// Copyright 1998-2016 Epic Games, Inc. All Rights Reserved.

#include "UnrealEnginePythonPrivatePCH.h"

void unreal_engine_init_py_module();

#define LOCTEXT_NAMESPACE "FUnrealEnginePythonModule"

void FUnrealEnginePythonModule::StartupModule()
{
	// This code will execute after your module is loaded into memory; the exact timing is specified in the .uplugin file per-module

	Py_Initialize();
	wchar_t *argv[] = { UTF8_TO_TCHAR("UnrealEngine"), NULL };
	PySys_SetArgv(1, argv);

	PyEval_InitThreads();

	unreal_engine_init_py_module();

	PyObject *py_sys = PyImport_ImportModule("sys");
	PyObject *py_sys_dict = PyModule_GetDict(py_sys);

	PyObject *py_path = PyDict_GetItemString(py_sys_dict, "path");

	char *zip_path = TCHAR_TO_UTF8(*FPaths::Combine(*FPaths::GameContentDir(), UTF8_TO_TCHAR("python35.zip")));
	PyObject *py_zip_path = PyUnicode_FromString(zip_path);
	PyList_Insert(py_path, 0, py_zip_path);

	char *scripts_path = TCHAR_TO_UTF8(*FPaths::Combine(*FPaths::GameContentDir(), UTF8_TO_TCHAR("Scripts")));
	PyObject *py_scripts_path = PyUnicode_FromString(scripts_path);
	PyList_Insert(py_path, 0, py_scripts_path);

	UE_LOG(LogPython, Log , TEXT("Python VM initialized: %s"), UTF8_TO_TCHAR(Py_GetVersion()));
	UE_LOG(LogPython, Log, TEXT("Python Scripts search path: %s"), UTF8_TO_TCHAR(scripts_path));

	if (PyImport_ImportModule("ue_site")) {
		UE_LOG(LogPython, Log, TEXT("ue_site Python module successfully imported"));
	}
	else {
		unreal_engine_py_log_error();
	}
}

void FUnrealEnginePythonModule::ShutdownModule()
{
	// This function may be called during shutdown to clean up your module.  For modules that support dynamic reloading,
	// we call this function before unloading the module.

	UE_LOG(LogPython, Log, TEXT("Goodbye Python"));
	Py_Finalize();
}

#undef LOCTEXT_NAMESPACE

IMPLEMENT_MODULE(FUnrealEnginePythonModule, UnrealEnginePython)

<|endoftext|>


<commit_before>/*
 *  Created on: 15 Jul 2014
 *      Author: Yiming Yang
 *
 * Copyright (c) 2016, University Of Edinburgh
 * 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  nor the names of its contributors may be used to
 *    endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <exotica/Problems/UnconstrainedEndPoseProblem.h>
#include <exotica/Setup.h>

REGISTER_PROBLEM_TYPE("UnconstrainedEndPoseProblem", exotica::UnconstrainedEndPoseProblem)

namespace exotica
{
    UnconstrainedEndPoseProblem::UnconstrainedEndPoseProblem()
    {
        Flags = KIN_FK | KIN_J;
    }

    UnconstrainedEndPoseProblem::~UnconstrainedEndPoseProblem()
    {
    }

    void UnconstrainedEndPoseProblem::Instantiate(UnconstrainedEndPoseProblemInitializer& init)
    {
        NumTasks = Tasks.size();
        PhiN = 0;
        JN = 0;
        for(int i=0;i<NumTasks;i++)
        {
            appendVector(y.map, Tasks[i]->getLieGroupIndices());
            PhiN += Tasks[i]->Length;
            JN += Tasks[i]->LengthJ;
        }

        Rho = Eigen::VectorXd::Ones(NumTasks);
        y.setZero(PhiN);
        W = Eigen::MatrixXd::Identity(N, N);
        if(init.W.rows()>0)
        {
            if(init.W.rows()==N)
            {
                W.diagonal() = init.W;
            }
            else
            {
                throw_named("W dimension mismatch! Expected "<<N<<", got "<<init.W.rows());
            }
        }
        Phi = y;
        J = Eigen::MatrixXd(JN, N);

        qNominal = init.NominalState;

        if(init.Rho.rows()>0 && init.Rho.rows()!=NumTasks) throw_named("Invalid size of Rho (" << init.Rho.rows() << ") expected: "<< NumTasks);
        if(init.Goal.rows()>0 && init.Goal.rows()!=PhiN) throw_named("Invalid size of Rho (" << init.Goal.rows() << ") expected: "<< PhiN);

        if(init.Rho.rows()==NumTasks) Rho = init.Rho;
        if(init.Goal.rows()==PhiN) y.data = init.Goal;
    }

    void UnconstrainedEndPoseProblem::Update(Eigen::VectorXdRefConst x)
    {
        scene_->Update(x);
        for(int i=0;i<NumTasks;i++)
        {
            Tasks[i]->update(x, Phi.data.segment(Tasks[i]->Start, Tasks[i]->Length), J.middleRows(Tasks[i]->StartJ, Tasks[i]->LengthJ));
        }
    }

    void UnconstrainedEndPoseProblem::setGoal(const std::string & task_name, Eigen::VectorXdRefConst goal)
    {
        try
        {
            TaskMap_ptr task = TaskMaps.at(task_name);
            if(goal.rows()!=task->Length) throw_named("Invalid goal dimension "<<goal.rows()<<" expected "<<task->Length);
            y.data.segment(task->Start, task->Length) = goal;
        }
        catch(std::out_of_range& e)
        {
            throw_pretty("Cannot set Goal. Task map '"<<task_name<<"' does not exist.");
        }
    }

    void UnconstrainedEndPoseProblem::setRho(const std::string & task_name, const double rho)
    {
        try
        {
            TaskMap_ptr task = TaskMaps.at(task_name);
            Rho(task->Id) = rho;
        }
        catch(std::out_of_range& e)
        {
            throw_pretty("Cannot set Rho. Task map '"<<task_name<<"' does not exist.");
        }
    }

    Eigen::VectorXd UnconstrainedEndPoseProblem::getGoal(const std::string & task_name)
    {
        try
        {
            TaskMap_ptr task = TaskMaps.at(task_name);
            return y.data.segment(task->Start, task->Length);
        }
        catch(std::out_of_range& e)
        {
            throw_pretty("Cannot get Goal. Task map '"<<task_name<<"' does not exist.");
        }
    }

    double UnconstrainedEndPoseProblem::getRho(const std::string & task_name)
    {
        try
        {
            TaskMap_ptr task = TaskMaps.at(task_name);
            return Rho(task->Id);
        }
        catch(std::out_of_range& e)
        {
            throw_pretty("Cannot get Rho. Task map '"<<task_name<<"' does not exist.");
        }
    }

    Eigen::VectorXd UnconstrainedEndPoseProblem::getNominalPose()
    {
        return qNominal;
    }

    void UnconstrainedEndPoseProblem::setNominalPose(Eigen::VectorXdRefConst qNominal_in)
    {
      if (qNominal_in.rows() == N)
        qNominal = qNominal_in;
      else
        throw_pretty("Cannot set qNominal - wrong number of rows (expected "
                     << N << ", received " << qNominal_in.rows() << ").");
    }
}

<commit_msg>Fixed error printout<commit_after>/*
 *  Created on: 15 Jul 2014
 *      Author: Yiming Yang
 *
 * Copyright (c) 2016, University Of Edinburgh
 * 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  nor the names of its contributors may be used to
 *    endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <exotica/Problems/UnconstrainedEndPoseProblem.h>
#include <exotica/Setup.h>

REGISTER_PROBLEM_TYPE("UnconstrainedEndPoseProblem", exotica::UnconstrainedEndPoseProblem)

namespace exotica
{
    UnconstrainedEndPoseProblem::UnconstrainedEndPoseProblem()
    {
        Flags = KIN_FK | KIN_J;
    }

    UnconstrainedEndPoseProblem::~UnconstrainedEndPoseProblem()
    {
    }

    void UnconstrainedEndPoseProblem::Instantiate(UnconstrainedEndPoseProblemInitializer& init)
    {
        NumTasks = Tasks.size();
        PhiN = 0;
        JN = 0;
        for(int i=0;i<NumTasks;i++)
        {
            appendVector(y.map, Tasks[i]->getLieGroupIndices());
            PhiN += Tasks[i]->Length;
            JN += Tasks[i]->LengthJ;
        }

        Rho = Eigen::VectorXd::Ones(NumTasks);
        y.setZero(PhiN);
        W = Eigen::MatrixXd::Identity(N, N);
        if(init.W.rows()>0)
        {
            if(init.W.rows()==N)
            {
                W.diagonal() = init.W;
            }
            else
            {
                throw_named("W dimension mismatch! Expected "<<N<<", got "<<init.W.rows());
            }
        }
        Phi = y;
        J = Eigen::MatrixXd(JN, N);

        qNominal = init.NominalState;

        if(init.Rho.rows()>0 && init.Rho.rows()!=NumTasks) throw_named("Invalid size of Rho (" << init.Rho.rows() << ") expected: "<< NumTasks);
        if(init.Goal.rows()>0 && init.Goal.rows()!=PhiN) throw_named("Invalid size of Goal (" << init.Goal.rows() << ") expected: "<< PhiN);

        if(init.Rho.rows()==NumTasks) Rho = init.Rho;
        if(init.Goal.rows()==PhiN) y.data = init.Goal;
    }

    void UnconstrainedEndPoseProblem::Update(Eigen::VectorXdRefConst x)
    {
        scene_->Update(x);
        for(int i=0;i<NumTasks;i++)
        {
            Tasks[i]->update(x, Phi.data.segment(Tasks[i]->Start, Tasks[i]->Length), J.middleRows(Tasks[i]->StartJ, Tasks[i]->LengthJ));
        }
    }

    void UnconstrainedEndPoseProblem::setGoal(const std::string & task_name, Eigen::VectorXdRefConst goal)
    {
        try
        {
            TaskMap_ptr task = TaskMaps.at(task_name);
            if(goal.rows()!=task->Length) throw_named("Invalid goal dimension "<<goal.rows()<<" expected "<<task->Length);
            y.data.segment(task->Start, task->Length) = goal;
        }
        catch(std::out_of_range& e)
        {
            throw_pretty("Cannot set Goal. Task map '"<<task_name<<"' does not exist.");
        }
    }

    void UnconstrainedEndPoseProblem::setRho(const std::string & task_name, const double rho)
    {
        try
        {
            TaskMap_ptr task = TaskMaps.at(task_name);
            Rho(task->Id) = rho;
        }
        catch(std::out_of_range& e)
        {
            throw_pretty("Cannot set Rho. Task map '"<<task_name<<"' does not exist.");
        }
    }

    Eigen::VectorXd UnconstrainedEndPoseProblem::getGoal(const std::string & task_name)
    {
        try
        {
            TaskMap_ptr task = TaskMaps.at(task_name);
            return y.data.segment(task->Start, task->Length);
        }
        catch(std::out_of_range& e)
        {
            throw_pretty("Cannot get Goal. Task map '"<<task_name<<"' does not exist.");
        }
    }

    double UnconstrainedEndPoseProblem::getRho(const std::string & task_name)
    {
        try
        {
            TaskMap_ptr task = TaskMaps.at(task_name);
            return Rho(task->Id);
        }
        catch(std::out_of_range& e)
        {
            throw_pretty("Cannot get Rho. Task map '"<<task_name<<"' does not exist.");
        }
    }

    Eigen::VectorXd UnconstrainedEndPoseProblem::getNominalPose()
    {
        return qNominal;
    }

    void UnconstrainedEndPoseProblem::setNominalPose(Eigen::VectorXdRefConst qNominal_in)
    {
      if (qNominal_in.rows() == N)
        qNominal = qNominal_in;
      else
        throw_pretty("Cannot set qNominal - wrong number of rows (expected "
                     << N << ", received " << qNominal_in.rows() << ").");
    }
}

<|endoftext|>


<commit_before>#include "_CONSTANTS.hpp"

#include "server.hpp"

#include "hooker.hpp"
#include "logging.hpp"
#include "john.hpp"
#include "text_message.hpp"
#include "ini_reader.hpp"

#include <sstream>

namespace jnet {
	server::server() : _config(nullptr)
	{
		load_configuration();
	}
	server::~server() {}

	void server::update() {
		timeout_clients();
		_worker_send_messages();
	}
	void server::timeout_clients() {
		std::vector<std::string> to_delete;

		std::lock_guard<std::mutex> lock(_clientsMutex);

		for (auto conn_ref : _clients) {
			std::chrono::milliseconds total_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - conn_ref.second->last_packet_time);
			float seconds = total_ms.count() / 1000.0f;
#ifdef DO_TRACE_CONNECTIONS
			LOG(DEBUG) << "[" << conn_ref.first << "] ms=" << total_ms.count() << ", seconds=" << seconds;
#endif
			if (seconds > DISCONNECT_TIMEOUT) {
				to_delete.push_back(conn_ref.first);
			}
		}
		for (auto del : to_delete) {
			LOG(WARNING) << "Lost connection from client [" << del << "]";
			_clients.erase(del);
		}
	}
	void server::reset() {
		std::lock_guard<std::mutex> lock(_clientsMutex);
		_clients.clear();
	}

	void server::queue_message(const std::string & dst, message_p msg) {
		std::lock_guard<std::mutex> lock(_outboundMessageQueueMutex);

		_outboundMessageQueue.push_back(std::make_pair(dst, msg) );
	}
	void server::_worker_send_messages() {
		if (_clients.size() < 1 || _outboundMessageQueue.size() < 1)
			return;

		{
			std::lock_guard<std::mutex> lock_queue(_outboundMessageQueueMutex);
			std::lock_guard<std::mutex> lock_clients(_clientsMutex);

			for (auto msg_pair : _outboundMessageQueue) {
				if (msg_pair.first == "") {
					LOG(DEBUG) << "Broadcasting message";
					for (auto client : _clients) {
						sendto(client.second->socket,
							msg_pair.second->buffer, msg_pair.second->length, 0,
							reinterpret_cast<struct sockaddr *>(&client.second->addr), client.second->addr_len);
					}
				} else {
					if (_clients.find(msg_pair.first) == _clients.end()) {
						continue;
					}

					LOG(DEBUG) << "Sending targeted message to [" << msg_pair.first << "]";
					sendto(_clients[msg_pair.first]->socket,
						msg_pair.second->buffer, msg_pair.second->length, 0,
						reinterpret_cast<struct sockaddr *>(&_clients[msg_pair.first]->addr), _clients[msg_pair.first]->addr_len);
				}
			}

			_outboundMessageQueue.clear();
		}
	}

	int server::recv(const connection_p conn, const message_p message) {
		//Check for headers, otherwise its a binary packet
		if (message->length > 4) {
			if (memcmp(msg_hello+5, message->buffer, 5) != 0) {

				text_message msg(const_cast<char *>(message->buffer), message->length);

				if (msg.getProcedureNameAsString().size() < 1)
					return 0;

				LOG(DEBUG) << "SRV NETWORK [" << msg.getData() << "]";

				if (msg.getProcedureNameAsString() == "client_ready") {
					std::string settings = compile_settings_message();

					if (msg.getParameterCount() > 0)
						conn->name = msg.getParameterAsString(0);

					LOG(INFO) << "Synchronizing with [" << conn->name << " @ " << conn->id << "]";

					text_message_p msg_config(text_message::formatNewMessage(
						"JNET server_settings:%1",
						const_cast<char *>(settings.c_str())
					));

					sys_sendto(conn->socket,
						reinterpret_cast<char *>(msg_config->getData()), msg_config->getLength(),
						0, reinterpret_cast<struct sockaddr *>(&conn->addr), conn->addr_len);
				}

				return 0;
			} else {			// GENERIC HELLO packets get handled seperate.
				h_sendto(message->socket, msg_welcome, 13, 0, message->addr, message->addr_len);

				{
					std::lock_guard<std::mutex> lock(_clientsMutex);
					if (_clients.find(conn->id) == _clients.end()) {
						LOG(DEBUG) << "New JNET Client Peer - {" << message->src->id << "}";
						_clients[conn->id] = conn;
						handle_new_client(conn);
					}
				}
			}
		}

		return -1;
	}
	std::string server::rv_command(const std::string &cmd) {
		std::string ret = "";

		LOG(DEBUG) << "SRV ["<< cmd << "]";
		text_message msg(const_cast<char *>(cmd.c_str()), cmd.size());

		if (msg.getProcedureNameAsString() == "getServerConfig") {
			std::string requestedConfig = msg.getParameterAsString(0);

			if(requestedConfig.find_first_of("jvon_") == 0 || requestedConfig.find_first_of("jnet_") == 0) {
				ret = config().Get("", requestedConfig, "");
			} else {
				ret = "You naughty little fucker";
			}
			LOG(DEBUG) << "return" << ret;
			return ret;
		} else if(msg.getProcedureNameAsString() == "broadcastSqf") {
			ret = "NOT_IMPLEMENTED";
		}

		return ret;
	}

	void server::handle_new_client(connection_p conn) {
		// Hardcode handling and sending JVON data here
	}
	std::string server::compile_settings_message() {
		std::stringstream str;

		LOG(INFO) << "Configuration File Loaded";

		str << "[jvon_settings]";
		str << "jvon_launcher = " << config().GetInteger("", "jvon_launcher", 0) << ";\n";
		str << "jvon_server_address = \"" << config().Get("", "jvon_server_address", "") << "\";\n";
		str << "jvon_password = \"" << config().Get("", "jvon_password", "") << "\";\n";
		str << "jvon_required = " << config().GetInteger("", "jvon_required", 0) << ";\n";
		str << "jvon_required_timeout = " << config().GetInteger("", "jvon_required_timeout", 0) << ";\n";
		str << "jvon_extended_config = " << config().Get("", "jvon_extended_config", "") << ";\n";

		LOG(DEBUG) << "Sending: " << str.str();

		return str.str();
	}
	void server::load_configuration() {
		// Since we are a server process, we need to determine the config file location. Either it will be command-line provided, or the default arma3 path.
		// Extract the config name
		std::string cfg_name = "";

		std::string cmd_line = get_cmdline();
		LOG(DEBUG) << "Server Parameters: " << cmd_line;

		if (cmd_line.find("-config") == std::string::npos) {
			LOG(DEBUG) << "* No server configuration detected; falling back on defaults";

		} else {
			if (cmd_line.find_first_of("-config") != std::string::npos) {
				cfg_name = cmd_line.substr(cmd_line.find("-config") + 8, cmd_line.size() - (cmd_line.find("-config") + 8));

				size_t end_index = cfg_name.find_first_of(" ");
				if (cfg_name.find_first_of(" ") == std::string::npos)
					end_index = cfg_name.size();
				cfg_name.resize(end_index);

			} else if (cmd_line.find_first_of("-cfg") != std::string::npos) {
				cfg_name = cmd_line.substr(cmd_line.find("-cfg") + 8, cmd_line.size() - (cmd_line.find("-cfg") + 8));
				size_t end_index = cfg_name.find_first_of(" ");
				if (cfg_name.find_first_of(" ") == std::string::npos)
					end_index = cfg_name.size();
				cfg_name.resize(end_index);
			} else {
				cfg_name = "";
			}
			LOG(DEBUG) << "Determined config name: '" << cfg_name << "'";

			std::ifstream file(cfg_name);
			if (!file) {
				LOG(DEBUG) << "Couldn't open server configuration file.";
			} else {
				// Close our test
				file.close();
				LOG(INFO) << "Loading server configuration {" << cfg_name << "}";
			}
		}
		_config = new ini_reader(cfg_name);

		if (config().ParseError() < 0) {
			LOG(ERROR) << "Can't load server configuration '" << cfg_name << "'";
			return;
		}
#ifdef _DEAD	// This was testing with our JVON configurations. We should implement fetching these from command line.
		LOG(INFO) << "Configuration File Loaded";
		LOG(INFO) << "\t jvon_launcher = " << config().GetInteger("", "jvon_launcher", 0);
		LOG(INFO) << "\t jvon_host = " << config().Get("", "jvon_host", "");
		LOG(INFO) << "\t jvon_host_port = " << config().GetInteger("", "jvon_host_port", 0);
		LOG(INFO) << "\t jvon_server_config = " << config().Get("", "jvon_server_config", "@JVON\jvon_server.json");
		LOG(INFO) << "\t jvon_password = " << config().Get("", "jvon_password", "");
		LOG(INFO) << "\t jvon_required = " << config().GetInteger("", "jvon_required", 0);
		LOG(INFO) << "\t jvon_required_timeout = " << config().GetInteger("", "jvon_required_timeout", 0);
		LOG(INFO) << "\t jvon_extended_config = " << config().Get("", "jvon_extended_config", "");
#endif
	}
};
<commit_msg>JNET handling par startup files too.<commit_after>#include "_CONSTANTS.hpp"

#include "server.hpp"

#include "hooker.hpp"
#include "logging.hpp"
#include "john.hpp"
#include "text_message.hpp"
#include "ini_reader.hpp"

#include <sstream>

namespace jnet {
	server::server() : _config(nullptr)
	{
		load_configuration();
	}
	server::~server() {}

	void server::update() {
		timeout_clients();
		_worker_send_messages();
	}
	void server::timeout_clients() {
		std::vector<std::string> to_delete;

		std::lock_guard<std::mutex> lock(_clientsMutex);

		for (auto conn_ref : _clients) {
			std::chrono::milliseconds total_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - conn_ref.second->last_packet_time);
			float seconds = total_ms.count() / 1000.0f;
#ifdef DO_TRACE_CONNECTIONS
			LOG(DEBUG) << "[" << conn_ref.first << "] ms=" << total_ms.count() << ", seconds=" << seconds;
#endif
			if (seconds > DISCONNECT_TIMEOUT) {
				to_delete.push_back(conn_ref.first);
			}
		}
		for (auto del : to_delete) {
			LOG(WARNING) << "Lost connection from client [" << del << "]";
			_clients.erase(del);
		}
	}
	void server::reset() {
		std::lock_guard<std::mutex> lock(_clientsMutex);
		_clients.clear();
	}

	void server::queue_message(const std::string & dst, message_p msg) {
		std::lock_guard<std::mutex> lock(_outboundMessageQueueMutex);

		_outboundMessageQueue.push_back(std::make_pair(dst, msg) );
	}
	void server::_worker_send_messages() {
		if (_clients.size() < 1 || _outboundMessageQueue.size() < 1)
			return;

		{
			std::lock_guard<std::mutex> lock_queue(_outboundMessageQueueMutex);
			std::lock_guard<std::mutex> lock_clients(_clientsMutex);

			for (auto msg_pair : _outboundMessageQueue) {
				if (msg_pair.first == "") {
					LOG(DEBUG) << "Broadcasting message";
					for (auto client : _clients) {
						sendto(client.second->socket,
							msg_pair.second->buffer, msg_pair.second->length, 0,
							reinterpret_cast<struct sockaddr *>(&client.second->addr), client.second->addr_len);
					}
				} else {
					if (_clients.find(msg_pair.first) == _clients.end()) {
						continue;
					}

					LOG(DEBUG) << "Sending targeted message to [" << msg_pair.first << "]";
					sendto(_clients[msg_pair.first]->socket,
						msg_pair.second->buffer, msg_pair.second->length, 0,
						reinterpret_cast<struct sockaddr *>(&_clients[msg_pair.first]->addr), _clients[msg_pair.first]->addr_len);
				}
			}

			_outboundMessageQueue.clear();
		}
	}

	int server::recv(const connection_p conn, const message_p message) {
		//Check for headers, otherwise its a binary packet
		if (message->length > 4) {
			if (memcmp(msg_hello+5, message->buffer, 5) != 0) {

				text_message msg(const_cast<char *>(message->buffer), message->length);

				if (msg.getProcedureNameAsString().size() < 1)
					return 0;

				LOG(DEBUG) << "SRV NETWORK [" << msg.getData() << "]";

				if (msg.getProcedureNameAsString() == "client_ready") {
					std::string settings = compile_settings_message();

					if (msg.getParameterCount() > 0)
						conn->name = msg.getParameterAsString(0);

					LOG(INFO) << "Synchronizing with [" << conn->name << " @ " << conn->id << "]";

					text_message_p msg_config(text_message::formatNewMessage(
						"JNET server_settings:%1",
						const_cast<char *>(settings.c_str())
					));

					sys_sendto(conn->socket,
						reinterpret_cast<char *>(msg_config->getData()), msg_config->getLength(),
						0, reinterpret_cast<struct sockaddr *>(&conn->addr), conn->addr_len);
				}

				return 0;
			} else {			// GENERIC HELLO packets get handled seperate.
				h_sendto(message->socket, msg_welcome, 13, 0, message->addr, message->addr_len);

				{
					std::lock_guard<std::mutex> lock(_clientsMutex);
					if (_clients.find(conn->id) == _clients.end()) {
						LOG(DEBUG) << "New JNET Client Peer - {" << message->src->id << "}";
						_clients[conn->id] = conn;
						handle_new_client(conn);
					}
				}
			}
		}

		return -1;
	}
	std::string server::rv_command(const std::string &cmd) {
		std::string ret = "";

		LOG(DEBUG) << "SRV ["<< cmd << "]";
		text_message msg(const_cast<char *>(cmd.c_str()), cmd.size());

		if (msg.getProcedureNameAsString() == "getServerConfig") {
			std::string requestedConfig = msg.getParameterAsString(0);

			if(requestedConfig.find_first_of("jvon_") == 0 || requestedConfig.find_first_of("jnet_") == 0) {
				ret = config().Get("", requestedConfig, "");
			} else {
				ret = "You naughty little fucker";
			}
			LOG(DEBUG) << "return" << ret;
			return ret;
		} else if(msg.getProcedureNameAsString() == "broadcastSqf") {
			ret = "NOT_IMPLEMENTED";
		}

		return ret;
	}

	void server::handle_new_client(connection_p conn) {
		// Hardcode handling and sending JVON data here
	}
	std::string server::compile_settings_message() {
		std::stringstream str;

		LOG(INFO) << "Configuration File Loaded";

		str << "[jvon_settings]";
		str << "jvon_launcher = " << config().GetInteger("", "jvon_launcher", 0) << ";\n";
		str << "jvon_server_address = \"" << config().Get("", "jvon_server_address", "") << "\";\n";
		str << "jvon_password = \"" << config().Get("", "jvon_password", "") << "\";\n";
		str << "jvon_required = " << config().GetInteger("", "jvon_required", 0) << ";\n";
		str << "jvon_required_timeout = " << config().GetInteger("", "jvon_required_timeout", 0) << ";\n";
		str << "jvon_extended_config = " << config().Get("", "jvon_extended_config", "") << ";\n";

		LOG(DEBUG) << "Sending: " << str.str();

		return str.str();
	}
	void server::load_configuration() {
		// Since we are a server process, we need to determine the config file location. Either it will be command-line provided, or the default arma3 path.
		// Extract the config name
		std::string cfg_name = "";

		std::string cmd_line = get_cmdline();
		LOG(DEBUG) << "Server Parameters: " << cmd_line;

		if (cmd_line.find_first_of("-par") != std::string::npos) {
			std::string par_path = cmd_line.substr(cmd_line.find("-par") + 5, cmd_line.size() - (cmd_line.find("-par") + 5));

			size_t end_index = par_path.find_first_of(" ");
			if (par_path.find_first_of(" ") == std::string::npos)
				end_index = par_path.size();
			par_path.resize(end_index);

			// We pull the par, condense the \r\n to spaces, then replace cmd_line with it
			std::ifstream par_file(par_path);
			if (par_file) {
				LOG(TRACE) << "Parsing from PAR file";
				std::string par_str((std::istreambuf_iterator<char>(par_file)),
					std::istreambuf_iterator<char>());
				std::replace(par_str.begin(), par_str.end(), '\r', ' ');
				std::replace(par_str.begin(), par_str.end(), '\n', ' ');

				cmd_line = par_str;
			}
		}

		if (cmd_line.find("-config") == std::string::npos) {
			LOG(DEBUG) << "* No server configuration detected; falling back on defaults";

		} else {
			if (cmd_line.find_first_of("-config") != std::string::npos) {
				cfg_name = cmd_line.substr(cmd_line.find("-config") + 8, cmd_line.size() - (cmd_line.find("-config") + 8));

				size_t end_index = cfg_name.find_first_of(" ");
				if (cfg_name.find_first_of(" ") == std::string::npos)
					end_index = cfg_name.size();
				cfg_name.resize(end_index);

			} else if (cmd_line.find_first_of("-cfg") != std::string::npos) {
				cfg_name = cmd_line.substr(cmd_line.find("-cfg") + 8, cmd_line.size() - (cmd_line.find("-cfg") + 8));
				size_t end_index = cfg_name.find_first_of(" ");
				if (cfg_name.find_first_of(" ") == std::string::npos)
					end_index = cfg_name.size();
				cfg_name.resize(end_index);
			} else {
				cfg_name = "";
			}
			LOG(DEBUG) << "Determined config name: '" << cfg_name << "'";

			std::ifstream file(cfg_name);
			if (!file) {
				LOG(DEBUG) << "Couldn't open server configuration file.";
			} else {
				// Close our test
				file.close();
				LOG(INFO) << "Loading server configuration {" << cfg_name << "}";
			}
		}
		_config = new ini_reader(cfg_name);

		if (config().ParseError() < 0) {
			LOG(ERROR) << "Can't load server configuration '" << cfg_name << "'";
			return;
		}
#ifdef _DEAD	// This was testing with our JVON configurations. We should implement fetching these from command line.
		LOG(INFO) << "Configuration File Loaded";
		LOG(INFO) << "\t jvon_launcher = " << config().GetInteger("", "jvon_launcher", 0);
		LOG(INFO) << "\t jvon_host = " << config().Get("", "jvon_host", "");
		LOG(INFO) << "\t jvon_host_port = " << config().GetInteger("", "jvon_host_port", 0);
		LOG(INFO) << "\t jvon_server_config = " << config().Get("", "jvon_server_config", "@JVON\jvon_server.json");
		LOG(INFO) << "\t jvon_password = " << config().Get("", "jvon_password", "");
		LOG(INFO) << "\t jvon_required = " << config().GetInteger("", "jvon_required", 0);
		LOG(INFO) << "\t jvon_required_timeout = " << config().GetInteger("", "jvon_required_timeout", 0);
		LOG(INFO) << "\t jvon_extended_config = " << config().Get("", "jvon_extended_config", "");
#endif
	}
};
<|endoftext|>


<commit_before>#include <sys/wait.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <string.h>
#include <map>
#include <random>
#include <sstream>
#include <set>
#include <fstream>
#include <sys/stat.h>
#include <dirent.h>
#include <unistd.h>
#include "../include/netutils.h"
#include "../include/strutils.h"

void handler(int signum) {
	while (waitpid(-1, NULL, WNOHANG) > 0);
	return;
}

int main() {
	int sockFd;
	if ((sockFd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
		perror("socket() error");
		return 1;
	}

	struct sockaddr_in servAddr;
	bzero(&servAddr, sizeof(servAddr));
	servAddr.sin_family = AF_INET;
	servAddr.sin_port = htons(57345);
	servAddr.sin_addr.s_addr = htonl(INADDR_ANY);

	if (bind(sockFd, (const struct sockaddr *) &servAddr, sizeof(servAddr)) < 0) {
		perror("bind() error");
		return 1;
	}

	if (listen(sockFd, 16) < 0) {
		perror("listen() error");
		return 1;
	}

	std::random_device rd;
	std::mt19937 gen(rd());
	std::uniform_int_distribution<> dis(0, 99999);
	std::multimap<std::string, int> clients;
	bool permanentId = false;

	struct sigaction act;
	act.sa_handler = handler;
	sigemptyset(&act.sa_mask);
	act.sa_flags = SA_RESTART;
	if (sigaction(SIGCHLD, &act, NULL) < 0) {
		perror("sigaction() error");
		return 1;
	}

	char buf[2048], tempBuf[128];
	std::ofstream *pFile = NULL;
	long decoded, fileSize = -1;
	DIR *dp;
	struct dirent *de;

	while (1) {
		int connFd;
		if ((connFd = accept(sockFd, (struct sockaddr *) NULL, NULL)) < 0)
			perror("accept() error");

		if (clients.size() == FD_SETSIZE) {
			fprintf(stderr, "maximum number of clients reached\n");
			continue;
		}

		char key[6];
		std::string clientId;
		while (1) {
			int rdNum = dis(gen);
			sprintf(key, "%05d", rdNum);
			clientId = key;
			if (clients.find(key) == clients.end()) {
				clients.insert(std::make_pair(key, connFd));
				break;
			}
		}

		pid_t pid;
		if ((pid = fork()) < 0)
			perror("fork() error");

		if (pid == 0) {
			if (close(sockFd) < 0)
				perror("close() error");

			strcpy(buf, "IG"); // CLIENT ID GET operation
			strcat(buf, key); // client id, 5 bytes
			if (forcewrite(connFd, buf, 2048) < 0)
				perror("write() error");

			while (1) {
				ssize_t len;
				if ((len = forceread(connFd, buf, 2048, false)) < 0)
					perror("read() error");

				else if (len == 0) {
					fputs("client has closed the connection\n", stdout);

					if (!permanentId) {
						dp = opendir(".");
						while ((de = readdir(dp)))
							if (!memcmp(de->d_name, clientId.c_str(), 5))
								unlink(de->d_name);
						closedir(dp);
					}

					if (close(connFd) < 0)
						perror("close() error");

					_exit(0);
				}

				else if (!memcmp(buf, "GI", 2)) { // GET FILE INITIALIZE operation
					memcpy(tempBuf, buf + 2, 5); // length of file name, 5 bytes
					tempBuf[5] = 0;
					decoded = strtol(tempBuf, NULL, 10);
					memcpy(tempBuf, buf + 7, (size_t) decoded); // file name
					tempBuf[decoded] = 0;
					std::string success(buf + 7 + decoded); // success message

					std::ifstream file(tempBuf, std::ios::in | std::ios::binary);
					struct stat statBuf;
					strcpy(buf, "GE"); // GET FILE ERROR operation
					if (!file.is_open() || stat(tempBuf, &statBuf) < 0)
						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");

					while (file.good()) {
						file.read(buf + 7, 1920); // data to receive
						strcpy(buf, "GD"); // GET FILE DATA operation
						sprintf(tempBuf, "%05d", (int) file.gcount());
						memcpy(buf + 2, tempBuf, 5); // length of data, 5 bytes

						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");
					}

					if (memcmp(buf, "GE", 2)) {
						strcpy(buf, "GF"); // GET FILE FINALIZE operation
						sprintf(tempBuf, "%010d", (int) statBuf.st_size);
						strcat(buf, tempBuf); // length of file, 10 bytes
						strcat(buf, success.c_str()); // success message

						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");

						file.close();
					}
				}

				else if (!memcmp(buf, "PI", 2)) { // PUT FILE INITIALIZE operation
					memcpy(tempBuf, buf + 2, 5); // length of file name, 5 bytes
					tempBuf[5] = 0;
					decoded = strtol(tempBuf, NULL, 10);
					memcpy(tempBuf, buf + 7, (size_t) decoded); // file name
					tempBuf[decoded] = 0;
					fileSize = strtol(buf + 7 + decoded, NULL, 10); // length of file, 10 bytes

					pFile = new std::ofstream(tempBuf,
							std::ios::out | std::ios::trunc | std::ios::binary);
					strcpy(buf, "PE"); // PUT FILE ERROR operation
					if (!pFile->is_open())
						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");
				}

				else if (!memcmp(buf, "PD", 2) && pFile->is_open()) { // PUT FILE DATA operation
					memcpy(tempBuf, buf + 2, 5); // length of data, 5 bytes
					tempBuf[5] = 0;
					decoded = strtol(tempBuf, NULL, 10);
					pFile->write(buf + 7, decoded); // data to send
				}

				else if (!memcmp(buf, "PF", 2) && pFile->is_open()) { // PUT FILE FINALIZE operation
					if (pFile->tellp() == fileSize) {
						memcpy(buf, "MP", 2); // MESSAGE PRINT STDOUT operation
						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");
					}
					else {
						strcpy(buf, "ME"); // MESSAGE PRINT STDERR operation
						strcat(buf, "server's file and client's file are different\n");
								// message to print
						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");
					}
					pFile->close();
				}

				else if (!memcmp(buf, "LR", 2)) { // LIST FILES REQUEST operation
					memcpy(tempBuf, buf + 2, 5); // client id as prefix of file name
					tempBuf[5] = 0;
					strcpy(buf, "MP"); // MESSAGE PRINT STDOUT operation

					dp = opendir(".");
					while ((de = readdir(dp))) {
						if (!memcmp(de->d_name, tempBuf, 5)) {
							strcat(buf, de->d_name + 5); // message to print
							strcat(buf, "\n"); // message to print
						}
					}
					closedir(dp);

					strcat(buf, "LIST succeeded\n"); // message to print
					if (forcewrite(connFd, buf, 2048) < 0)
						perror("write() error");
				}

				else if (!memcmp(buf, "IS", 2)) { // CLIENT ID SET operation
					memcpy(tempBuf, buf + 2, 5); // client id, 5 bytes
					tempBuf[5] = 0;
					clientId = tempBuf;

					if (!permanentId) {
						dp = opendir(".");
						while ((de = readdir(dp)))
							if (!memcmp(de->d_name, clientId.c_str(), 5))
								unlink(de->d_name);
						closedir(dp);
					}
					permanentId = true;
				}

				else {
					strcpy(buf, "ME"); // MESSAGE PRINT STDERR operation
					strcat(buf, "invalid request\n"); // message to print
					if (forcewrite(connFd, buf, 2048) < 0)
						perror("write() error");
				}
			}
		}

		if (close(connFd) < 0)
			perror("close() error");
	}
}
<commit_msg>All is done.<commit_after>#include <sys/wait.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <string.h>
#include <map>
#include <random>
#include <sstream>
#include <set>
#include <fstream>
#include <sys/stat.h>
#include <dirent.h>
#include <unistd.h>
#include "../include/netutils.h"
#include "../include/strutils.h"

void handler(int signum) {
	while (waitpid(-1, NULL, WNOHANG) > 0);
	return;
}

int main() {
	int sockFd;
	if ((sockFd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
		perror("socket() error");
		return 1;
	}

	struct sockaddr_in servAddr;
	bzero(&servAddr, sizeof(servAddr));
	servAddr.sin_family = AF_INET;
	servAddr.sin_port = htons(57345);
	servAddr.sin_addr.s_addr = htonl(INADDR_ANY);

	if (bind(sockFd, (const struct sockaddr *) &servAddr, sizeof(servAddr)) < 0) {
		perror("bind() error");
		return 1;
	}

	if (listen(sockFd, 16) < 0) {
		perror("listen() error");
		return 1;
	}

	std::random_device rd;
	std::mt19937 gen(rd());
	std::uniform_int_distribution<> dis(0, 99999);
	std::multimap<std::string, int> clients;
	bool permanentId = false;

	struct sigaction act;
	act.sa_handler = handler;
	sigemptyset(&act.sa_mask);
	act.sa_flags = SA_RESTART;
	if (sigaction(SIGCHLD, &act, NULL) < 0) {
		perror("sigaction() error");
		return 1;
	}

	char buf[2048], tempBuf[128];
	std::ofstream *pFile = NULL;
	long decoded, fileSize = -1;
	DIR *dp;
	struct dirent *de;

	while (1) {
		int connFd;
		if ((connFd = accept(sockFd, (struct sockaddr *) NULL, NULL)) < 0)
			perror("accept() error");

		if (clients.size() == FD_SETSIZE) {
			fprintf(stderr, "maximum number of clients reached\n");
			continue;
		}

		char key[6];
		std::string clientId;
		while (1) {
			int rdNum = dis(gen);
			sprintf(key, "%05d", rdNum);
			clientId = key;
			if (clients.find(key) == clients.end()) {
				clients.insert(std::make_pair(key, connFd));
				break;
			}
		}

		pid_t pid;
		if ((pid = fork()) < 0)
			perror("fork() error");

		if (pid == 0) {
			if (close(sockFd) < 0)
				perror("close() error");

			strcpy(buf, "IG"); // CLIENT ID GET operation
			strcat(buf, key); // client id, 5 bytes
			if (forcewrite(connFd, buf, 2048) < 0)
				perror("write() error");

			while (1) {
				ssize_t len;
				if ((len = forceread(connFd, buf, 2048, false)) < 0)
					perror("read() error");

				else if (len == 0) {
					fputs("client has closed the connection\n", stdout);

					if (!permanentId) {
						dp = opendir(".");
						while ((de = readdir(dp)))
							if (!memcmp(de->d_name, clientId.c_str(), 5))
								unlink(de->d_name);
						closedir(dp);
					}

					if (close(connFd) < 0)
						perror("close() error");

					_exit(0);
				}

				else if (!memcmp(buf, "GI", 2)) { // GET FILE INITIALIZE operation
					memcpy(tempBuf, buf + 2, 5); // length of file name, 5 bytes
					tempBuf[5] = 0;
					decoded = strtol(tempBuf, NULL, 10);
					memcpy(tempBuf, buf + 7, (size_t) decoded); // file name
					tempBuf[decoded] = 0;
					std::string success(buf + 7 + decoded); // success message

					std::ifstream file(tempBuf, std::ios::in | std::ios::binary);
					struct stat statBuf;
					strcpy(buf, "GE"); // GET FILE ERROR operation
					if (!file.is_open() || stat(tempBuf, &statBuf) < 0)
						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");

					while (file.good()) {
						file.read(buf + 7, 1920); // data to receive
						strcpy(buf, "GD"); // GET FILE DATA operation
						sprintf(tempBuf, "%05d", (int) file.gcount());
						memcpy(buf + 2, tempBuf, 5); // length of data, 5 bytes

						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");
					}

					if (memcmp(buf, "GE", 2)) {
						strcpy(buf, "GF"); // GET FILE FINALIZE operation
						sprintf(tempBuf, "%010d", (int) statBuf.st_size);
						strcat(buf, tempBuf); // length of file, 10 bytes
						strcat(buf, success.c_str()); // success message

						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");

						file.close();
					}
				}

				else if (!memcmp(buf, "PI", 2)) { // PUT FILE INITIALIZE operation
					memcpy(tempBuf, buf + 2, 5); // length of file name, 5 bytes
					tempBuf[5] = 0;
					decoded = strtol(tempBuf, NULL, 10);
					memcpy(tempBuf, buf + 7, (size_t) decoded); // file name
					tempBuf[decoded] = 0;
					fileSize = strtol(buf + 7 + decoded, NULL, 10); // length of file, 10 bytes

					pFile = new std::ofstream(tempBuf,
							std::ios::out | std::ios::trunc | std::ios::binary);
					strcpy(buf, "PE"); // PUT FILE ERROR operation
					if (!pFile->is_open())
						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");
				}

				else if (!memcmp(buf, "PD", 2) && pFile->is_open()) { // PUT FILE DATA operation
					memcpy(tempBuf, buf + 2, 5); // length of data, 5 bytes
					tempBuf[5] = 0;
					decoded = strtol(tempBuf, NULL, 10);
					pFile->write(buf + 7, decoded); // data to send
				}

				else if (!memcmp(buf, "PF", 2) && pFile->is_open()) { // PUT FILE FINALIZE operation
					if (pFile->tellp() == fileSize) {
						memcpy(buf, "MP", 2); // MESSAGE PRINT STDOUT operation
						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");
					}
					else {
						strcpy(buf, "ME"); // MESSAGE PRINT STDERR operation
						strcat(buf, "server's file and client's file are different\n");
								// message to print
						if (forcewrite(connFd, buf, 2048) < 0)
							perror("write() error");
					}
					pFile->close();
				}

				else if (!memcmp(buf, "LR", 2)) { // LIST FILES REQUEST operation
					memcpy(tempBuf, buf + 2, 5); // client id as prefix of file name
					tempBuf[5] = 0;
					strcpy(buf, "MP"); // MESSAGE PRINT STDOUT operation

					dp = opendir(".");
					while ((de = readdir(dp))) {
						if (!memcmp(de->d_name, tempBuf, 5)) {
							strcat(buf, de->d_name + 5); // message to print
							strcat(buf, "\n"); // message to print
						}
					}
					closedir(dp);

					strcat(buf, "LIST succeeded\n"); // message to print
					if (forcewrite(connFd, buf, 2048) < 0)
						perror("write() error");
				}

				else if (!memcmp(buf, "IS", 2)) { // CLIENT ID SET operation
					memcpy(tempBuf, buf + 2, 5); // client id, 5 bytes
					tempBuf[5] = 0;

					if (!permanentId) {
						dp = opendir(".");
						while ((de = readdir(dp)))
							if (!memcmp(de->d_name, clientId.c_str(), 5))
								unlink(de->d_name);
						closedir(dp);
					}
					permanentId = true;

					clientId = tempBuf;
				}

				else {
					strcpy(buf, "ME"); // MESSAGE PRINT STDERR operation
					strcat(buf, "invalid request\n"); // message to print
					if (forcewrite(connFd, buf, 2048) < 0)
						perror("write() error");
				}
			}
		}

		if (close(connFd) < 0)
			perror("close() error");
	}
}
<|endoftext|>


<commit_before>#include <map>

#include <ros/ros.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <actionlib/server/simple_action_server.h>
#include <std_msgs/Float32.h>

typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;

std::map<std::string, ros::Publisher> g_gripper_servo_angle_pub;

void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as_, const std::string& side)
{
  bool success = true;
  std::string node_name(ros::this_node::getName());

  ROS_INFO("%s: Executing requested joint trajectory for %s gripper", node_name.c_str(), side.c_str());

  // Wait for the specified execution time, if not provided use now
  ros::Time start_time = goal->trajectory.header.stamp;
  if (start_time == ros::Time(0, 0))
    start_time = ros::Time::now();
  ros::Duration wait = start_time - ros::Time::now();
  if (wait > ros::Duration(0))
    wait.sleep();

  // Loop until end of trajectory
  for (int i = 0; i < goal->trajectory.points.size(); i++)
  {
    if (as_->isPreemptRequested())
    {
      ROS_INFO("%s: Preempted for %s gripper", node_name.c_str(), side.c_str());
      as_->setPreempted();
      success = false;
      break;
    }

    if (!ros::ok())
    {
      ROS_INFO("%s: Aborted for %s gripper", node_name.c_str(), side.c_str());
      as_->setAborted();
      return;
    }

    float rad = goal->trajectory.points[i].positions[0];
    std_msgs::Float32 angle_msg;
    angle_msg.data = rad;
    g_gripper_servo_angle_pub[side].publish(angle_msg);

    // Publish feedbacks until next point
    ros::Rate feedback_rate(100);
    do
    {
      feedback_rate.sleep();

      control_msgs::FollowJointTrajectoryFeedback feedback_;

      feedback_.joint_names.push_back(goal->trajectory.joint_names[0]);
      feedback_.desired.positions.resize(1);
      feedback_.actual.positions.resize(1);
      feedback_.error.positions.resize(1);

      feedback_.desired.positions[0] = rad;
      feedback_.actual.positions[0] = rad;
      feedback_.error.positions[0] = 0;

      ros::Time now = ros::Time::now();
      feedback_.desired.time_from_start = now - start_time;
      feedback_.actual.time_from_start = now - start_time;
      feedback_.error.time_from_start = now - start_time;
      feedback_.header.stamp = ros::Time::now();
      as_->publishFeedback(feedback_);
    } while (ros::ok() && i < (goal->trajectory.points.size() - 1) &&
             ros::Time::now() < (goal->trajectory.header.stamp + goal->trajectory.points[i + 1].time_from_start));
  }

  control_msgs::FollowJointTrajectoryResult result_;
  if (success)
  {
    ROS_INFO("%s: Succeeded for %s gripper", node_name.c_str(), side.c_str());
    result_.error_code = result_.SUCCESSFUL;
    as_->setSucceeded(result_);
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "gripper_joint_trajectory_action_server");
  ros::NodeHandle n;
  g_gripper_servo_angle_pub["right"] = n.advertise<std_msgs::Float32>("gripper_front/limb/right/servo/angle", 10);
  g_gripper_servo_angle_pub["left"] = n.advertise<std_msgs::Float32>("gripper_front/limb/left/servo/angle", 10);
  Server right_server(n, "gripper_front/limb/right/follow_joint_trajectory",
                      boost::bind(&execute, _1, &right_server, "right"), false);
  Server left_server(n, "gripper_front/limb/left/follow_joint_trajectory",
                     boost::bind(&execute, _1, &left_server, "left"), false);
  right_server.start();
  left_server.start();
  ros::spin();
  return 0;
}
<commit_msg>minjerk and continuous feedback<commit_after>#include <map>

#include <ros/ros.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <actionlib/server/simple_action_server.h>
#include <std_msgs/Float32.h>

typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;

std::map<std::string, ros::Publisher> g_gripper_servo_angle_pub;

void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as_, const std::string& side)
{
  bool success = true;
  std::string node_name(ros::this_node::getName());

  ROS_INFO("%s: Executing requested joint trajectory for %s gripper", node_name.c_str(), side.c_str());

  // Wait for the specified execution time, if not provided use now
  ros::Time start_time = goal->trajectory.header.stamp;
  if (start_time == ros::Time(0, 0))
    start_time = ros::Time::now();
  ros::Duration wait = start_time - ros::Time::now();
  if (wait > ros::Duration(0))
    wait.sleep();
  ros::Time start_segment_time = start_time;
  float prev_rad = 0;

  // Loop until end of trajectory
  for (int i = 0; i < goal->trajectory.points.size(); i++)
  {
    ros::Duration segment_time = goal->trajectory.points[i].time_from_start;
    if ( i > 0 )
      segment_time = segment_time -  goal->trajectory.points[i-1].time_from_start;
    if (as_->isPreemptRequested())
    {
      ROS_INFO("%s: Preempted for %s gripper", node_name.c_str(), side.c_str());
      as_->setPreempted();
      success = false;
      break;
    }

    if (!ros::ok())
    {
      ROS_INFO("%s: Aborted for %s gripper", node_name.c_str(), side.c_str());
      as_->setAborted();
      return;
    }

    float rad = goal->trajectory.points[i].positions[0];
    std_msgs::Float32 angle_msg;

    // Publish feedbacks until next point
    ros::Rate feedback_rate(100); // 10 msec
    ros::Time now;
    //
    float velocity = 0;
    if ( i < goal->trajectory.points.size() - 1) {
      float d0 = rad - prev_rad;
      float d1 = goal->trajectory.points[i+1].positions[0];
      float t0 = segment_time.toSec();
      float t1 = goal->trajectory.points[i+1].time_from_start.toSec();
      float v0 = d0/t0;
      float v1 = d1/t1;
      if ( v0 * v1 >= 0 ) {
        velocity = 0.5 * (v0 + v1);
      } else {
        velocity = 0;
      }
    }
    static float x = prev_rad;
    static float v = 0;
    static float a = 0;
    float gx = rad;
    float gv = velocity;
    float ga = 0;
    float target_t = segment_time.toSec();
    float A=(gx-(x+v*target_t+(a/2.0)*target_t*target_t))/(target_t*target_t*target_t);
    float B=(gv-(v+a*target_t))/(target_t*target_t);
    float C=(ga-a)/target_t;

    float a0=x;
    float a1=v;
    float a2=a/2.0;
    float a3=10*A-4*B+0.5*C;
    float a4=(-15*A+7*B-C)/target_t;
    float a5=(6*A-3*B+0.5*C)/(target_t*target_t);
    do
    {
      // see seqplay::playPattern in https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/seqplay.cpp
      now = ros::Time::now();
      // hoff arbib code is copied from https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/interpolator.cpp
      float t = (now - start_segment_time).toSec();
      // angle_msg.data = (rad - prev_rad) * (now - start_segment_time).toSec()/segment_time.toSec() + prev_rad; // linear
      angle_msg.data = a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t;

      g_gripper_servo_angle_pub[side].publish(angle_msg);
      feedback_rate.sleep();

      control_msgs::FollowJointTrajectoryFeedback feedback_;

      feedback_.joint_names.push_back(goal->trajectory.joint_names[0]);
      feedback_.desired.positions.resize(1);
      feedback_.actual.positions.resize(1);
      feedback_.error.positions.resize(1);

      feedback_.desired.positions[0] = rad;
      feedback_.actual.positions[0] = rad;
      feedback_.error.positions[0] = 0;

      feedback_.desired.time_from_start = now - start_time;
      feedback_.actual.time_from_start = now - start_time;
      feedback_.error.time_from_start = now - start_time;
      feedback_.header.stamp = ros::Time::now();
      as_->publishFeedback(feedback_);

    } while (ros::ok() &&
             now < (start_time + goal->trajectory.points[i].time_from_start));

    start_segment_time = now;
    prev_rad = rad;
    x = gx;
    v = gv;
    a = ga;
  }

  control_msgs::FollowJointTrajectoryResult result_;
  if (success)
  {
    ROS_INFO("%s: Succeeded for %s gripper", node_name.c_str(), side.c_str());
    result_.error_code = result_.SUCCESSFUL;
    as_->setSucceeded(result_);
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "gripper_joint_trajectory_action_server");
  ros::NodeHandle n;
  g_gripper_servo_angle_pub["right"] = n.advertise<std_msgs::Float32>("gripper_front/limb/right/servo/angle", 10);
  g_gripper_servo_angle_pub["left"] = n.advertise<std_msgs::Float32>("gripper_front/limb/left/servo/angle", 10);
  Server right_server(n, "gripper_front/limb/right/follow_joint_trajectory",
                      boost::bind(&execute, _1, &right_server, "right"), false);
  Server left_server(n, "gripper_front/limb/left/follow_joint_trajectory",
                     boost::bind(&execute, _1, &left_server, "left"), false);
  right_server.start();
  left_server.start();
  ros::spin();
  return 0;
}
<|endoftext|>


<commit_before>//===--- OwnershipModelEliminator.cpp - Eliminate SILOwnership Instr. -----===//
//
// This source file is part of the Swift.org open source project
//
// Copyright (c) 2014 - 2017 Apple Inc. and the Swift project authors
// Licensed under Apache License v2.0 with Runtime Library Exception
//
// See https://swift.org/LICENSE.txt for license information
// See https://swift.org/CONTRIBUTORS.txt for the list of Swift project authors
//
//===----------------------------------------------------------------------===//
///
///  \file
///
///  This file contains a small pass that lowers SIL ownership instructions to
///  their constituent operations. This will enable us to separate
///  implementation
///  of Semantic ARC in SIL and SILGen from ensuring that all of the optimizer
///  passes respect Semantic ARC. This is done by running this pass right after
///  SILGen and as the pass pipeline is updated, moving this pass further and
///  further back in the pipeline.
///
//===----------------------------------------------------------------------===//

#define DEBUG_TYPE "sil-ownership-model-eliminator"
#include "swift/SILOptimizer/PassManager/Transforms.h"
#include "swift/SIL/SILBuilder.h"
#include "swift/SIL/SILFunction.h"
#include "swift/SIL/SILVisitor.h"

using namespace swift;

//===----------------------------------------------------------------------===//
//                               Implementation
//===----------------------------------------------------------------------===//

namespace {

struct OwnershipModelEliminatorVisitor
    : SILInstructionVisitor<OwnershipModelEliminatorVisitor, bool> {
  SILBuilder &B;

  OwnershipModelEliminatorVisitor(SILBuilder &B) : B(B) {}
  void beforeVisit(ValueBase *V) {
    auto *I = cast<SILInstruction>(V);
    B.setInsertionPoint(I);
    B.setCurrentDebugScope(I->getDebugScope());
  }

  bool visitValueBase(ValueBase *V) { return false; }
  bool visitLoadInst(LoadInst *LI);
  bool visitStoreInst(StoreInst *SI);
  bool visitStoreBorrowInst(StoreBorrowInst *SI);
  bool visitCopyValueInst(CopyValueInst *CVI);
  bool visitCopyUnownedValueInst(CopyUnownedValueInst *CVI);
  bool visitDestroyValueInst(DestroyValueInst *DVI);
  bool visitLoadBorrowInst(LoadBorrowInst *LBI);
  bool visitBeginBorrowInst(BeginBorrowInst *BBI) {
    BBI->replaceAllUsesWith(BBI->getOperand());
    BBI->eraseFromParent();
    return true;
  }
  bool visitEndBorrowInst(EndBorrowInst *EBI) {
    EBI->eraseFromParent();
    return true;
  }
  bool visitUnmanagedRetainValueInst(UnmanagedRetainValueInst *URVI);
  bool visitUnmanagedReleaseValueInst(UnmanagedReleaseValueInst *URVI);
  bool visitUnmanagedAutoreleaseValueInst(UnmanagedAutoreleaseValueInst *UAVI);
};

} // end anonymous namespace

bool OwnershipModelEliminatorVisitor::visitLoadInst(LoadInst *LI) {
  auto Qualifier = LI->getOwnershipQualifier();

  // If the qualifier is unqualified, there is nothing further to do
  // here. Just return.
  if (Qualifier == LoadOwnershipQualifier::Unqualified)
    return false;

  SILValue Result = B.emitLoadValueOperation(LI->getLoc(), LI->getOperand(),
                                             LI->getOwnershipQualifier());

  // Then remove the qualified load and use the unqualified load as the def of
  // all of LI's uses.
  LI->replaceAllUsesWith(Result);
  LI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitStoreInst(StoreInst *SI) {
  auto Qualifier = SI->getOwnershipQualifier();

  // If the qualifier is unqualified, there is nothing further to do
  // here. Just return.
  if (Qualifier == StoreOwnershipQualifier::Unqualified)
    return false;

  B.emitStoreValueOperation(SI->getLoc(), SI->getSrc(), SI->getDest(),
                            SI->getOwnershipQualifier());

  // Then remove the qualified store.
  SI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitStoreBorrowInst(
    StoreBorrowInst *SI) {
  B.emitStoreValueOperation(SI->getLoc(), SI->getSrc(), SI->getDest(),
                            StoreOwnershipQualifier::Init);

  // Then remove the qualified store.
  SI->eraseFromParent();
  return true;
}

bool
OwnershipModelEliminatorVisitor::visitLoadBorrowInst(LoadBorrowInst *LBI) {
  // Break down the load borrow into an unqualified load.
  auto *UnqualifiedLoad = B.createLoad(LBI->getLoc(), LBI->getOperand(),
                                       LoadOwnershipQualifier::Unqualified);

  // Then remove the qualified load and use the unqualified load as the def of
  // all of LI's uses.
  LBI->replaceAllUsesWith(UnqualifiedLoad);
  LBI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitCopyValueInst(CopyValueInst *CVI) {
  // A copy_value of an address-only type cannot be replaced.
  if (CVI->getType().isAddressOnly(B.getModule()))
    return false;

  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.emitCopyValueOperation(CVI->getLoc(), CVI->getOperand());
  CVI->replaceAllUsesWith(CVI->getOperand());
  CVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitCopyUnownedValueInst(
    CopyUnownedValueInst *CVI) {
  B.createStrongRetainUnowned(CVI->getLoc(), CVI->getOperand(),
                              Atomicity::Atomic);
  // Users of copy_value_unowned expect an owned value. So we need to convert
  // our unowned value to a ref.
  auto *UTRI =
      B.createUnownedToRef(CVI->getLoc(), CVI->getOperand(), CVI->getType());
  CVI->replaceAllUsesWith(UTRI);
  CVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitUnmanagedRetainValueInst(
    UnmanagedRetainValueInst *URVI) {
  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.emitCopyValueOperation(URVI->getLoc(), URVI->getOperand());
  URVI->replaceAllUsesWith(URVI->getOperand());
  URVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitUnmanagedReleaseValueInst(
    UnmanagedReleaseValueInst *URVI) {
  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.emitDestroyValueOperation(URVI->getLoc(), URVI->getOperand());
  URVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitUnmanagedAutoreleaseValueInst(
    UnmanagedAutoreleaseValueInst *UAVI) {
  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.createAutoreleaseValue(UAVI->getLoc(), UAVI->getOperand(),
                           UAVI->getAtomicity());
  UAVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitDestroyValueInst(DestroyValueInst *DVI) {
  // A destroy_value of an address-only type cannot be replaced.
  if (DVI->getOperand()->getType().isAddressOnly(B.getModule()))
    return false;

  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.emitDestroyValueOperation(DVI->getLoc(), DVI->getOperand());
  DVI->eraseFromParent();
  return true;
}

//===----------------------------------------------------------------------===//
//                           Top Level Entry Point
//===----------------------------------------------------------------------===//

namespace {

struct OwnershipModelEliminator : SILFunctionTransform {
  void run() override {
    SILFunction *F = getFunction();

    // Set F to have unqualified ownership.
    F->setUnqualifiedOwnership();

    bool MadeChange = false;
    SILBuilder B(*F);
    OwnershipModelEliminatorVisitor Visitor(B);

    for (auto &BB : *F) {
      for (auto II = BB.begin(), IE = BB.end(); II != IE;) {
        // Since we are going to be potentially removing instructions, we need
        // to make sure to grab out instruction and increment first.
        SILInstruction *I = &*II;
        ++II;

        MadeChange |= Visitor.visit(I);
      }
    }

    if (MadeChange) {
      // If we made any changes, we just changed instructions, so invalidate
      // that analysis.
      invalidateAnalysis(SILAnalysis::InvalidationKind::Instructions);
    }
  }

  StringRef getName() override { return "Ownership Model Eliminator"; }
};

} // end anonymous namespace

SILTransform *swift::createOwnershipModelEliminator() {
  return new OwnershipModelEliminator();
}
<commit_msg>Replace newly introduced Atomicity::Atomic<commit_after>//===--- OwnershipModelEliminator.cpp - Eliminate SILOwnership Instr. -----===//
//
// This source file is part of the Swift.org open source project
//
// Copyright (c) 2014 - 2017 Apple Inc. and the Swift project authors
// Licensed under Apache License v2.0 with Runtime Library Exception
//
// See https://swift.org/LICENSE.txt for license information
// See https://swift.org/CONTRIBUTORS.txt for the list of Swift project authors
//
//===----------------------------------------------------------------------===//
///
///  \file
///
///  This file contains a small pass that lowers SIL ownership instructions to
///  their constituent operations. This will enable us to separate
///  implementation
///  of Semantic ARC in SIL and SILGen from ensuring that all of the optimizer
///  passes respect Semantic ARC. This is done by running this pass right after
///  SILGen and as the pass pipeline is updated, moving this pass further and
///  further back in the pipeline.
///
//===----------------------------------------------------------------------===//

#define DEBUG_TYPE "sil-ownership-model-eliminator"
#include "swift/SILOptimizer/PassManager/Transforms.h"
#include "swift/SIL/SILBuilder.h"
#include "swift/SIL/SILFunction.h"
#include "swift/SIL/SILVisitor.h"

using namespace swift;

//===----------------------------------------------------------------------===//
//                               Implementation
//===----------------------------------------------------------------------===//

namespace {

struct OwnershipModelEliminatorVisitor
    : SILInstructionVisitor<OwnershipModelEliminatorVisitor, bool> {
  SILBuilder &B;

  OwnershipModelEliminatorVisitor(SILBuilder &B) : B(B) {}
  void beforeVisit(ValueBase *V) {
    auto *I = cast<SILInstruction>(V);
    B.setInsertionPoint(I);
    B.setCurrentDebugScope(I->getDebugScope());
  }

  bool visitValueBase(ValueBase *V) { return false; }
  bool visitLoadInst(LoadInst *LI);
  bool visitStoreInst(StoreInst *SI);
  bool visitStoreBorrowInst(StoreBorrowInst *SI);
  bool visitCopyValueInst(CopyValueInst *CVI);
  bool visitCopyUnownedValueInst(CopyUnownedValueInst *CVI);
  bool visitDestroyValueInst(DestroyValueInst *DVI);
  bool visitLoadBorrowInst(LoadBorrowInst *LBI);
  bool visitBeginBorrowInst(BeginBorrowInst *BBI) {
    BBI->replaceAllUsesWith(BBI->getOperand());
    BBI->eraseFromParent();
    return true;
  }
  bool visitEndBorrowInst(EndBorrowInst *EBI) {
    EBI->eraseFromParent();
    return true;
  }
  bool visitUnmanagedRetainValueInst(UnmanagedRetainValueInst *URVI);
  bool visitUnmanagedReleaseValueInst(UnmanagedReleaseValueInst *URVI);
  bool visitUnmanagedAutoreleaseValueInst(UnmanagedAutoreleaseValueInst *UAVI);
};

} // end anonymous namespace

bool OwnershipModelEliminatorVisitor::visitLoadInst(LoadInst *LI) {
  auto Qualifier = LI->getOwnershipQualifier();

  // If the qualifier is unqualified, there is nothing further to do
  // here. Just return.
  if (Qualifier == LoadOwnershipQualifier::Unqualified)
    return false;

  SILValue Result = B.emitLoadValueOperation(LI->getLoc(), LI->getOperand(),
                                             LI->getOwnershipQualifier());

  // Then remove the qualified load and use the unqualified load as the def of
  // all of LI's uses.
  LI->replaceAllUsesWith(Result);
  LI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitStoreInst(StoreInst *SI) {
  auto Qualifier = SI->getOwnershipQualifier();

  // If the qualifier is unqualified, there is nothing further to do
  // here. Just return.
  if (Qualifier == StoreOwnershipQualifier::Unqualified)
    return false;

  B.emitStoreValueOperation(SI->getLoc(), SI->getSrc(), SI->getDest(),
                            SI->getOwnershipQualifier());

  // Then remove the qualified store.
  SI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitStoreBorrowInst(
    StoreBorrowInst *SI) {
  B.emitStoreValueOperation(SI->getLoc(), SI->getSrc(), SI->getDest(),
                            StoreOwnershipQualifier::Init);

  // Then remove the qualified store.
  SI->eraseFromParent();
  return true;
}

bool
OwnershipModelEliminatorVisitor::visitLoadBorrowInst(LoadBorrowInst *LBI) {
  // Break down the load borrow into an unqualified load.
  auto *UnqualifiedLoad = B.createLoad(LBI->getLoc(), LBI->getOperand(),
                                       LoadOwnershipQualifier::Unqualified);

  // Then remove the qualified load and use the unqualified load as the def of
  // all of LI's uses.
  LBI->replaceAllUsesWith(UnqualifiedLoad);
  LBI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitCopyValueInst(CopyValueInst *CVI) {
  // A copy_value of an address-only type cannot be replaced.
  if (CVI->getType().isAddressOnly(B.getModule()))
    return false;

  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.emitCopyValueOperation(CVI->getLoc(), CVI->getOperand());
  CVI->replaceAllUsesWith(CVI->getOperand());
  CVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitCopyUnownedValueInst(
    CopyUnownedValueInst *CVI) {
  B.createStrongRetainUnowned(CVI->getLoc(), CVI->getOperand(),
                              B.getDefaultAtomicity());
  // Users of copy_value_unowned expect an owned value. So we need to convert
  // our unowned value to a ref.
  auto *UTRI =
      B.createUnownedToRef(CVI->getLoc(), CVI->getOperand(), CVI->getType());
  CVI->replaceAllUsesWith(UTRI);
  CVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitUnmanagedRetainValueInst(
    UnmanagedRetainValueInst *URVI) {
  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.emitCopyValueOperation(URVI->getLoc(), URVI->getOperand());
  URVI->replaceAllUsesWith(URVI->getOperand());
  URVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitUnmanagedReleaseValueInst(
    UnmanagedReleaseValueInst *URVI) {
  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.emitDestroyValueOperation(URVI->getLoc(), URVI->getOperand());
  URVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitUnmanagedAutoreleaseValueInst(
    UnmanagedAutoreleaseValueInst *UAVI) {
  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.createAutoreleaseValue(UAVI->getLoc(), UAVI->getOperand(),
                           UAVI->getAtomicity());
  UAVI->eraseFromParent();
  return true;
}

bool OwnershipModelEliminatorVisitor::visitDestroyValueInst(DestroyValueInst *DVI) {
  // A destroy_value of an address-only type cannot be replaced.
  if (DVI->getOperand()->getType().isAddressOnly(B.getModule()))
    return false;

  // Now that we have set the unqualified ownership flag, destroy value
  // operation will delegate to the appropriate strong_release, etc.
  B.emitDestroyValueOperation(DVI->getLoc(), DVI->getOperand());
  DVI->eraseFromParent();
  return true;
}

//===----------------------------------------------------------------------===//
//                           Top Level Entry Point
//===----------------------------------------------------------------------===//

namespace {

struct OwnershipModelEliminator : SILFunctionTransform {
  void run() override {
    SILFunction *F = getFunction();

    // Set F to have unqualified ownership.
    F->setUnqualifiedOwnership();

    bool MadeChange = false;
    SILBuilder B(*F);
    OwnershipModelEliminatorVisitor Visitor(B);

    for (auto &BB : *F) {
      for (auto II = BB.begin(), IE = BB.end(); II != IE;) {
        // Since we are going to be potentially removing instructions, we need
        // to make sure to grab out instruction and increment first.
        SILInstruction *I = &*II;
        ++II;

        MadeChange |= Visitor.visit(I);
      }
    }

    if (MadeChange) {
      // If we made any changes, we just changed instructions, so invalidate
      // that analysis.
      invalidateAnalysis(SILAnalysis::InvalidationKind::Instructions);
    }
  }

  StringRef getName() override { return "Ownership Model Eliminator"; }
};

} // end anonymous namespace

SILTransform *swift::createOwnershipModelEliminator() {
  return new OwnershipModelEliminator();
}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: AIndexes.hxx,v $
 *
 *  $Revision: 1.7 $
 *
 *  last change: $Author: vg $ $Date: 2005-03-10 15:35:46 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/
#ifndef _CONNECTIVITY_ADO_INDEXES_HXX_
#define _CONNECTIVITY_ADO_INDEXES_HXX_

#ifndef _CONNECTIVITY_SDBCX_COLLECTION_HXX_
#include "connectivity/sdbcx/VCollection.hxx"
#endif
#ifndef _COM_SUN_STAR_SDBC_XDATABASEMETADATA_HPP_
#include <com/sun/star/sdbc/XDatabaseMetaData.hpp>
#endif
#ifndef _CONNECTIVITY_ADO_AWRAPADOX_HXX_
#include "ado/Awrapadox.hxx"
#endif

namespace connectivity
{
    namespace ado
    {
        class OConnection;
        class OIndexes : public sdbcx::OCollection
        {
            WpADOIndexes    m_aCollection;
            OConnection*    m_pConnection;
        protected:
            virtual sdbcx::ObjectType createObject(const ::rtl::OUString& _rName);
            virtual void impl_refresh() throw(::com::sun::star::uno::RuntimeException);
            virtual ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet > createEmptyObject();
            // return a object which is the copy of the descriptor
            virtual sdbcx::ObjectType cloneObject(const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >& _xDescriptor);
            virtual void appendObject( const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >& descriptor );
            virtual void dropObject(sal_Int32 _nPos,const ::rtl::OUString _sElementName);
        public:
            OIndexes(::cppu::OWeakObject& _rParent,
                     ::osl::Mutex& _rMutex,
                     const TStringVector &_rVector,
                     const WpADOIndexes&    _rCollection,
                     sal_Bool _bCase,
                     OConnection* _pConnection)
                    : sdbcx::OCollection(_rParent,_bCase,_rMutex,_rVector)
                    , m_aCollection(_rCollection)
                    , m_pConnection(_pConnection)
            {
            }
        };
    }
}

#endif // _CONNECTIVITY_ADO_INDEXES_HXX_

<commit_msg>INTEGRATION: CWS ooo19126 (1.7.74); FILE MERGED 2005/09/05 17:25:02 rt 1.7.74.1: #i54170# Change license header: remove SISSL<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: AIndexes.hxx,v $
 *
 *  $Revision: 1.8 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-08 06:51:42 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef _CONNECTIVITY_ADO_INDEXES_HXX_
#define _CONNECTIVITY_ADO_INDEXES_HXX_

#ifndef _CONNECTIVITY_SDBCX_COLLECTION_HXX_
#include "connectivity/sdbcx/VCollection.hxx"
#endif
#ifndef _COM_SUN_STAR_SDBC_XDATABASEMETADATA_HPP_
#include <com/sun/star/sdbc/XDatabaseMetaData.hpp>
#endif
#ifndef _CONNECTIVITY_ADO_AWRAPADOX_HXX_
#include "ado/Awrapadox.hxx"
#endif

namespace connectivity
{
    namespace ado
    {
        class OConnection;
        class OIndexes : public sdbcx::OCollection
        {
            WpADOIndexes    m_aCollection;
            OConnection*    m_pConnection;
        protected:
            virtual sdbcx::ObjectType createObject(const ::rtl::OUString& _rName);
            virtual void impl_refresh() throw(::com::sun::star::uno::RuntimeException);
            virtual ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet > createEmptyObject();
            // return a object which is the copy of the descriptor
            virtual sdbcx::ObjectType cloneObject(const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >& _xDescriptor);
            virtual void appendObject( const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >& descriptor );
            virtual void dropObject(sal_Int32 _nPos,const ::rtl::OUString _sElementName);
        public:
            OIndexes(::cppu::OWeakObject& _rParent,
                     ::osl::Mutex& _rMutex,
                     const TStringVector &_rVector,
                     const WpADOIndexes&    _rCollection,
                     sal_Bool _bCase,
                     OConnection* _pConnection)
                    : sdbcx::OCollection(_rParent,_bCase,_rMutex,_rVector)
                    , m_aCollection(_rCollection)
                    , m_pConnection(_pConnection)
            {
            }
        };
    }
}

#endif // _CONNECTIVITY_ADO_INDEXES_HXX_

<|endoftext|>


<commit_before>#include <gmock/gmock.h>

#include <type_traits>

#include "connection.h"

using namespace testing;

class Statement : public Test
{
protected:
    void SetUp()
    {
        conn.reset(new SqliteWrapper::Connection(":memory:"));
    }

    std::unique_ptr<SqliteWrapper::Connection> conn;
};

TEST_F(Statement, canMove)
{
    EXPECT_THAT((std::is_move_constructible<SqliteWrapper::Statement>::value),
                Eq(true));
    EXPECT_THAT((std::is_move_assignable<SqliteWrapper::Statement>::value),
                Eq(true));
}

TEST_F(Statement, cannotCopy)
{
    EXPECT_THAT((std::is_copy_constructible<SqliteWrapper::Statement>::value),
                Eq(false));
    EXPECT_THAT((std::is_copy_assignable<SqliteWrapper::Statement>::value),
                Eq(false));
}

TEST_F(Statement, canClearBindings)
{
    SqliteWrapper::Statement stmt = conn->prepare("PRAGMA user_version");
    stmt.clearBindings();
}

TEST_F(Statement, canBeReset)
{
    SqliteWrapper::Statement stmt = conn->prepare("PRAGMA user_version");
    stmt.reset();
}
<commit_msg>Refactor statement_test<commit_after>#include <gmock/gmock.h>
#include <type_traits>

#include "connection.h"

using namespace testing;

class Statement : public Test
{
protected:
    void SetUp()
    {
        conn.reset(new SqliteWrapper::Connection(":memory:"));
        conn->exec("CREATE TABLE answers (question TEXT, answer INT)");
    }

    SqliteWrapper::Statement makeSelect() const
    {
        return conn->prepare("SELECT question FROM answers WHERE answer = ?");
    }

    std::unique_ptr<SqliteWrapper::Connection> conn;
};

TEST_F(Statement, canMove)
{
    EXPECT_THAT((std::is_move_constructible<SqliteWrapper::Statement>::value),
                Eq(true));
    EXPECT_THAT((std::is_move_assignable<SqliteWrapper::Statement>::value),
                Eq(true));
}

TEST_F(Statement, cannotCopy)
{
    EXPECT_THAT((std::is_copy_constructible<SqliteWrapper::Statement>::value),
                Eq(false));
    EXPECT_THAT((std::is_copy_assignable<SqliteWrapper::Statement>::value),
                Eq(false));
}

TEST_F(Statement, canClearBindings)
{
    makeSelect().clearBindings();
}

TEST_F(Statement, canBeReset)
{
    makeSelect().reset();
}
<|endoftext|>