<commit_before>/* bzflag
 * Copyright (c) 1993 - 2004 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named LICENSE that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

/* interface header */
#include "GameKeeper.h"

/* system headers */
#include <vector>
#include <string>

// players list FIXME should be resized based on maxPlayers
extern PlayerState      lastState[PlayerSlot];

GameKeeper::Player *GameKeeper::Player::playerList[PlayerSlot] = {NULL};

#if defined(USE_THREADS)
pthread_mutex_t GameKeeper::Player::mutex = PTHREAD_MUTEX_INITIALIZER;

static void *tcpRx(void* arg) {
  GameKeeper::Player *playerData = (GameKeeper::Player *)arg;
  playerData->handleTcpPacketT();
  return NULL;
}
#endif

GameKeeper::Player::Player(int _playerIndex,
			   const struct sockaddr_in &clientAddr, int fd,
			   tcpCallback _clientCallback):
  player(_playerIndex), lagInfo(&player),
  lastState(&::lastState[_playerIndex]),
  playerIndex(_playerIndex), closed(false), clientCallback(_clientCallback)
{
  playerList[playerIndex] = this;

  lastState->order = 0;
  netHandler       = new NetHandler(&player, clientAddr, playerIndex, fd);
#if defined(USE_THREADS)
  int result = pthread_create(&thread, NULL, tcpRx, (void *)this);
  if (result)
    std::cerr << "Could not create thread" << std::endl;
  refCount	 = 1;
#endif
}

GameKeeper::Player::~Player()
{
  flagHistory.clear();
  delete netHandler;
#if defined(USE_THREADS)
  int result = pthread_join(thread, NULL);
  if (result)
    std::cerr << "Could not join thread" << std::endl;
#endif

  playerList[playerIndex] = NULL;
}

int GameKeeper::Player::count()
{
  Player *playerData;
  int     count = 0;

  for (int i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && !playerData->closed
	&& playerData->player.isPlaying())
      count++;
  return count;
}

void GameKeeper::Player::updateLatency(float &waitTime)
{
  Player* playerData;
  int p;

  for (p = 0; p < PlayerSlot; p++)
    if ((playerData = playerList[p]) && !playerData->closed) {

      // get time for next lagping
      playerData->lagInfo.updateLatency(waitTime);

      // get time for next delayed packet (Lag Flag)
      float nextTime = playerData->delayq.nextPacketTime();
      if (nextTime < waitTime) {
	waitTime = nextTime;
      }

    }
}

void GameKeeper::Player::dumpScore()
{
  Player *playerData;

  std::cout << "\n#players\n";
  int p;
  for (p = 0; p < PlayerSlot; p++)
    if ((playerData = playerList[p]) && !playerData->closed
	&& playerData->player.isPlaying()) {
      playerData->score.dump();
      std::cout << ' ' << playerData->player.getCallSign() << std::endl;
    }
}

int GameKeeper::Player::anointRabbit(int oldRabbit)
{
  float topRatio    = -100000.0f;
  int   rabbitIndex = NoPlayer;

  Player *playerData;
  int     i;
  bool    goodRabbitSelected = false;

  for (i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && !playerData->closed
	&& playerData->player.canBeRabbit(true)) {
	bool  goodRabbit = i != oldRabbit && playerData->player.isAlive();
	float ratio      = playerData->score.ranking();
	bool  select     = false;
	if (goodRabbitSelected) {
	  if (goodRabbit && (ratio > topRatio)) {
	    select = true;
	  }
	} else {
	  if (goodRabbit) {
	    select	     = true;
	    goodRabbitSelected = true;
	  } else {
	    if (ratio > topRatio)
	      select = true;
	  }
	}
	if (select) {
	  topRatio = ratio;
	  rabbitIndex = i;
	}
      }
  return rabbitIndex;
}

void *GameKeeper::Player::packAdminInfo(void *buf)
{
  buf = nboPackUByte(buf, netHandler->sizeOfIP());
  buf = nboPackUByte(buf, playerIndex);
  buf = netHandler->packAdminInfo(buf);
  return buf;
}

void *GameKeeper::Player::packPlayerInfo(void *buf)
{
  buf = nboPackUByte(buf, playerIndex);
  buf = nboPackUByte(buf, accessInfo.getPlayerProperties());
  return buf;
}

void *GameKeeper::Player::packPlayerUpdate(void *buf)
{
  buf = nboPackUByte(buf, playerIndex);
  buf = player.packUpdate(buf);
  buf = score.pack(buf);
  buf = player.packId(buf);
  return buf;
}

std::vector<int> GameKeeper::Player::allowed(PlayerAccessInfo::AccessPerm
					     right,
					     int targetPlayer)
{
  std::vector<int> receivers;
  Player* playerData;

  if (targetPlayer != -1) {
    if ((playerData = playerList[targetPlayer]) && !playerData->closed
	&& playerData->accessInfo.hasPerm(right))
      receivers.push_back(targetPlayer);
  } else {
    for (int i = 0; i < PlayerSlot; i++)
      if ((playerData = playerList[i]) && !playerData->closed
	  && playerData->accessInfo.hasPerm(right))
	receivers.push_back(i);
  }

  return receivers;
}

bool GameKeeper::Player::loadEnterData(void *buf,
				       uint16_t &rejectCode,
				       char *rejectMsg)
{
  bool result = player.unpackEnter(buf, rejectCode, rejectMsg);
  if (!result)
    return false;
  // look if there is as name clash, we won't allow this
  for (int i = 0; i < PlayerSlot; i++) {
    Player *otherData = playerList[i];
    if (i == playerIndex || !otherData || !otherData->player.isPlaying())
      continue;
    if (otherData->closed)
      continue;
    if (!strcasecmp(otherData->player.getCallSign(), player.getCallSign())) {
      rejectCode   = RejectRepeatCallsign;
      strcpy(rejectMsg, "The callsign specified is already in use.");
      return false;
    }
  }
  return true;
}

void GameKeeper::Player::signingOn(bool ctf)
{
  accessInfo.setName(player.getCallSign());
  player.resetPlayer(ctf);
  player.signingOn();
  lagInfo.reset();
}

int GameKeeper::Player::getPlayerIDByName(const std::string &name)
{
  Player* playerData;
  for (int i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && !playerData->closed
	&& (playerData->accessInfo.getName() == name))
      return i;
  return -1;
}

void GameKeeper::Player::reloadAccessDatabase()
{
  Player* playerData;
  for (int i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && !playerData->closed)
      playerData->accessInfo.reloadInfo();
}

void GameKeeper::Player::close()
{
  closed = true;
}

void GameKeeper::Player::clean()
{
  Player* playerData;
  for (int i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && playerData->closed
#if defined(USE_THREADS)
	&& !playerData->refCount
#endif
	)
      delete playerData;
}

int GameKeeper::Player::getFreeIndex(int min, int max)
{
  for (int i = min; i < max; i++)
    if (!playerList[i])
      return i;
  return max;
}

#if defined(USE_THREADS)
void GameKeeper::Player::handleTcpPacketT()
{
  while (!closed) {
    const RxStatus e = netHandler->tcpReceive();
    if (e == ReadPart)
      continue;
    passTCPMutex();
    clientCallback(*netHandler, playerIndex, e);
    freeTCPMutex();
  }
  refCount = 0;
}
#else
void GameKeeper::Player::handleTcpPacket(fd_set *set)
{
  if (netHandler->isFdSet(set)) {
    const RxStatus e = netHandler->tcpReceive();
    if (e == ReadPart)
      return;
    clientCallback(*netHandler, playerIndex, e);
  }
}
#endif

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8
<commit_msg>formatting<commit_after>/* bzflag
 * Copyright (c) 1993 - 2004 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named LICENSE that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

/* interface header */
#include "GameKeeper.h"

/* system headers */
#include <vector>
#include <string>

// players list FIXME should be resized based on maxPlayers
extern PlayerState      lastState[PlayerSlot];

GameKeeper::Player *GameKeeper::Player::playerList[PlayerSlot] = {NULL};

#if defined(USE_THREADS)
pthread_mutex_t GameKeeper::Player::mutex = PTHREAD_MUTEX_INITIALIZER;

static void *tcpRx(void* arg) {
  GameKeeper::Player *playerData = (GameKeeper::Player *)arg;
  playerData->handleTcpPacketT();
  return NULL;
}
#endif

GameKeeper::Player::Player(int _playerIndex,
			   const struct sockaddr_in &clientAddr, int fd,
			   tcpCallback _clientCallback):
  player(_playerIndex), lagInfo(&player),
  lastState(&::lastState[_playerIndex]),
  playerIndex(_playerIndex), closed(false), clientCallback(_clientCallback)
{
  playerList[playerIndex] = this;

  lastState->order = 0;
  netHandler       = new NetHandler(&player, clientAddr, playerIndex, fd);
#if defined(USE_THREADS)
  int result = pthread_create(&thread, NULL, tcpRx, (void *)this);
  if (result)
    std::cerr << "Could not create thread" << std::endl;
  refCount	 = 1;
#endif
}

GameKeeper::Player::~Player()
{
  flagHistory.clear();
  delete netHandler;
#if defined(USE_THREADS)
  int result = pthread_join(thread, NULL);
  if (result)
    std::cerr << "Could not join thread" << std::endl;
#endif

  playerList[playerIndex] = NULL;
}

int GameKeeper::Player::count()
{
  Player *playerData;
  int     count = 0;

  for (int i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && !playerData->closed
	&& playerData->player.isPlaying())
      count++;
  return count;
}

void GameKeeper::Player::updateLatency(float &waitTime)
{
  Player* playerData;
  int p;

  for (p = 0; p < PlayerSlot; p++)
    if ((playerData = playerList[p]) && !playerData->closed) {

      // get time for next lagping
      playerData->lagInfo.updateLatency(waitTime);

      // get time for next delayed packet (Lag Flag)
      float nextTime = playerData->delayq.nextPacketTime();
      if (nextTime < waitTime) {
	waitTime = nextTime;
      }

    }
}

void GameKeeper::Player::dumpScore()
{
  Player *playerData;

  std::cout << "\n#players\n";
  int p;
  for (p = 0; p < PlayerSlot; p++)
    if ((playerData = playerList[p]) && !playerData->closed
	&& playerData->player.isPlaying()) {
      playerData->score.dump();
      std::cout << ' ' << playerData->player.getCallSign() << std::endl;
    }
}

int GameKeeper::Player::anointRabbit(int oldRabbit)
{
  float topRatio    = -100000.0f;
  int   rabbitIndex = NoPlayer;

  Player *playerData;
  int     i;
  bool    goodRabbitSelected = false;

  for (i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && !playerData->closed
	&& playerData->player.canBeRabbit(true)) {
	bool  goodRabbit = i != oldRabbit && playerData->player.isAlive();
	float ratio      = playerData->score.ranking();
	bool  select     = false;
	if (goodRabbitSelected) {
	  if (goodRabbit && (ratio > topRatio)) {
	    select = true;
	  }
	} else {
	  if (goodRabbit) {
	    select	     = true;
	    goodRabbitSelected = true;
	  } else {
	    if (ratio > topRatio)
	      select = true;
	  }
	}
	if (select) {
	  topRatio = ratio;
	  rabbitIndex = i;
	}
      }
  return rabbitIndex;
}

void *GameKeeper::Player::packAdminInfo(void *buf)
{
  buf = nboPackUByte(buf, netHandler->sizeOfIP());
  buf = nboPackUByte(buf, playerIndex);
  buf = netHandler->packAdminInfo(buf);
  return buf;
}

void *GameKeeper::Player::packPlayerInfo(void *buf)
{
  buf = nboPackUByte(buf, playerIndex);
  buf = nboPackUByte(buf, accessInfo.getPlayerProperties());
  return buf;
}

void *GameKeeper::Player::packPlayerUpdate(void *buf)
{
  buf = nboPackUByte(buf, playerIndex);
  buf = player.packUpdate(buf);
  buf = score.pack(buf);
  buf = player.packId(buf);
  return buf;
}

std::vector<int> GameKeeper::Player::allowed(PlayerAccessInfo::AccessPerm right,
					     int targetPlayer)
{
  std::vector<int> receivers;
  Player* playerData;

  if (targetPlayer != -1) {
    if ((playerData = playerList[targetPlayer]) && !playerData->closed
	&& playerData->accessInfo.hasPerm(right))
      receivers.push_back(targetPlayer);
  } else {
    for (int i = 0; i < PlayerSlot; i++)
      if ((playerData = playerList[i]) && !playerData->closed
	  && playerData->accessInfo.hasPerm(right))
	receivers.push_back(i);
  }

  return receivers;
}

bool GameKeeper::Player::loadEnterData(void *buf,
				       uint16_t &rejectCode,
				       char *rejectMsg)
{
  bool result = player.unpackEnter(buf, rejectCode, rejectMsg);
  if (!result)
    return false;
  // look if there is as name clash, we won't allow this
  for (int i = 0; i < PlayerSlot; i++) {
    Player *otherData = playerList[i];
    if (i == playerIndex || !otherData || !otherData->player.isPlaying())
      continue;
    if (otherData->closed)
      continue;
    if (!strcasecmp(otherData->player.getCallSign(), player.getCallSign())) {
      rejectCode   = RejectRepeatCallsign;
      strcpy(rejectMsg, "The callsign specified is already in use.");
      return false;
    }
  }
  return true;
}

void GameKeeper::Player::signingOn(bool ctf)
{
  accessInfo.setName(player.getCallSign());
  player.resetPlayer(ctf);
  player.signingOn();
  lagInfo.reset();
}

int GameKeeper::Player::getPlayerIDByName(const std::string &name)
{
  Player* playerData;
  for (int i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && !playerData->closed
	&& (playerData->accessInfo.getName() == name))
      return i;
  return -1;
}

void GameKeeper::Player::reloadAccessDatabase()
{
  Player* playerData;
  for (int i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && !playerData->closed)
      playerData->accessInfo.reloadInfo();
}

void GameKeeper::Player::close()
{
  closed = true;
}

void GameKeeper::Player::clean()
{
  Player* playerData;
  for (int i = 0; i < PlayerSlot; i++)
    if ((playerData = playerList[i]) && playerData->closed
#if defined(USE_THREADS)
	&& !playerData->refCount
#endif
	)
      delete playerData;
}

int GameKeeper::Player::getFreeIndex(int min, int max)
{
  for (int i = min; i < max; i++)
    if (!playerList[i])
      return i;
  return max;
}

#if defined(USE_THREADS)
void GameKeeper::Player::handleTcpPacketT()
{
  while (!closed) {
    const RxStatus e = netHandler->tcpReceive();
    if (e == ReadPart)
      continue;
    passTCPMutex();
    clientCallback(*netHandler, playerIndex, e);
    freeTCPMutex();
  }
  refCount = 0;
}
#else
void GameKeeper::Player::handleTcpPacket(fd_set *set)
{
  if (netHandler->isFdSet(set)) {
    const RxStatus e = netHandler->tcpReceive();
    if (e == ReadPart)
      return;
    clientCallback(*netHandler, playerIndex, e);
  }
}
#endif

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8
<|endoftext|>


<commit_before>//---------------------------------------------------------- -*- Mode: C++ -*-
// $Id$
//
// Created 2013/06/08
// Author: Mike Ovsiannikov
//
// Copyright 2013 Quantcast Corp.
//
// This file is part of Kosmos File System (KFS).
//
// Licensed under the Apache License, Version 2.0
// (the "License"); you may not use this file except in compliance with
// the License. You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.
//
// Kerberos 5 client side authentication implementation.
//
//----------------------------------------------------------------------------

#include "KrbClient.h"

#include <krb5/krb5.h>

#include <string.h>
#include <errno.h>

#include <string>
#include <algorithm>

namespace KFS
{

using std::string;
using std::max;

class KrbClient::Impl
{
public:
    Impl()
        : mServiceHost(),
          mKeyTabFileName(),
          mClientName(),
          mCtx(),
          mAuthCtx(),
          mErrCode(0),
          mOutBuf(),
          mCreds(),
          mServerPtr(0),
          mClientPtr(0),
          mKeyBlockPtr(0),
          mCachePtr(0),
          mInitOptionsPtr(0),
          mKeyTabPtr(0),
          mInitedFlag(false),
          mUseKeyTabFlag(false),
          mCredInitedFlag(false),
          mServiceName(),
          mErrorMsg()
    {
        mOutBuf.data   = 0;
        mOutBuf.length = 0;
        memset(&mCreds, 0, sizeof(mCreds));
    }
    ~Impl()
        { Impl::CleanupSelf(); }
    const char* Init(
        const char* inServiceHostNamePtr,
        const char* inServeiceNamePtr,
        const char* inKeyTabNamePtr,
        const char* inClientNamePtr)
    {
        CleanupSelf();
        mServiceHost    = inServiceHostNamePtr ? inServiceHostNamePtr : "";
        mServiceName    = inServeiceNamePtr    ? inServeiceNamePtr    : "";
        mKeyTabFileName = inKeyTabNamePtr ? inKeyTabNamePtr : "";
        mClientName     = inClientNamePtr ? inClientNamePtr : "";
        mUseKeyTabFlag  = inKeyTabNamePtr != 0;
        mErrCode     = 0;
        mErrorMsg.clear();
        InitSelf();
        if (mErrCode) {
            return ErrStr();
        }
        return 0;
    }
    const char* Cleanup()
    {
        mErrorMsg.clear();
        mErrCode = CleanupSelf();
        if (mErrCode) {
            return ErrStr();
        }
        return 0;
    }
    const char* Request(
        const char*& outDataPtr,
        int&         outDataLen,
        const char*& outSessionKeyPtr,
        int&         outSessionKeyLen)
    {
        if (! mInitedFlag) {
            mErrCode  = EINVAL;
            mErrorMsg = "not initialized yet, invoke KrbClient::Init";
            return mErrorMsg.c_str();
        }
        CleanupAuth();
        if (! mCredInitedFlag) {
            mCreds.server = mServerPtr;
	    if ((mErrCode = krb5_cc_get_principal(
                    mCtx, mCachePtr, &mCreds.client)) != 0) {
                return ErrStr();
            }
        }
        krb5_creds* theCredsPtr = 0;
	if ((mErrCode = krb5_get_credentials(
                mCtx, 0, mCachePtr, &mCreds, &theCredsPtr)) != 0) {
            return ErrStr();
        }
#ifdef _KFS_KRB_CLIENT_SET_AUTH_FLAGS
        krb5_int32 theFlags = 0;
        if ((mErrCode = krb5_auth_con_init(mCtx, &mAuthCtx)) ||
               (mErrCode = krb5_auth_con_getflags(mCtx, mAuthCtx, &theFlags))) {
            krb5_free_creds(mCtx, theCredsPtr);
            return ErrStr();
        }
        theFlags |= KRB5_AUTH_CONTEXT_DO_SEQUENCE;
        theFlags &= ~(KRB5_AUTH_CONTEXT_DO_TIME | KRB5_AUTH_CONTEXT_RET_TIME);
        if ((mErrCode = krb5_auth_con_setflags(mCtx, mAuthCtx, theFlags))) {
            krb5_free_creds(mCtx, theCredsPtr);
            return ErrStr();
        }
#endif
        krb5_data theAppData = { 0 };
        theAppData.data   = 0;
        theAppData.length = 0;
        mErrCode = krb5_mk_req_extended(
            mCtx,
            &mAuthCtx,
            AP_OPTS_MUTUAL_REQUIRED,
            &theAppData,
            theCredsPtr,
            &mOutBuf
        );
        krb5_free_creds(mCtx, theCredsPtr);
        if (mErrCode != 0) {
            return ErrStr();
        }
        if ((mErrCode = krb5_auth_con_getkey(mCtx, mAuthCtx, &mKeyBlockPtr))) {
            return ErrStr();
        }
        outDataPtr       = (const char*)mOutBuf.data;
        outDataLen       = (int)mOutBuf.length;
        outSessionKeyPtr =
            reinterpret_cast<const char*>(mKeyBlockPtr->contents);
        outSessionKeyLen = (int)mKeyBlockPtr->length;
        return 0;
    }
    const char* Reply(
        const char*  inReplyPtr,
        int          inReplyLen)
    {
        if (! mInitedFlag) {
            mErrCode  = EINVAL;
            mErrorMsg = "not initialized yet, invoke KrbClient::Init";
            return mErrorMsg.c_str();
        }
        if (mOutBuf.length <= 0 || ! mOutBuf.data) {
            mErrCode  = EINVAL;
            mErrorMsg = "not ready to process reply, invoke KrbClient::Request";
            return mErrorMsg.c_str();
        }
        krb5_data theData = { 0 };
        theData.length = max(0, inReplyLen);
        theData.data   = const_cast<char*>(inReplyPtr);
        krb5_ap_rep_enc_part* theReplPtr = 0;
        if ((mErrCode = krb5_rd_rep(
                mCtx,
                mAuthCtx,
                &theData,
                &theReplPtr))) {
            return ErrStr();
        }
        krb5_free_ap_rep_enc_part(mCtx, theReplPtr);
        krb5_free_data_contents(mCtx, &mOutBuf);
        return 0;
    }
    int GetErrorCode() const
    {
        return mErrCode;
    }
private:
    string                   mServiceHost;
    string                   mKeyTabFileName;
    string                   mClientName;
    krb5_context             mCtx;
    krb5_auth_context        mAuthCtx;
    krb5_error_code          mErrCode;
    krb5_data                mOutBuf;
    krb5_creds               mCreds;
    krb5_principal           mServerPtr;
    krb5_principal           mClientPtr;
    krb5_keyblock*           mKeyBlockPtr;
    krb5_ccache              mCachePtr;
    krb5_get_init_creds_opt* mInitOptionsPtr;
    krb5_keytab              mKeyTabPtr;
    bool                     mInitedFlag;
    bool                     mUseKeyTabFlag;
    bool                     mCredInitedFlag;
    string                   mServiceName;
    string                   mErrorMsg;

    void InitCredCacheKeytab()
    {
        if (! mInitedFlag) {
            mErrCode  = EINVAL;
            return;
        }
        mInitOptionsPtr = 0;
#ifdef _KFS_KRB_CLIENT_USE_INIT_OPTIONS
        if ((mErrCode = krb5_get_init_creds_opt_alloc(
                mCtx, &mInitOptionsPtr))) {
            return;
        }
        if ((mErrCode = krb5_get_init_creds_opt_set_out_ccache(
                mCtx, mInitOptionsPtr, mCachePtr))) {
            return;
        }
#endif
        mKeyTabPtr = 0;
        if ((mErrCode = mKeyTabFileName.empty() ?
                krb5_kt_default(mCtx, &mKeyTabPtr) :
                krb5_kt_resolve(mCtx, mKeyTabFileName.c_str(),
                &mKeyTabPtr))) {
            return;
        }
        mClientPtr = 0;
        if ((mErrCode = krb5_parse_name(
                mCtx, mClientName.c_str(), &mClientPtr))) {
            return;
        }
        mErrCode = krb5_get_init_creds_keytab(
                mCtx,
                &mCreds,
                mClientPtr,
                mKeyTabPtr,
                0,
                0,
                mInitOptionsPtr
        );
        mCredInitedFlag = mErrCode == 0;
    }
    void InitSelf()
    {
        mErrCode = krb5_init_context(&mCtx);
        if (mErrCode) {
            return;
        }
        mInitedFlag = true;
        memset(&mCreds, 0, sizeof(mCreds));
        mCachePtr = 0;
	if ((mErrCode = krb5_sname_to_principal(
                mCtx,
                mServiceHost.c_str(),
                mServiceName.c_str(),
                KRB5_NT_UNKNOWN, // KRB5_NT_SRV_HST,
                &mServerPtr
            ))) {
            return;
        }
	if ((mErrCode = krb5_cc_default(mCtx, &mCachePtr)) != 0) {
            return;
        }
        if (mUseKeyTabFlag) {
            InitCredCacheKeytab();
        }
    }
    krb5_error_code CleanupSelf()
    {
        if (! mInitedFlag) {
            return 0;
        }
        krb5_error_code theErr = CleanupAuth();
        mInitedFlag = false;
        if (mCredInitedFlag) {
            if (mClientPtr && mClientPtr == mCreds.client) {
                mCreds.client = 0;
            }
            krb5_free_cred_contents(mCtx, &mCreds);
            mCredInitedFlag = false;
        }
        memset(&mCreds, 0, sizeof(mCreds));
	if (mServerPtr) {
            krb5_free_principal(mCtx, mServerPtr);
            mServerPtr = 0;
        }
	if (mClientPtr) {
            krb5_free_principal(mCtx, mClientPtr);
            mClientPtr = 0;
        }
        if (mCachePtr) {
            krb5_error_code const theCErr = krb5_cc_close(mCtx, mCachePtr);
            mCachePtr = 0;
            if (! theErr) {
                theErr = theCErr;
            }
        }
#ifdef _KFS_KRB_CLIENT_USE_INIT_OPTIONS
        if (mInitOptionsPtr) {
            krb5_get_init_creds_opt_free(mCtx, mInitOptionsPtr);
            mInitOptionsPtr = 0;
        }
#endif
        if (mKeyTabPtr) {
            krb5_error_code const theCloseErr = krb5_kt_close(mCtx, mKeyTabPtr);
            mKeyTabPtr = 0;
            if (! theErr && theCloseErr) {
                theErr = theCloseErr;
            }
        }
        krb5_free_context(mCtx);
        return theErr;
    }
    krb5_error_code CleanupAuth()
    {
        if (! mInitedFlag) {
            return 0;
        }
	if (! mClientPtr && mCreds.client) {
            krb5_free_principal(mCtx, mCreds.client);
            mCreds.client = 0;
        }
        if (mKeyBlockPtr) {
            krb5_free_keyblock(mCtx, mKeyBlockPtr);
            mKeyBlockPtr = 0;
        }
        krb5_free_data_contents(mCtx, &mOutBuf);
        if (! mAuthCtx) {
            return 0;
        }
        const krb5_error_code theErr = krb5_auth_con_free(mCtx, mAuthCtx);
        memset(&mCreds, 0, sizeof(mCreds));
        mAuthCtx = 0;
        return theErr;
    }
    const char* ErrStr()
    {
        mErrorMsg = ErrToStr(mErrCode);
        return mErrorMsg.c_str();
    }
    string ErrToStr(
        krb5_error_code inErrCode) const
    {
        if (! inErrCode) {
            return string();
        }
        if ( ! mCtx) {
            return string("no kerberos context");
        }
        const char* const theMsgPtr = krb5_get_error_message(mCtx, inErrCode);
        const string theMsg((theMsgPtr && *theMsgPtr) ?
            theMsgPtr : "unspecified kerberos error");
        if (theMsgPtr) {
            // cast away const to make it compatible with older krb5 releases.
            krb5_free_error_message(mCtx, const_cast<char*>(theMsgPtr));
        }
        return theMsg;
    }

private:
    Impl(
        const Impl& inImpl);
    Impl& operator=(
        const Impl& inImpl);
};

KrbClient::KrbClient()
    : mImpl(*(new Impl()))
{
}

KrbClient::~KrbClient()
{
    delete &mImpl;
}

    const char*
KrbClient::Init(
    const char* inServiceHostNamePtr,
    const char* inServeiceNamePtr,
    const char* inKeyTabNamePtr,
    const char* inClientNamePtr)
{
    return mImpl.Init(
        inServiceHostNamePtr,
        inServeiceNamePtr,
        inKeyTabNamePtr,
        inClientNamePtr
    );
}

    const char*
KrbClient::Cleanup()
{
    return mImpl.Cleanup();
}

    const char*
KrbClient::Request(
    const char*& outDataPtr,
    int&         outDataLen,
    const char*& outSessionKeyPtr,
    int&         outSessionKeyLen)
{
    return mImpl.Request(
        outDataPtr, outDataLen, outSessionKeyPtr, outSessionKeyLen);
}

    const char*
KrbClient::Reply(
    const char* inReplyPtr,
    int         inReplyLen)
{
    return mImpl.Reply(
        inReplyPtr, inReplyLen);
}

    int
KrbClient::GetErrorCode() const
{
    return mImpl.GetErrorCode();
}

}
<commit_msg>Qfs kerberos lib: fix client ticket cache initialization with keytab.<commit_after>//---------------------------------------------------------- -*- Mode: C++ -*-
// $Id$
//
// Created 2013/06/08
// Author: Mike Ovsiannikov
//
// Copyright 2013 Quantcast Corp.
//
// This file is part of Kosmos File System (KFS).
//
// Licensed under the Apache License, Version 2.0
// (the "License"); you may not use this file except in compliance with
// the License. You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.
//
// Kerberos 5 client side authentication implementation.
//
//----------------------------------------------------------------------------

#include "KrbClient.h"

#include <krb5/krb5.h>

#include <string.h>
#include <errno.h>

#include <string>
#include <algorithm>

namespace KFS
{

using std::string;
using std::max;

class KrbClient::Impl
{
public:
    Impl()
        : mServiceHost(),
          mKeyTabFileName(),
          mClientName(),
          mCtx(),
          mAuthCtx(),
          mErrCode(0),
          mOutBuf(),
          mCreds(),
          mServerPtr(0),
          mKeyBlockPtr(0),
          mCachePtr(0),
          mInitedFlag(false),
          mUseKeyTabFlag(false),
          mServiceName(),
          mErrorMsg()
    {
        mOutBuf.data   = 0;
        mOutBuf.length = 0;
        memset(&mCreds, 0, sizeof(mCreds));
    }
    ~Impl()
        { Impl::CleanupSelf(); }
    const char* Init(
        const char* inServiceHostNamePtr,
        const char* inServeiceNamePtr,
        const char* inKeyTabNamePtr,
        const char* inClientNamePtr)
    {
        CleanupSelf();
        mServiceHost    = inServiceHostNamePtr ? inServiceHostNamePtr : "";
        mServiceName    = inServeiceNamePtr    ? inServeiceNamePtr    : "";
        mKeyTabFileName = inKeyTabNamePtr ? inKeyTabNamePtr : "";
        mClientName     = inClientNamePtr ? inClientNamePtr : "";
        mUseKeyTabFlag  = inKeyTabNamePtr != 0;
        mErrCode     = 0;
        mErrorMsg.clear();
        InitSelf();
        if (mErrCode) {
            return ErrStr();
        }
        return 0;
    }
    const char* Cleanup()
    {
        mErrorMsg.clear();
        mErrCode = CleanupSelf();
        if (mErrCode) {
            return ErrStr();
        }
        return 0;
    }
    const char* Request(
        const char*& outDataPtr,
        int&         outDataLen,
        const char*& outSessionKeyPtr,
        int&         outSessionKeyLen)
    {
        if (! mInitedFlag) {
            mErrCode  = EINVAL;
            mErrorMsg = "not initialized yet, invoke KrbClient::Init";
            return mErrorMsg.c_str();
        }
        CleanupAuth();
        mCreds.server = mServerPtr;
	if ((mErrCode = krb5_cc_get_principal(
                mCtx, mCachePtr, &mCreds.client)) != 0) {
            return ErrStr();
        }
        krb5_creds* theCredsPtr = 0;
	if ((mErrCode = krb5_get_credentials(
                mCtx, 0, mCachePtr, &mCreds, &theCredsPtr)) != 0) {
            return ErrStr();
        }
#ifdef _KFS_KRB_CLIENT_SET_AUTH_FLAGS
        krb5_int32 theFlags = 0;
        if ((mErrCode = krb5_auth_con_init(mCtx, &mAuthCtx)) ||
               (mErrCode = krb5_auth_con_getflags(mCtx, mAuthCtx, &theFlags))) {
            krb5_free_creds(mCtx, theCredsPtr);
            return ErrStr();
        }
        theFlags |= KRB5_AUTH_CONTEXT_DO_SEQUENCE;
        theFlags &= ~(KRB5_AUTH_CONTEXT_DO_TIME | KRB5_AUTH_CONTEXT_RET_TIME);
        if ((mErrCode = krb5_auth_con_setflags(mCtx, mAuthCtx, theFlags))) {
            krb5_free_creds(mCtx, theCredsPtr);
            return ErrStr();
        }
#endif
        krb5_data theAppData = { 0 };
        theAppData.data   = 0;
        theAppData.length = 0;
        mErrCode = krb5_mk_req_extended(
            mCtx,
            &mAuthCtx,
            AP_OPTS_MUTUAL_REQUIRED,
            &theAppData,
            theCredsPtr,
            &mOutBuf
        );
        krb5_free_creds(mCtx, theCredsPtr);
        if (mErrCode != 0) {
            return ErrStr();
        }
        if ((mErrCode = krb5_auth_con_getkey(mCtx, mAuthCtx, &mKeyBlockPtr))) {
            return ErrStr();
        }
        outDataPtr       = (const char*)mOutBuf.data;
        outDataLen       = (int)mOutBuf.length;
        outSessionKeyPtr =
            reinterpret_cast<const char*>(mKeyBlockPtr->contents);
        outSessionKeyLen = (int)mKeyBlockPtr->length;
        return 0;
    }
    const char* Reply(
        const char*  inReplyPtr,
        int          inReplyLen)
    {
        if (! mInitedFlag) {
            mErrCode  = EINVAL;
            mErrorMsg = "not initialized yet, invoke KrbClient::Init";
            return mErrorMsg.c_str();
        }
        if (mOutBuf.length <= 0 || ! mOutBuf.data) {
            mErrCode  = EINVAL;
            mErrorMsg = "not ready to process reply, invoke KrbClient::Request";
            return mErrorMsg.c_str();
        }
        krb5_data theData = { 0 };
        theData.length = max(0, inReplyLen);
        theData.data   = const_cast<char*>(inReplyPtr);
        krb5_ap_rep_enc_part* theReplPtr = 0;
        if ((mErrCode = krb5_rd_rep(
                mCtx,
                mAuthCtx,
                &theData,
                &theReplPtr))) {
            return ErrStr();
        }
        krb5_free_ap_rep_enc_part(mCtx, theReplPtr);
        krb5_free_data_contents(mCtx, &mOutBuf);
        return 0;
    }
    int GetErrorCode() const
    {
        return mErrCode;
    }
private:
    string                   mServiceHost;
    string                   mKeyTabFileName;
    string                   mClientName;
    krb5_context             mCtx;
    krb5_auth_context        mAuthCtx;
    krb5_error_code          mErrCode;
    krb5_data                mOutBuf;
    krb5_creds               mCreds;
    krb5_principal           mServerPtr;
    krb5_keyblock*           mKeyBlockPtr;
    krb5_ccache              mCachePtr;
    bool                     mInitedFlag;
    bool                     mUseKeyTabFlag;
    string                   mServiceName;
    string                   mErrorMsg;

    void InitCredCacheKeyTab()
    {
        if (! mInitedFlag) {
            mErrCode  = EINVAL;
            return;
        }
        krb5_get_init_creds_opt* theInitOptionsPtr = 0;
#ifdef KRB5_GET_INIT_CREDS_OPT_CHG_PWD_PRMPT
        if ((mErrCode = krb5_get_init_creds_opt_alloc(
                mCtx, &theInitOptionsPtr))) {
            return;
        }
#else
        krb5_get_init_creds_opt theInitOptions;
        krb5_get_init_creds_opt_init(&theInitOptions);
        theInitOptionsPtr = &theInitOptions;
#endif
        if ((mErrCode = krb5_get_init_creds_opt_set_out_ccache(
                mCtx, theInitOptionsPtr, mCachePtr)) == 0) {
            krb5_keytab theKeyTabPtr = 0;
            if ((mErrCode = mKeyTabFileName.empty() ?
                    krb5_kt_default(mCtx, &theKeyTabPtr) :
                    krb5_kt_resolve(mCtx, mKeyTabFileName.c_str(),
                        &theKeyTabPtr))) {
                return;
            }
            krb5_principal theClientPtr = 0;
            if ((mErrCode = krb5_parse_name(
                    mCtx, mClientName.c_str(), &theClientPtr)) == 0) {
                if ((mErrCode = krb5_get_init_creds_keytab(
                        mCtx,
                        &mCreds,
                        theClientPtr,
                        theKeyTabPtr,
                        0,
                        0,
                        theInitOptionsPtr))) {
                    if (theClientPtr && theClientPtr == mCreds.client) {
                        mCreds.client = 0;
                    }
                    krb5_free_cred_contents(mCtx, &mCreds);
                }
                memset(&mCreds, 0, sizeof(mCreds));
	        if (theClientPtr) {
                    krb5_free_principal(mCtx, theClientPtr);
                    theClientPtr = 0;
                }
            }
            if (theKeyTabPtr) {
                krb5_error_code const theErr = krb5_kt_close(
                    mCtx, theKeyTabPtr);
                if (! mErrCode && theErr) {
                    mErrCode = theErr;
                }
            }
        }
#ifdef KRB5_GET_INIT_CREDS_OPT_CHG_PWD_PRMPT
        if (theInitOptionsPtr) {
            krb5_get_init_creds_opt_free(mCtx, theInitOptionsPtr);
        }
#endif
    }
    void InitSelf()
    {
        mErrCode = krb5_init_context(&mCtx);
        if (mErrCode) {
            return;
        }
        mInitedFlag = true;
        memset(&mCreds, 0, sizeof(mCreds));
        mCachePtr = 0;
	if ((mErrCode = krb5_sname_to_principal(
                mCtx,
                mServiceHost.c_str(),
                mServiceName.c_str(),
                KRB5_NT_UNKNOWN, // KRB5_NT_SRV_HST,
                &mServerPtr
            ))) {
            return;
        }
	if ((mErrCode = krb5_cc_default(mCtx, &mCachePtr)) != 0) {
            return;
        }
        if (mUseKeyTabFlag) {
            InitCredCacheKeyTab();
        }
    }
    krb5_error_code CleanupSelf()
    {
        if (! mInitedFlag) {
            return 0;
        }
        krb5_error_code theErr = CleanupAuth();
        mInitedFlag = false;
        memset(&mCreds, 0, sizeof(mCreds));
	if (mServerPtr) {
            krb5_free_principal(mCtx, mServerPtr);
            mServerPtr = 0;
        }
        if (mCachePtr) {
            krb5_error_code const theCErr = krb5_cc_close(mCtx, mCachePtr);
            mCachePtr = 0;
            if (! theErr) {
                theErr = theCErr;
            }
        }
        krb5_free_context(mCtx);
        return theErr;
    }
    krb5_error_code CleanupAuth()
    {
        if (! mInitedFlag) {
            return 0;
        }
	if (mCreds.client) {
            krb5_free_principal(mCtx, mCreds.client);
            mCreds.client = 0;
        }
        if (mKeyBlockPtr) {
            krb5_free_keyblock(mCtx, mKeyBlockPtr);
            mKeyBlockPtr = 0;
        }
        krb5_free_data_contents(mCtx, &mOutBuf);
        if (! mAuthCtx) {
            return 0;
        }
        const krb5_error_code theErr = krb5_auth_con_free(mCtx, mAuthCtx);
        memset(&mCreds, 0, sizeof(mCreds));
        mAuthCtx = 0;
        return theErr;
    }
    const char* ErrStr()
    {
        mErrorMsg = ErrToStr(mErrCode);
        return mErrorMsg.c_str();
    }
    string ErrToStr(
        krb5_error_code inErrCode) const
    {
        if (! inErrCode) {
            return string();
        }
        if ( ! mCtx) {
            return string("no kerberos context");
        }
        const char* const theMsgPtr = krb5_get_error_message(mCtx, inErrCode);
        const string theMsg((theMsgPtr && *theMsgPtr) ?
            theMsgPtr : "unspecified kerberos error");
        if (theMsgPtr) {
            // cast away const to make it compatible with older krb5 releases.
            krb5_free_error_message(mCtx, const_cast<char*>(theMsgPtr));
        }
        return theMsg;
    }

private:
    Impl(
        const Impl& inImpl);
    Impl& operator=(
        const Impl& inImpl);
};

KrbClient::KrbClient()
    : mImpl(*(new Impl()))
{
}

KrbClient::~KrbClient()
{
    delete &mImpl;
}

    const char*
KrbClient::Init(
    const char* inServiceHostNamePtr,
    const char* inServeiceNamePtr,
    const char* inKeyTabNamePtr,
    const char* inClientNamePtr)
{
    return mImpl.Init(
        inServiceHostNamePtr,
        inServeiceNamePtr,
        inKeyTabNamePtr,
        inClientNamePtr
    );
}

    const char*
KrbClient::Cleanup()
{
    return mImpl.Cleanup();
}

    const char*
KrbClient::Request(
    const char*& outDataPtr,
    int&         outDataLen,
    const char*& outSessionKeyPtr,
    int&         outSessionKeyLen)
{
    return mImpl.Request(
        outDataPtr, outDataLen, outSessionKeyPtr, outSessionKeyLen);
}

    const char*
KrbClient::Reply(
    const char* inReplyPtr,
    int         inReplyLen)
{
    return mImpl.Reply(
        inReplyPtr, inReplyLen);
}

    int
KrbClient::GetErrorCode() const
{
    return mImpl.GetErrorCode();
}

}
<|endoftext|>


<commit_before>//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/core/kinodynamic-distance.hh>
#include <limits>
#include <hpp/util/debug.hh>
#include <hpp/model/body.hh>
#include <hpp/model/device.hh>
#include <hpp/model/joint.hh>
#include <hpp/core/problem.hh>
#include <hpp/model/joint-configuration.hh>
#include <hpp/model/children-iterator.hh>
#include <hpp/model/configuration.hh>
#include <Eigen/SVD>

namespace hpp {
namespace core {

KinodynamicDistancePtr_t KinodynamicDistance::create (const DevicePtr_t& robot)
{
    KinodynamicDistance* ptr = new KinodynamicDistance (robot);
    KinodynamicDistancePtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
}

KinodynamicDistancePtr_t KinodynamicDistance::createFromProblem (const ProblemPtr_t& problem)
{
    KinodynamicDistance* ptr = new KinodynamicDistance (problem->robot());
    KinodynamicDistancePtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
}

KinodynamicDistancePtr_t KinodynamicDistance::createCopy
(const KinodynamicDistancePtr_t& distance)
{
    KinodynamicDistance* ptr = new KinodynamicDistance (*distance);
    KinodynamicDistancePtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
}

DistancePtr_t KinodynamicDistance::clone () const
{
    return createCopy (weak_.lock ());
}

KinodynamicDistance::KinodynamicDistance (const DevicePtr_t& robot) :
    robot_ (robot)
{
    if((2*(robot_->extraConfigSpace().dimension())) < robot_->configSize()){
        std::cout<<"Error : you need at least "<<robot_->configSize()-robot_->extraConfigSpace().dimension()<<" extra DOF"<<std::endl;
        hppDout(error,"Error : you need at least "<<robot_->configSize() - robot_->extraConfigSpace().dimension()<<" extra DOF");
    }
    aMax_ = 0.5;
    vMax_ = 2;
}

KinodynamicDistance::KinodynamicDistance (const KinodynamicDistance& distance) :
    robot_ (distance.robot_)
{
}

void KinodynamicDistance::init (KinodynamicDistanceWkPtr_t self)
{
    weak_ = self;
}

inline double sgnenum(double val) {
  return ((0. < val ) - (val < 0.));
}

inline int sgn(double d)  {
  return d >= 0.0 ? 1 : -1;
}

inline double sgnf(double d)  {
  return d >= 0.0 ? 1.0 : -1.0;
}

double KinodynamicDistance::computeMinTime(double p1, double p2, double v1, double v2) const{
  //hppDout(info,"p1 = "<<p1<<"  p2 = "<<p2<<"   ; v1 = "<<v1<<"    v2 = "<<v2);
  // compute the sign of each acceleration
  double t1,t2,tv;
  int sigma;
  double deltaPacc = 0.5*(v1+v2)*(fabs(v2-v1)/aMax_);
  sigma = sgn(p2-p1-deltaPacc);  //TODO bug sigma == 0, temp fix ?
  //hppDout(info,"sigma = "<<sigma);
  if(sigma == 0){ // ??? FIXME
    sigma = sgn(p2-p1);
    //hppDout(info,"sigma Bis= "<<sigma);
  }
  double a1 = (sigma)*aMax_;
  double a2 = -a1;
  double vLim = (sigma) * vMax_;
  //hppDout(info,"Vlim = "<<vLim<<"   ;  aMax = "<<aMax_);
  if((p2-p1) == 0. && (v2-v1)==0. ){
    //hppDout(notice,"No movement in this joints, abort.");
    return 0.;
  }
  // test if two segment trajectory is valid :
  bool twoSegment = false;

  // solve quadratic equation (cf eq 13 article)
  const double a = a1;
  const double b = 2. * v1;
  const double c = (0.5*(v1+v2)*(v2-v1)/a2) - (p2-p1);

  const double q = -0.5*(b+sgnf(b)*sqrt(b*b-4*a*c));
  //hppDout(info, "sign of "<<b<<" is : "<<sgnf(b));
  const double x1 = q/a;
  const double x2 = c/q;
  const double x = std::max(x1,x2);
  //hppDout(info,"Solve quadratic equation : x1 = "<<x1<<"  ; x2 = "<<x2);
  //hppDout(info," x = "<<x);
  //hppDout(info,"t1 before vel limit = "<<x);

  //hppDout(info,"inf bound on t1 (from t2 > 0) "<<-((v2-v1)/a2));
  double minT1 = std::max(0.,-((v2-v1)/a2));  //lower bound for valid t1 value (cf eq 14)

  if(x >= minT1){
    twoSegment = true;
    t1 = x;
    //hppDout(info,"t1 >= minT1");
  }
  if(twoSegment){ // check if max velocity is respected
    if(std::abs(v1+(t1)*a1) > vMax_){
      twoSegment = false;
      //hppDout(info,"Doesn't respect max velocity, need 3 segments");
    }
  }
  if(twoSegment){ // compute t2 for two segment trajectory
    tv = 0.;
    t2 = ((v2-v1)/a2) + (t1);// eq 14
  }else{// compute 3 segment trajectory, with constant velocity phase :
    t1 = (vLim - v1)/a1;  //eq 15
    tv = ((v1*v1+v2*v2 - 2*vLim*vLim)/(2*vLim*a1)) + (p2-p1)/vLim ; //eq 16
    t2 = (v2-vLim)/a2;  //eq 17
  }
  if(twoSegment){
    //hppDout(notice,"Trajectory with 2 segments");
  }else{
    //hppDout(notice,"Trajectory with 3 segments");
  }
  //hppDout(notice,"a1 = "<<a1<<"  ;  a2 ="<<a2);
  //hppDout(notice,"t = "<<(t1)<<"   ;   "<<(tv)<<"   ;   "<<(t2));
  double T = (t1)+(tv)+(t2);
  //hppDout(notice,"T = "<<T);
  return T;
}

value_type KinodynamicDistance::impl_distance (ConfigurationIn_t q1,
                                               ConfigurationIn_t q2) const
{
    double Tmax = 0;
    double T = 0;

    size_type configSize = robot_->configSize() - robot_->extraConfigSpace().dimension ();
    // looking for Tmax
    //hppDout(notice,"KinodynamicDistance :  Looking for Tmax :");
    hppDout(info,"Distance between : "<<model::displayConfig(q1));
    hppDout(info,"and              : "<<model::displayConfig(q2));

    for(int indexConfig = 0 ; indexConfig < 3 ; indexConfig++){// FIX ME : only work with freeflyer
        size_type indexVel = indexConfig + configSize;
        //hppDout(notice,"For joint :"<<robot_->getJointAtConfigRank(indexConfig)->name());
        if(robot_->getJointAtConfigRank(indexConfig)->name() != "base_joint_SO3"){
            T = computeMinTime(q1[indexConfig],q2[indexConfig],q1[indexVel],q2[indexVel]);
            if(T > Tmax)
                Tmax = T;
        }else{
            //hppDout(notice,"!! Kinodynamic distance for quaternion not implemented yet.");
        }

    }
    hppDout(info," is : "<<Tmax);
    return Tmax;
}
} //   namespace core
} // namespace hpp
<commit_msg>kinodynamic-distance retrieve velocity and accelerations bounds from probleme parameters<commit_after>//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/core/kinodynamic-distance.hh>
#include <limits>
#include <hpp/util/debug.hh>
#include <hpp/model/body.hh>
#include <hpp/model/device.hh>
#include <hpp/model/joint.hh>
#include <hpp/core/problem.hh>
#include <hpp/model/joint-configuration.hh>
#include <hpp/model/children-iterator.hh>
#include <hpp/model/configuration.hh>
#include <Eigen/SVD>

namespace hpp {
namespace core {

KinodynamicDistancePtr_t KinodynamicDistance::create (const DevicePtr_t& robot)
{
    KinodynamicDistance* ptr = new KinodynamicDistance (robot);
    KinodynamicDistancePtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
}

KinodynamicDistancePtr_t KinodynamicDistance::createFromProblem (const ProblemPtr_t& problem)
{
    KinodynamicDistance* ptr = new KinodynamicDistance (problem->robot());
    KinodynamicDistancePtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
}

KinodynamicDistancePtr_t KinodynamicDistance::createCopy
(const KinodynamicDistancePtr_t& distance)
{
    KinodynamicDistance* ptr = new KinodynamicDistance (*distance);
    KinodynamicDistancePtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
}

DistancePtr_t KinodynamicDistance::clone () const
{
    return createCopy (weak_.lock ());
}

KinodynamicDistance::KinodynamicDistance (const DevicePtr_t& robot) :
    robot_ (robot)
{
    if((2*(robot_->extraConfigSpace().dimension())) < robot_->configSize()){
        std::cout<<"Error : you need at least "<<robot_->configSize()-robot_->extraConfigSpace().dimension()<<" extra DOF"<<std::endl;
        hppDout(error,"Error : you need at least "<<robot_->configSize() - robot_->extraConfigSpace().dimension()<<" extra DOF");
    }
    // get velocity and acceleration bounds from problem :
    try {
      aMax_ = problem_->get<double> (std::string("aMax"));
    } catch (const std::exception& e) {
      std::cout<<"Warning : no acceleration bounds set, use 1.0 as default"<<std::endl;
      aMax_ = 1.;
    }
    try {
      vMax_ = problem_->get<double> (std::string("vMax"));
    } catch (const std::exception& e) {
      std::cout<<"Warning : no velocity bounds set, use 1.0 as default"<<std::endl;
      vMax_ = 1.;
    }
    hppDout(info,"#### create steering kinodynamic, vMax = "<<vMax_<<" ; aMax = "<<aMax_);
}

KinodynamicDistance::KinodynamicDistance (const KinodynamicDistance& distance) :
    robot_ (distance.robot_)
{
}

void KinodynamicDistance::init (KinodynamicDistanceWkPtr_t self)
{
    weak_ = self;
}

inline double sgnenum(double val) {
  return ((0. < val ) - (val < 0.));
}

inline int sgn(double d)  {
  return d >= 0.0 ? 1 : -1;
}

inline double sgnf(double d)  {
  return d >= 0.0 ? 1.0 : -1.0;
}

double KinodynamicDistance::computeMinTime(double p1, double p2, double v1, double v2) const{
  //hppDout(info,"p1 = "<<p1<<"  p2 = "<<p2<<"   ; v1 = "<<v1<<"    v2 = "<<v2);
  // compute the sign of each acceleration
  double t1,t2,tv;
  int sigma;
  double deltaPacc = 0.5*(v1+v2)*(fabs(v2-v1)/aMax_);
  sigma = sgn(p2-p1-deltaPacc);  //TODO bug sigma == 0, temp fix ?
  //hppDout(info,"sigma = "<<sigma);
  if(sigma == 0){ // ??? FIXME
    sigma = sgn(p2-p1);
    //hppDout(info,"sigma Bis= "<<sigma);
  }
  double a1 = (sigma)*aMax_;
  double a2 = -a1;
  double vLim = (sigma) * vMax_;
  //hppDout(info,"Vlim = "<<vLim<<"   ;  aMax = "<<aMax_);
  if((p2-p1) == 0. && (v2-v1)==0. ){
    //hppDout(notice,"No movement in this joints, abort.");
    return 0.;
  }
  // test if two segment trajectory is valid :
  bool twoSegment = false;

  // solve quadratic equation (cf eq 13 article)
  const double a = a1;
  const double b = 2. * v1;
  const double c = (0.5*(v1+v2)*(v2-v1)/a2) - (p2-p1);

  const double q = -0.5*(b+sgnf(b)*sqrt(b*b-4*a*c));
  //hppDout(info, "sign of "<<b<<" is : "<<sgnf(b));
  const double x1 = q/a;
  const double x2 = c/q;
  const double x = std::max(x1,x2);
  //hppDout(info,"Solve quadratic equation : x1 = "<<x1<<"  ; x2 = "<<x2);
  //hppDout(info," x = "<<x);
  //hppDout(info,"t1 before vel limit = "<<x);

  //hppDout(info,"inf bound on t1 (from t2 > 0) "<<-((v2-v1)/a2));
  double minT1 = std::max(0.,-((v2-v1)/a2));  //lower bound for valid t1 value (cf eq 14)

  if(x >= minT1){
    twoSegment = true;
    t1 = x;
    //hppDout(info,"t1 >= minT1");
  }
  if(twoSegment){ // check if max velocity is respected
    if(std::abs(v1+(t1)*a1) > vMax_){
      twoSegment = false;
      //hppDout(info,"Doesn't respect max velocity, need 3 segments");
    }
  }
  if(twoSegment){ // compute t2 for two segment trajectory
    tv = 0.;
    t2 = ((v2-v1)/a2) + (t1);// eq 14
  }else{// compute 3 segment trajectory, with constant velocity phase :
    t1 = (vLim - v1)/a1;  //eq 15
    tv = ((v1*v1+v2*v2 - 2*vLim*vLim)/(2*vLim*a1)) + (p2-p1)/vLim ; //eq 16
    t2 = (v2-vLim)/a2;  //eq 17
  }
  if(twoSegment){
    //hppDout(notice,"Trajectory with 2 segments");
  }else{
    //hppDout(notice,"Trajectory with 3 segments");
  }
  //hppDout(notice,"a1 = "<<a1<<"  ;  a2 ="<<a2);
  //hppDout(notice,"t = "<<(t1)<<"   ;   "<<(tv)<<"   ;   "<<(t2));
  double T = (t1)+(tv)+(t2);
  //hppDout(notice,"T = "<<T);
  return T;
}

value_type KinodynamicDistance::impl_distance (ConfigurationIn_t q1,
                                               ConfigurationIn_t q2) const
{
    double Tmax = 0;
    double T = 0;

    size_type configSize = robot_->configSize() - robot_->extraConfigSpace().dimension ();
    // looking for Tmax
    //hppDout(notice,"KinodynamicDistance :  Looking for Tmax :");
    hppDout(info,"Distance between : "<<model::displayConfig(q1));
    hppDout(info,"and              : "<<model::displayConfig(q2));

    for(int indexConfig = 0 ; indexConfig < 3 ; indexConfig++){// FIX ME : only work with freeflyer
        size_type indexVel = indexConfig + configSize;
        //hppDout(notice,"For joint :"<<robot_->getJointAtConfigRank(indexConfig)->name());
        if(robot_->getJointAtConfigRank(indexConfig)->name() != "base_joint_SO3"){
            T = computeMinTime(q1[indexConfig],q2[indexConfig],q1[indexVel],q2[indexVel]);
            if(T > Tmax)
                Tmax = T;
        }else{
            //hppDout(notice,"!! Kinodynamic distance for quaternion not implemented yet.");
        }

    }
    hppDout(info," is : "<<Tmax);
    return Tmax;
}
} //   namespace core
} // namespace hpp
<|endoftext|>


<commit_before>/*
 * (C) 2015-2016 Augustin Cavalier
 * All rights reserved. Distributed under the terms of the MIT license.
 */

#include <stdlib.h>

#include "Tester.h"

#include "util/StringUtil.h"
#include "util/FSUtil.h"

int main(int argc, char* argv[])
{
	Tester t(true);

	t.beginGroup("StringUtil");
	t.result(StringUtil::count("0 1 2 3 4", " ") == 4, "count-1");
	t.result(StringUtil::count("Not many 'a's in this sentence.", "a") == 2, "count-2");
	t.result(StringUtil::count("creatively correcting complex concatenations", " co") == 3, "count-3");

	std::vector<std::string> split1 = StringUtil::split("-C:C++:/usr/bin/who/cares/really", ":");
	t.result(split1.size() == 3, "split-1.size");
	t.result(split1[0] == "-C", "split-1[0]");
	t.result(split1[2] == "/usr/bin/who/cares/really", "split-1[2]");
	std::vector<std::string> split2 = StringUtil::split("longer_-?things?-_to()split&&by\n_-?but", "_-?");
	t.result(split2.size() == 3, "split-2.size");
	t.result(split2[0] == "longer", "split-2[0]");
	t.result(split2[2] == "but", "split-2[2]");

	t.result(StringUtil::join({"this is", "", "sparta"}, " not ", false) ==
		"this is not  not sparta", "join-1#skipEmpty=false");
	t.result(StringUtil::join({"this is", "", "sparta"}, " not ", true) ==
		"this is not sparta", "join-1#skipEmpty=true");
	t.result(StringUtil::join({"Hello", "world", "it", "is", "I"}, " ") ==
		"Hello world it is I", "join-2");

	t.result(StringUtil::startsWith("Hay stack", "Hay"), "startsWith-1");
	t.result(StringUtil::startsWith("Haystack", "Hay"), "startsWith-2");
	t.result(!StringUtil::startsWith("Heystack", "Hay"), "startsWith-3");
	t.result(!StringUtil::startsWith("\nHaystack", "Hay"), "startsWith-4");

	t.result(StringUtil::endsWith("\nHaystack", "ack"), "endsWith-1");
	t.result(StringUtil::endsWith("\nHayst ack", "ack"), "endsWith-2");
	t.result(!StringUtil::endsWith("\nHaysteck", "ack"), "endsWith-3");
	t.result(!StringUtil::endsWith("\nHayst eck", "ack"), "endsWith-4");

	std::string replace = "stRing sub_ject StRING tO re_place-_IN_SIDE";
	StringUtil::replaceAll(replace, "_", "");
	t.result(replace ==	"stRing subject StRING tO replace-INSIDE", "replaceAll-1");

	replace = "stRing subject StRING tO replace-INSIDE";
	StringUtil::replaceAll(replace, "RING", "ring");
	t.result(replace ==	"stRing subject String tO replace-INSIDE", "replaceAll-2");
	t.endGroup();

	t.beginGroup("FSUtil");
	t.result(FSUtil::exists(argv[0]), "exists-1");
	t.result(!FSUtil::exists("this_file_does_not_exist.txt"), "exists-2");

	t.result(FSUtil::isFile(argv[0]), "isFile-1");
	t.result(!FSUtil::isFile("this_file_does_not_exist.txt"), "isFile-2");
	t.result(!FSUtil::isFile(".."), "isFile-3");

	t.result(FSUtil::isDir(".."), "isDir-1");
	t.result(FSUtil::isDir("/"), "isDir-2");
	t.result(!FSUtil::isDir(argv[0]), "isDir-3");
	t.result(!FSUtil::isDir("this_file_does_not_exist.txt"), "isDir-4");

	FSUtil::putContents("this_file_exists.txt", "These are the contents of this file.");
	t.result(FSUtil::isFile("this_file_exists.txt"), "putContents-1/isFile-4");
	t.result(FSUtil::exists("this_file_exists.txt"), "putContents-2");

	t.result(FSUtil::getContents("this_file_exists.txt") == "These are the contents of this file.",
		"getContents-1");
	t.result(FSUtil::getContents("this_file_does_not_exist.txt") == "", "getContents-2");
	FSUtil::putContents("this_file_exists.txt", "These are the modified contents of this file.");
	t.result(FSUtil::getContents("this_file_exists.txt") == "These are the modified contents of this file.",
		"getContents-3/putContents-3");

	FSUtil::deleteFile("this_file_exists.txt");
	t.result(!FSUtil::exists("this_file_exists.txt"), "deleteFile-1/exists-3");

	// TODO: searchForFiles, which

	t.result(FSUtil::normalizePath("/whatever/this//is//../././../whatever/dir2/dir9/apathto.txt") ==
		"/whatever/whatever/dir2/dir9/apathto.txt", "normalizePath-1");
#ifdef _WIN32
	t.result(FSUtil::normalizePath("C:\\directory\\otherdirectory/someplace\\..\\../.\\wow") ==
		"C:/directory/wow", "normalizePath-win_1");
	t.result(FSUtil::normalizePath("/c/this/not/really/../../is/a/MSYS/or/Cygwin/path.txt") ==
		"C:/this/is/a/MSYS/or/Cygwin/path.txt", "normalizePath-win_2");
	t.result(FSUtil::normalizePath("/Nc/this/is/not/././is/../a/MSYS/or/Cygwin/path.txt") ==
		"/Nc/this/is/not/a/MSYS/or/Cygwin/path.txt", "normalizePath-win_3");
#else
	t.result(FSUtil::normalizePath("/c/this/not/really/../../is/a/MSYS/or/Cygwin/path.txt") ==
		"/c/this/is/a/MSYS/or/Cygwin/path.txt", "normalizePath-!win_1");
#endif

	t.result(FSUtil::combinePaths({"/whatever/", "this//is//", ".././",
			"/./../", "whatever/dir2/", "/dir9/apathto.txt"}) ==
		"/whatever/whatever/dir2/dir9/apathto.txt", "combinePaths-1");
	t.result(FSUtil::combinePaths({"/this/not", "really", "", "../..", "is/a//path.txt"}) ==
		"/this/is/a/path.txt", "combinePaths-2");

	t.result(FSUtil::parentDirectory("/test/1.3.4.2/blah/") == "/test/1.3.4.2",
		"parentDirectory-1");
	t.result(FSUtil::parentDirectory("/test/1.3.4.2/blah/file.txt") == "/test/1.3.4.2/blah",
		"parentDirectory-2");
	t.result(FSUtil::parentDirectory("file.txt") == ".",
		"parentDirectory-3");

	FSUtil::mkdir("this_directory_now_exists");
	t.result(FSUtil::isDir("this_directory_now_exists"), "mkdir-1/isDir-5");
	FSUtil::rmdir("this_directory_now_exists");
	t.result(!FSUtil::isDir("this_directory_now_exists"), "rmdir-1/isDir-6");
	t.endGroup();

	return t.done();
}
<commit_msg>tests/Util: Add another test.<commit_after>/*
 * (C) 2015-2016 Augustin Cavalier
 * All rights reserved. Distributed under the terms of the MIT license.
 */

#include <stdlib.h>

#include "Tester.h"

#include "util/StringUtil.h"
#include "util/FSUtil.h"

int main(int argc, char* argv[])
{
	Tester t(true);

	t.beginGroup("StringUtil");
	t.result(StringUtil::count("0 1 2 3 4", " ") == 4, "count-1");
	t.result(StringUtil::count("Not many 'a's in this sentence.", "a") == 2, "count-2");
	t.result(StringUtil::count("creatively correcting complex concatenations", " co") == 3, "count-3");

	std::vector<std::string> split1 = StringUtil::split("-C:C++:/usr/bin/who/cares/really", ":");
	t.result(split1.size() == 3, "split-1.size");
	t.result(split1[0] == "-C", "split-1[0]");
	t.result(split1[2] == "/usr/bin/who/cares/really", "split-1[2]");
	std::vector<std::string> split2 = StringUtil::split("longer_-?things?-_to()split&&by\n_-?but", "_-?");
	t.result(split2.size() == 3, "split-2.size");
	t.result(split2[0] == "longer", "split-2[0]");
	t.result(split2[2] == "but", "split-2[2]");

	t.result(StringUtil::join({"this is", "", "sparta"}, " not ", false) ==
		"this is not  not sparta", "join-1#skipEmpty=false");
	t.result(StringUtil::join({"this is", "", "sparta"}, " not ", true) ==
		"this is not sparta", "join-1#skipEmpty=true");
	t.result(StringUtil::join({"Hello", "world", "it", "is", "I"}, " ") ==
		"Hello world it is I", "join-2");

	t.result(StringUtil::startsWith("Hay stack", "Hay"), "startsWith-1");
	t.result(StringUtil::startsWith("Haystack", "Hay"), "startsWith-2");
	t.result(!StringUtil::startsWith("Heystack", "Hay"), "startsWith-3");
	t.result(!StringUtil::startsWith("\nHaystack", "Hay"), "startsWith-4");

	t.result(StringUtil::endsWith("\nHaystack", "ack"), "endsWith-1");
	t.result(StringUtil::endsWith("\nHayst ack", "ack"), "endsWith-2");
	t.result(!StringUtil::endsWith("\nHaysteck", "ack"), "endsWith-3");
	t.result(!StringUtil::endsWith("\nHayst eck", "ack"), "endsWith-4");

	std::string replace = "stRing sub_ject StRING tO re_place-_IN_SIDE";
	StringUtil::replaceAll(replace, "_", "");
	t.result(replace ==	"stRing subject StRING tO replace-INSIDE", "replaceAll-1");

	replace = "stRing subject StRING tO replace-INSIDE";
	StringUtil::replaceAll(replace, "RING", "ring");
	t.result(replace ==	"stRing subject String tO replace-INSIDE", "replaceAll-2");
	t.endGroup();

	t.beginGroup("FSUtil");
	t.result(FSUtil::exists(argv[0]), "exists-1");
	t.result(!FSUtil::exists("this_file_does_not_exist.txt"), "exists-2");

	t.result(FSUtil::isFile(argv[0]), "isFile-1");
	t.result(!FSUtil::isFile("this_file_does_not_exist.txt"), "isFile-2");
	t.result(!FSUtil::isFile(".."), "isFile-3");

	t.result(FSUtil::isDir(".."), "isDir-1");
	t.result(FSUtil::isDir("/"), "isDir-2");
	t.result(!FSUtil::isDir(argv[0]), "isDir-3");
	t.result(!FSUtil::isDir("this_file_does_not_exist.txt"), "isDir-4");

	FSUtil::putContents("this_file_exists.txt", "These are the contents of this file.");
	t.result(FSUtil::isFile("this_file_exists.txt"), "putContents-1/isFile-4");
	t.result(FSUtil::exists("this_file_exists.txt"), "putContents-2");

	t.result(FSUtil::getContents("this_file_exists.txt") == "These are the contents of this file.",
		"getContents-1");
	t.result(FSUtil::getContents("this_file_does_not_exist.txt") == "", "getContents-2");
	FSUtil::putContents("this_file_exists.txt", "These are the modified contents of this file.");
	t.result(FSUtil::getContents("this_file_exists.txt") == "These are the modified contents of this file.",
		"getContents-3/putContents-3");

	FSUtil::deleteFile("this_file_exists.txt");
	t.result(!FSUtil::exists("this_file_exists.txt"), "deleteFile-1/exists-3");

	// TODO: searchForFiles, which

	t.result(FSUtil::normalizePath("/whatever/this//is//../././../whatever/dir2/dir9/apathto.txt") ==
		"/whatever/whatever/dir2/dir9/apathto.txt", "normalizePath-1");
#ifdef _WIN32
	t.result(FSUtil::normalizePath("C:\\directory\\otherdirectory/someplace\\..\\../.\\wow") ==
		"C:/directory/wow", "normalizePath-win_1");
	t.result(FSUtil::normalizePath("/c/this/not/really/../../is/a/MSYS/or/Cygwin/path.txt") ==
		"C:/this/is/a/MSYS/or/Cygwin/path.txt", "normalizePath-win_2");
	t.result(FSUtil::normalizePath("/Nc/this/is/not/././is/../a/MSYS/or/Cygwin/path.txt") ==
		"/Nc/this/is/not/a/MSYS/or/Cygwin/path.txt", "normalizePath-win_3");
#else
	t.result(FSUtil::normalizePath("/c/this/not/really/../../is/a/MSYS/or/Cygwin/path.txt") ==
		"/c/this/is/a/MSYS/or/Cygwin/path.txt", "normalizePath-!win_1");
#endif

	t.result(FSUtil::combinePaths({"/whatever/", "this//is//", ".././",
			"/./../", "whatever/dir2/", "/dir9/apathto.txt"}) ==
		"/whatever/whatever/dir2/dir9/apathto.txt", "combinePaths-1");
	t.result(FSUtil::combinePaths({"/this/not", "really", "", "../..", "is/a//path.txt"}) ==
		"/this/is/a/path.txt", "combinePaths-2");
	t.result(FSUtil::combinePaths({"shortpath", "../../data/languages/"}) ==
		"../data/languages", "combinePaths-3");

	t.result(FSUtil::parentDirectory("/test/1.3.4.2/blah/") == "/test/1.3.4.2",
		"parentDirectory-1");
	t.result(FSUtil::parentDirectory("/test/1.3.4.2/blah/file.txt") == "/test/1.3.4.2/blah",
		"parentDirectory-2");
	t.result(FSUtil::parentDirectory("file.txt") == ".",
		"parentDirectory-3");

	FSUtil::mkdir("this_directory_now_exists");
	t.result(FSUtil::isDir("this_directory_now_exists"), "mkdir-1/isDir-5");
	FSUtil::rmdir("this_directory_now_exists");
	t.result(!FSUtil::isDir("this_directory_now_exists"), "rmdir-1/isDir-6");
	t.endGroup();

	return t.done();
}
<|endoftext|>


<commit_before>#ifndef UNIT_CONFIG_HPP_
#define UNIT_CONFIG_HPP_

namespace unit_config {

const float DT = 0.001;

// Maximum angular position in the roll and pitch axes (deg)
const float MAX_PITCH_ROLL_POS = 30.0;

// Maximum angular velocity in the roll and pitch axes (deg/s)
const float MAX_PITCH_ROLL_VEL = 100.0;

// Sensor offsets
const float GYR_X_OFFSET = -0.025;
const float GYR_Y_OFFSET = 0.021;
const float GYR_Z_OFFSET = 0.040;
const float ACC_X_OFFSET = 0.0;
const float ACC_Y_OFFSET = 0.0;
const float ACC_Z_OFFSET = -0.02;

// Initial angular velocity controller gains
const float ANGVEL_X_KP = 0.2;
const float ANGVEL_X_KI = 0.0;
const float ANGVEL_X_KD = 0.0;
const float ANGVEL_Y_KP = 0.2;
const float ANGVEL_Y_KI = 0.0;
const float ANGVEL_Y_KD = 0.0;
const float ANGVEL_Z_KP = 0.2;
const float ANGVEL_Z_KI = 0.0;
const float ANGVEL_Z_KD = 0.0;

// Initial angular position controller gains
const float ANGPOS_X_KP = 3.0;
const float ANGPOS_X_KI = 0.0;
const float ANGPOS_X_KD = 0.0;
const float ANGPOS_Y_KP = 3.0;
const float ANGPOS_Y_KI = 0.0;
const float ANGPOS_Y_KD = 0.0;
const float ANGPOS_Z_KP = 0.2;
const float ANGPOS_Z_KI = 0.0;
const float ANGPOS_Z_KD = 0.0;

}

#endif // UNIT_CONFIG_HPP_
<commit_msg>Calibrate icarus.<commit_after>#ifndef UNIT_CONFIG_HPP_
#define UNIT_CONFIG_HPP_

namespace unit_config {

const float DT = 0.001;

// Maximum angular position in the roll and pitch axes (deg)
const float MAX_PITCH_ROLL_POS = 30.0;

// Maximum angular velocity in the roll and pitch axes (deg/s)
const float MAX_PITCH_ROLL_VEL = 100.0;

// Sensor offsets
const float GYR_X_OFFSET = -0.016;
const float GYR_Y_OFFSET = -0.021;
const float GYR_Z_OFFSET = -0.002;
const float ACC_X_OFFSET = -0.005;
const float ACC_Y_OFFSET = -0.020;
const float ACC_Z_OFFSET =  0.071;

// Initial angular velocity controller gains
const float ANGVEL_X_KP = 0.2;
const float ANGVEL_X_KI = 0.0;
const float ANGVEL_X_KD = 0.0;
const float ANGVEL_Y_KP = 0.2;
const float ANGVEL_Y_KI = 0.0;
const float ANGVEL_Y_KD = 0.0;
const float ANGVEL_Z_KP = 0.2;
const float ANGVEL_Z_KI = 0.0;
const float ANGVEL_Z_KD = 0.0;

// Initial angular position controller gains
const float ANGPOS_X_KP = 3.0;
const float ANGPOS_X_KI = 0.0;
const float ANGPOS_X_KD = 0.0;
const float ANGPOS_Y_KP = 3.0;
const float ANGPOS_Y_KI = 0.0;
const float ANGPOS_Y_KD = 0.0;
const float ANGPOS_Z_KP = 0.2;
const float ANGPOS_Z_KI = 0.0;
const float ANGPOS_Z_KD = 0.0;

}

#endif // UNIT_CONFIG_HPP_
<|endoftext|>


<commit_before>/*
 * fMBT, free Model Based Testing tool
 * Copyright (c) 2011, Intel Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU Lesser General Public License,
 * version 2.1, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope 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 program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
 *
 */
#include "adapter_mapper.hh"
#include "model.hh"
#include "dparse.h"
#include <cstring>
#include "helper.hh"
#include "conf.hh"

#include <sstream>
#include <iostream>
#ifndef DROI
#include <boost/regex.hpp>
#endif

extern "C" {
extern D_ParserTables parser_tables_mrules;
};

extern Rules* amobj;

#define debugprint(...)

Adapter_mapper::Adapter_mapper(Log& log, std::string _params)
  : Adapter(log), Rules(), params(_params)
{
  robin=0;

}

bool Adapter_mapper::init()
{
  load(params);
  for(unsigned i=0;i<adapters.size();i++) {
    if (adapters[i]) {
      if (!adapters[i]->init()) {
        return false;
      }
    }
  }
  return true;
}

std::string Adapter_mapper::stringify()
{
  std::ostringstream t(std::ios::out | std::ios::binary);

  /* Write adapters */

  for(unsigned i=1;i<adapters.size();i++) {
    if (adapters[i]) {
      t << i << "=\"" << removehash(adapter_names[i])
	<< capsulate(adapters[i]->stringify()) << std::endl;
    }
  }

  t << std::endl;
  /*
  for(std::map<int,adapter_action>::iterator it=m2.begin();
      it!=m2.end();it++) {
  */
  for(unsigned j=0;j<actions->size();j++) {
    std::map<int,adapter_action>::iterator it=m2.find(j);
    if (it!=m2.end()) {
      adapter_action a=it->second;
      int i=it->first;

      t << "\"" << (*actions)[i] << "\" -> ("
	<< a.first << ", \""
	<< adapter_anames[a.first][a.second]
	<< "\")" << std::endl;
    }
  }

  return t.str();
}

int Adapter_mapper::anum_create(int index,std::string& n) {

  for(unsigned int i=0;i<adapter_anames[index].size();i++) {
    if (adapter_anames[index][i]==n) {
      return i;
    }
  }

  if (adapter_anames[index].size()==0) {
    std::string tau("");
    adapter_anames[index].push_back(tau);
  }

  adapter_anames[index].push_back(n);
  return adapter_anames[index].size()-1;
}

void Adapter_mapper::add_map(std::vector<int>& index,std::vector<std::string>& n,int action) {

  int anum = anum_create(index[0],n[0]);

  log.debug("%s(%i,%s,%i) %i\n",__func__,index[0],n[0].c_str(),action,anum);

  adapter_action a(index[0],anum);
  if (!is_used(action) && !is_used(a)) {
    m1[a]=action;
    m2[action]=a;
    for(unsigned u=1;u<index.size();u++) {
      int bnum = anum_create(index[u],n[u]);
      adapter_action b(index[u],bnum);
      log.debug("%s(%i,%s,%i) %i",__func__,index[u],n[u].c_str(),action,bnum);
      if (l_tau[u]) {
	_tm2.insert(std::pair<int,adapter_action>(action,b));
      } else {
	_fm2.insert(std::pair<int,adapter_action>(action,b));
      }
    }
  } else {
      log.debug("Error in '%s': ", load_name.c_str());
    if (is_used(action)) {
      log.debug("duplicate action on the model side: \"%s\"",
                (*actions)[action].c_str());
    } else { //  is_used(a)
      log.debug("duplicate action on the adapter side: \"%s\"",
        adapter_anames[a.first][a.second].c_str());
    }
    throw (int)42424;
  }
  log.debug("%s ready",__func__);
}

bool Adapter_mapper::load(std::string& name)
{
  D_Parser *p = new_D_Parser(&parser_tables_mrules, 16);
  char *s;

  log.debug("%s called",__func__);

  load_name = name;

  Rules* tmp=amobj;

  amobj=this;

  s=readfile(name.c_str());

  bool ret=dparse(p,s,std::strlen(s));

  if (ret) {
    log.debug("loading %s ok",name.c_str());
  } else {
    log.debug("loading %s failed",name.c_str());
    status=false;
  }

  free(s);

  free_D_Parser(p);

  amobj=tmp;

  /* Time to load adapters */

  for(unsigned int i=0;i<adapter_names.size();i++) {
    if (adapter_names[i]!=std::string("")) {

      log.debug("Loading adapter \"%s\"",
             adapter_names[i].c_str());

      std::string adapter_class;
      std::string adapter_params;

      Conf::split(adapter_names[i],adapter_class,adapter_params);

      log.debug("class %s, params %i",
             adapter_class.c_str(),
             adapter_params.c_str());

      Adapter* a = AdapterFactory::create(log, adapter_class,
                                          adapter_params);
      a->set_actions(&adapter_anames[i]);
      log.debug("Created adapter to %p",a);
      a->setparent(this);
      adapters[i] = a;
      if (!a->status) {
	status=false;
      }
    }
  }

  return true;
}

bool Adapter_mapper::is_used(int action)
{
  return (m2.find(action) != m2.end());
}

bool Adapter_mapper::is_used(adapter_action& action)
{
  return (m1.find(action)!=m1.end());
}

void Adapter_mapper::add_file(unsigned index, std::string& adaptername)
{
  log.debug("%s(%i,%s)",__func__,index,adaptername.c_str());
  if (adapter_names.capacity()<=index) {
    adapter_names.resize(index+2);
    adapter_anames.resize(index+2);
    adapters.resize(index+2);
  }
  adapter_names[index]=std::string(adaptername);
}

int Adapter_mapper::action_number(std::string& name)
{
  for(size_t i=0;i<actions->size();i++) {
    if ((*actions)[i]==name) {
      return i;
    }
  }
  return -1;
}

/*
 * Adds result action (We'll assume that there are at lest one
 * component)
 *
 */
void Adapter_mapper::add_result_action(std::string* name)
{
  int action=action_number(*name);

  log.debug("%s(%s) called",__func__,name->c_str());

  if (action==-1) {
#ifndef DROI
    /* try regexp case */
    log.debug("Regexp case");
    const char* format_string = l_name[0].c_str();

    log.debug("Format string \"%s\"",format_string);

    boost::regex expression(*name);
    boost::cmatch what;

    for(unsigned int i=1;i<actions->size();i++) {
      log.debug("Action %s",(*actions)[i].c_str());
      if(boost::regex_match((*actions)[i].c_str(), what, expression)) {
        /* Match */
        if (!is_used(i)) {
	  std::vector<std::string> s;

	  if (l_index.size()!=l_name.size()) {
	    abort();
	  }

	  for(unsigned j=0;j<l_index.size();j++) {
	    s.push_back(replace(expression,
				l_name[j].c_str(),
				(*actions)[i].begin(),
				(*actions)[i].end()));
	  }
          add_map(l_index,s,i);
        } else {
          log.debug("action %s already used, won't add it again.");
        }
      }
    }
#endif
  } else {
    add_map(l_index,l_name,action);
  }
  l_index.resize(0);
  l_name.resize(0);
  l_tau.resize(0);
}

void Adapter_mapper::add_component(unsigned int index,std::string& name, bool tau)
{
  /* Validate index */
  log.debug("%s(%i,%s)",__func__,index,name.c_str());

  if (index>=adapter_names.size() || adapter_names[index]==std::string("")) {

    log.debug("adapter_names.size() %i,"
           "adapter_names[index] \"%s\"",
           adapter_names.size(),adapter_names[index].c_str());

    log.debug("%s\n%s\n",
           adapter_names[1].c_str(),
           adapter_names[2].c_str());

    throw((int)420);
  }

  l_name.push_back(name);
  l_index.push_back(index);
  l_tau.push_back(tau);
}

int Adapter_mapper::adapter_action_execute(adapter_action& a)
{
  log.print("<redirect id=\"%i\" name=\"%s\" action=\"%s\"/>\n",
            a.first,adapter_names[a.first].c_str(),adapter_anames[a.first][a.second].c_str());

  std::vector<int> f;
  f.push_back(a.second);

  adapters[a.first]->execute(f);
  return f[0];
}

int Adapter_mapper::map_execute(adapter_action& a)
{
  /* redirect to correct adapter */
  log.debug("execute with adapter %i (%s) action %i (%s)\n",
         a.first,adapter_names[a.first].c_str(),a.second,
         adapter_anames[a.first][a.second].c_str());

  log.print("<redirect id=\"%i\" name=\"%s\" action=\"%s\"/>\n",
            a.first,adapter_names[a.first].c_str(),adapter_anames[a.first][a.second].c_str());

  std::vector<int> f;
  f.push_back(a.second);

  adapters[a.first]->execute(f);
  m1_convert(a.first,f);
  return f[0];
}

bool Adapter_mapper::map_execute(int& action)
{
  adapter_action a=m2[action];

  if (a.first==0) { /* No such action map? */
    log.debug("execute without adapter: %i (action not mapped to any adapter)\n",action);
    return false;
  }

  action=map_execute(a);

  return true;
}

void Adapter_mapper::execute(std::vector<int>& action)
{
  log.push("adapter_mapper");
  /* Output Actions for each component */

  bool tau=false;

  if (map_execute(action[0])) {
    std::pair<std::multimap<int,adapter_action>::iterator,
      std::multimap<int,adapter_action>::iterator> r =
      _tm2.equal_range(action[0]);
    adapter_action a;

    for(std::multimap<int,adapter_action>::iterator i=r.first;
	i!=r.second;i++)
      {
	a=i->second;
	log.debug("execute extras with check adapter %i (%s) action %i (%s)\n",
		  a.first,adapter_names[a.first].c_str(),a.second,
		  adapter_anames[a.first][a.second].c_str());

	// Handle true case
	if (a.second!=
	    adapter_action_execute(a)) {
	  /* ERROR. adapter didn't response as expected */
	  log.print("<configuration failure, returning tau>\n");
	  tau=true;
	}
      }

    r = _fm2.equal_range(action[0]);

    for(std::multimap<int,adapter_action>::iterator i=r.first;
	i!=r.second;i++)
      {
	// Handle false case
	a=i->second;
	log.debug("execute extras without check adapter %i (%s) action %i (%s)\n",
		  a.first,adapter_names[a.first].c_str(),a.second,
		  adapter_anames[a.first][a.second].c_str());

	adapter_action_execute(a);
      }

  }

  if (tau) {
    action.resize(1);
    action[0]=0;
  }

  log.pop();
}

void Adapter_mapper::m1_convert(int index,
				std::vector<int>&action)
{
  for(unsigned i=0;i<action.size();i++) {
    action[i] = m1[adapter_action(index,action[i])];
  }
}

bool Adapter_mapper::observeRobin(std::vector<int> &action)
{
  if (adapters.size()==0) {
    abort();
  }

  if (robin==adapters.size()) {
    robin=0;
    /* Let's update the current time */
    gettimeofday(&Adapter::current_time,NULL);
  }

  if (adapters[robin]==NULL) {
    robin++;
    return observeRobin(action);
  }
  bool ret = adapters[robin]->observe(action,false);
  if (ret)
    m1_convert(robin,action);
  robin++;
  return ret;
}

bool Adapter_mapper::observe(std::vector<int> &action,bool block)
{
  /* Ok. This is a bit hairy */
  do {
    if (observeRobin(action)) {
      return true;
    }
  } while (block);
  return false;
}

FACTORY_DEFAULT_CREATOR(Adapter, Adapter_mapper, "mapper");
<commit_msg>don't throw. report errors<commit_after>/*
 * fMBT, free Model Based Testing tool
 * Copyright (c) 2011, Intel Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU Lesser General Public License,
 * version 2.1, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope 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 program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
 *
 */
#include "adapter_mapper.hh"
#include "model.hh"
#include "dparse.h"
#include <cstring>
#include "helper.hh"
#include "conf.hh"

#include <sstream>
#include <iostream>
#ifndef DROI
#include <boost/regex.hpp>
#endif

extern "C" {
extern D_ParserTables parser_tables_mrules;
};

extern Rules* amobj;

#define debugprint(...)

Adapter_mapper::Adapter_mapper(Log& log, std::string _params)
  : Adapter(log), Rules(), params(_params)
{
  robin=0;

}

bool Adapter_mapper::init()
{
  load(params);
  for(unsigned i=0;i<adapters.size();i++) {
    if (adapters[i]) {
      if (!adapters[i]->init()) {
        return false;
      }
    }
  }
  return true;
}

std::string Adapter_mapper::stringify()
{
  std::ostringstream t(std::ios::out | std::ios::binary);

  /* Write adapters */

  for(unsigned i=1;i<adapters.size();i++) {
    if (adapters[i]) {
      t << i << "=\"" << removehash(adapter_names[i])
	<< capsulate(adapters[i]->stringify()) << std::endl;
    }
  }

  t << std::endl;
  /*
  for(std::map<int,adapter_action>::iterator it=m2.begin();
      it!=m2.end();it++) {
  */
  for(unsigned j=0;j<actions->size();j++) {
    std::map<int,adapter_action>::iterator it=m2.find(j);
    if (it!=m2.end()) {
      adapter_action a=it->second;
      int i=it->first;

      t << "\"" << (*actions)[i] << "\" -> ("
	<< a.first << ", \""
	<< adapter_anames[a.first][a.second]
	<< "\")" << std::endl;
    }
  }

  return t.str();
}

int Adapter_mapper::anum_create(int index,std::string& n) {

  for(unsigned int i=0;i<adapter_anames[index].size();i++) {
    if (adapter_anames[index][i]==n) {
      return i;
    }
  }

  if (adapter_anames[index].size()==0) {
    std::string tau("");
    adapter_anames[index].push_back(tau);
  }

  adapter_anames[index].push_back(n);
  return adapter_anames[index].size()-1;
}

void Adapter_mapper::add_map(std::vector<int>& index,std::vector<std::string>& n,int action) {

  if (!status) {
    return;
  }
  int anum = anum_create(index[0],n[0]);

  log.debug("%s(%i,%s,%i) %i\n",__func__,index[0],n[0].c_str(),action,anum);

  adapter_action a(index[0],anum);
  if (!is_used(action) && !is_used(a)) {
    m1[a]=action;
    m2[action]=a;
    for(unsigned u=1;u<index.size();u++) {
      int bnum = anum_create(index[u],n[u]);
      adapter_action b(index[u],bnum);
      log.debug("%s(%i,%s,%i) %i",__func__,index[u],n[u].c_str(),action,bnum);
      if (l_tau[u]) {
	_tm2.insert(std::pair<int,adapter_action>(action,b));
      } else {
	_fm2.insert(std::pair<int,adapter_action>(action,b));
      }
    }
  } else {
    //log.debug("Error in '%s': ", load_name.c_str());
    if (is_used(action)) {
      errormsg=std::string("duplicate action on the model side: \"")+(*actions)[action]+std::string("\"");
    } else { //  is_used(a)
      errormsg=std::string("duplicate action on the adapter side: \"")+(*actions)[action]+std::string("\"");
    }
    status=false;
  }
  log.debug("%s ready",__func__);
}

bool Adapter_mapper::load(std::string& name)
{
  D_Parser *p = new_D_Parser(&parser_tables_mrules, 16);
  char *s;

  log.debug("%s called",__func__);

  load_name = name;

  Rules* tmp=amobj;

  amobj=this;

  s=readfile(name.c_str());

  bool ret=dparse(p,s,std::strlen(s));

  if (ret) {
    log.debug("loading %s ok",name.c_str());
  } else {
    log.debug("loading %s failed",name.c_str());
    errormsg=std::string("Loading file \"")+name+std::string("\" failed");
    status=false;
    return status;
  }

  free(s);

  free_D_Parser(p);

  amobj=tmp;

  /* Time to load adapters */

  for(unsigned int i=0;i<adapter_names.size()&&status;i++) {
    if (adapter_names[i]!=std::string("")) {

      log.debug("Loading adapter \"%s\"",
             adapter_names[i].c_str());

      std::string adapter_class;
      std::string adapter_params;

      Conf::split(adapter_names[i],adapter_class,adapter_params);

      log.debug("class %s, params %i",
             adapter_class.c_str(),
             adapter_params.c_str());

      Adapter* a = AdapterFactory::create(log, adapter_class,
                                          adapter_params);
      a->set_actions(&adapter_anames[i]);
      log.debug("Created adapter to %p",a);
      a->setparent(this);
      adapters[i] = a;
      if (!a->status) {
	status=false;
      }
    }
  }

  return status;
}

bool Adapter_mapper::is_used(int action)
{
  return (m2.find(action) != m2.end());
}

bool Adapter_mapper::is_used(adapter_action& action)
{
  return (m1.find(action)!=m1.end());
}

void Adapter_mapper::add_file(unsigned index, std::string& adaptername)
{
  log.debug("%s(%i,%s)",__func__,index,adaptername.c_str());
  if (adapter_names.capacity()<=index) {
    adapter_names.resize(index+2);
    adapter_anames.resize(index+2);
    adapters.resize(index+2);
  }
  adapter_names[index]=std::string(adaptername);
}

int Adapter_mapper::action_number(std::string& name)
{
  for(size_t i=0;i<actions->size();i++) {
    if ((*actions)[i]==name) {
      return i;
    }
  }
  return -1;
}

/*
 * Adds result action (We'll assume that there are at lest one
 * component)
 *
 */
void Adapter_mapper::add_result_action(std::string* name)
{
  if (!status) {
    return;
  }
  int action=action_number(*name);

  log.debug("%s(%s) called",__func__,name->c_str());

  if (action==-1) {
#ifndef DROI
    /* try regexp case */
    log.debug("Regexp case");
    const char* format_string = l_name[0].c_str();

    log.debug("Format string \"%s\"",format_string);

    boost::regex expression(*name);
    boost::cmatch what;

    for(unsigned int i=1;i<actions->size();i++) {
      log.debug("Action %s",(*actions)[i].c_str());
      if(boost::regex_match((*actions)[i].c_str(), what, expression)) {
        /* Match */
        if (!is_used(i)) {
	  std::vector<std::string> s;

	  if (l_index.size()!=l_name.size()) {
	    abort();
	  }

	  for(unsigned j=0;j<l_index.size();j++) {
	    s.push_back(replace(expression,
				l_name[j].c_str(),
				(*actions)[i].begin(),
				(*actions)[i].end()));
	  }
          add_map(l_index,s,i);
        } else {
          log.debug("action %s already used, won't add it again.");
        }
      }
    }
#endif
  } else {
    add_map(l_index,l_name,action);
  }
  l_index.resize(0);
  l_name.resize(0);
  l_tau.resize(0);
}

void Adapter_mapper::add_component(unsigned int index,std::string& name, bool tau)
{
  if (!status) {
    return;
  }
  /* Validate index */
  log.debug("%s(%i,%s)",__func__,index,name.c_str());

  if (index>=adapter_names.size() || adapter_names[index]==std::string("")) {

    log.debug("adapter_names.size() %i,"
           "adapter_names[index] \"%s\"",
           adapter_names.size(),adapter_names[index].c_str());

    log.debug("%s\n%s\n",
           adapter_names[1].c_str(),
           adapter_names[2].c_str());

    errormsg=std::string("index ") + to_string(index) + std::string(" out of bounds");
    status=false;
  }

  l_name.push_back(name);
  l_index.push_back(index);
  l_tau.push_back(tau);
}

int Adapter_mapper::adapter_action_execute(adapter_action& a)
{
  log.print("<redirect id=\"%i\" name=\"%s\" action=\"%s\"/>\n",
            a.first,adapter_names[a.first].c_str(),adapter_anames[a.first][a.second].c_str());

  std::vector<int> f;
  f.push_back(a.second);

  adapters[a.first]->execute(f);
  return f[0];
}

int Adapter_mapper::map_execute(adapter_action& a)
{
  /* redirect to correct adapter */
  log.debug("execute with adapter %i (%s) action %i (%s)\n",
         a.first,adapter_names[a.first].c_str(),a.second,
         adapter_anames[a.first][a.second].c_str());

  log.print("<redirect id=\"%i\" name=\"%s\" action=\"%s\"/>\n",
            a.first,adapter_names[a.first].c_str(),adapter_anames[a.first][a.second].c_str());

  std::vector<int> f;
  f.push_back(a.second);

  adapters[a.first]->execute(f);
  m1_convert(a.first,f);
  return f[0];
}

bool Adapter_mapper::map_execute(int& action)
{
  adapter_action a=m2[action];

  if (a.first==0) { /* No such action map? */
    log.debug("execute without adapter: %i (action not mapped to any adapter)\n",action);
    return false;
  }

  action=map_execute(a);

  return true;
}

void Adapter_mapper::execute(std::vector<int>& action)
{
  log.push("adapter_mapper");
  /* Output Actions for each component */

  bool tau=false;

  if (map_execute(action[0])) {
    std::pair<std::multimap<int,adapter_action>::iterator,
      std::multimap<int,adapter_action>::iterator> r =
      _tm2.equal_range(action[0]);
    adapter_action a;

    for(std::multimap<int,adapter_action>::iterator i=r.first;
	i!=r.second;i++)
      {
	a=i->second;
	log.debug("execute extras with check adapter %i (%s) action %i (%s)\n",
		  a.first,adapter_names[a.first].c_str(),a.second,
		  adapter_anames[a.first][a.second].c_str());

	// Handle true case
	if (a.second!=
	    adapter_action_execute(a)) {
	  /* ERROR. adapter didn't response as expected */
	  log.print("<configuration failure, returning tau>\n");
	  tau=true;
	}
      }

    r = _fm2.equal_range(action[0]);

    for(std::multimap<int,adapter_action>::iterator i=r.first;
	i!=r.second;i++)
      {
	// Handle false case
	a=i->second;
	log.debug("execute extras without check adapter %i (%s) action %i (%s)\n",
		  a.first,adapter_names[a.first].c_str(),a.second,
		  adapter_anames[a.first][a.second].c_str());

	adapter_action_execute(a);
      }

  }

  if (tau) {
    action.resize(1);
    action[0]=0;
  }

  log.pop();
}

void Adapter_mapper::m1_convert(int index,
				std::vector<int>&action)
{
  for(unsigned i=0;i<action.size();i++) {
    action[i] = m1[adapter_action(index,action[i])];
  }
}

bool Adapter_mapper::observeRobin(std::vector<int> &action)
{
  if (adapters.size()==0) {
    abort();
  }

  if (robin==adapters.size()) {
    robin=0;
    /* Let's update the current time */
    gettimeofday(&Adapter::current_time,NULL);
  }

  if (adapters[robin]==NULL) {
    robin++;
    return observeRobin(action);
  }
  bool ret = adapters[robin]->observe(action,false);
  if (ret)
    m1_convert(robin,action);
  robin++;
  return ret;
}

bool Adapter_mapper::observe(std::vector<int> &action,bool block)
{
  /* Ok. This is a bit hairy */
  do {
    if (observeRobin(action)) {
      return true;
    }
  } while (block);
  return false;
}

FACTORY_DEFAULT_CREATOR(Adapter, Adapter_mapper, "mapper");
<|endoftext|>


<commit_before>/*  This file is part of the KDE project
    Copyright (C) 2006-2007 Matthias Kretz <kretz@kde.org>

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

    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
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public License
    along with this library; see the file COPYING.LIB.  If not, write to
    the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
    Boston, MA 02110-1301, USA.

*/

#include "main.h"
#include <kgenericfactory.h>
#include <kaboutdata.h>
#include <kdeversion.h>
#include <QtGui/QBoxLayout>
#include <kdialog.h>
#include "outputdevicechoice.h"
#include <QtGui/QTabWidget>
#include "backendselection.h"

K_PLUGIN_FACTORY(PhononKcmFactory, registerPlugin<PhononKcm>();)
K_EXPORT_PLUGIN(PhononKcmFactory("kcm_phonon"))

PhononKcm::PhononKcm(QWidget *parent, const QVariantList &args)
    : KCModule(PhononKcmFactory::componentData(), parent, args)
{
    KAboutData *about = new KAboutData(
            "kcm_phonon", 0, ki18n("Phonon Configuration Module"),
            KDE_VERSION_STRING, KLocalizedString(), KAboutData::License_GPL,
            ki18n("Copyright 2006 Matthias Kretz"));
    about->addAuthor(ki18n("Matthias Kretz"), KLocalizedString(), "kretz@kde.org");
    setAboutData(about);

    setLayout(new QHBoxLayout);
    layout()->setMargin(0);
    layout()->setSpacing(0);

    QTabWidget *tabs = new QTabWidget(this);
    layout()->addWidget(tabs);

    m_outputDeviceWidget = new OutputDeviceChoice;
    tabs->addTab(m_outputDeviceWidget, i18n("Device Preference"));
    m_backendSelection = new BackendSelection;
    tabs->addTab(m_backendSelection, i18n("Backend"));
    load();
    connect(m_backendSelection, SIGNAL(changed()), SLOT(changed()));
    connect(m_outputDeviceWidget, SIGNAL(changed()), SLOT(changed()));
}

void PhononKcm::load()
{
    m_outputDeviceWidget->load();
    m_backendSelection->load();
}

void PhononKcm::save()
{
    m_outputDeviceWidget->save();
    m_backendSelection->save();
}

void PhononKcm::defaults()
{
    m_outputDeviceWidget->defaults();
    m_backendSelection->defaults();
}

#include "main.moc"
// vim: ts=4
<commit_msg>krazy thinks I should use KTabWidget. Not a bad idea.<commit_after>/*  This file is part of the KDE project
    Copyright (C) 2006-2007 Matthias Kretz <kretz@kde.org>

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

    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
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public License
    along with this library; see the file COPYING.LIB.  If not, write to
    the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
    Boston, MA 02110-1301, USA.

*/

#include "main.h"
#include <kgenericfactory.h>
#include <ktabwidget.h>
#include <kaboutdata.h>
#include <kdeversion.h>
#include <QtGui/QBoxLayout>
#include <kdialog.h>
#include "outputdevicechoice.h"
#include "backendselection.h"

K_PLUGIN_FACTORY(PhononKcmFactory, registerPlugin<PhononKcm>();)
K_EXPORT_PLUGIN(PhononKcmFactory("kcm_phonon"))

PhononKcm::PhononKcm(QWidget *parent, const QVariantList &args)
    : KCModule(PhononKcmFactory::componentData(), parent, args)
{
    KAboutData *about = new KAboutData(
            "kcm_phonon", 0, ki18n("Phonon Configuration Module"),
            KDE_VERSION_STRING, KLocalizedString(), KAboutData::License_GPL,
            ki18n("Copyright 2006 Matthias Kretz"));
    about->addAuthor(ki18n("Matthias Kretz"), KLocalizedString(), "kretz@kde.org");
    setAboutData(about);

    setLayout(new QHBoxLayout);
    layout()->setMargin(0);
    layout()->setSpacing(0);

    KTabWidget *tabs = new KTabWidget(this);
    layout()->addWidget(tabs);

    m_outputDeviceWidget = new OutputDeviceChoice;
    tabs->addTab(m_outputDeviceWidget, i18n("Device Preference"));
    m_backendSelection = new BackendSelection;
    tabs->addTab(m_backendSelection, i18n("Backend"));
    load();
    connect(m_backendSelection, SIGNAL(changed()), SLOT(changed()));
    connect(m_outputDeviceWidget, SIGNAL(changed()), SLOT(changed()));
}

void PhononKcm::load()
{
    m_outputDeviceWidget->load();
    m_backendSelection->load();
}

void PhononKcm::save()
{
    m_outputDeviceWidget->save();
    m_backendSelection->save();
}

void PhononKcm::defaults()
{
    m_outputDeviceWidget->defaults();
    m_backendSelection->defaults();
}

#include "main.moc"
// vim: ts=4
<|endoftext|>