<commit_before>/*
 * SchemeSmobAV.c
 *
 * Scheme small objects (SMOBS) for attention values.
 *
 * Copyright (c) 2008,2009 Linas Vepstas <linas@linas.org>
 */

#include <cstddef>
#include <libguile.h>

#include <opencog/attentionbank/AttentionBank.h>
#include <opencog/truthvalue/AttentionValue.h>
#include <opencog/guile/SchemeSmob.h>

using namespace opencog;

/* ============================================================== */
/**
 * Search for an attention value in a list of values.
 * Return the attention value if found, else return null.
 * Throw errors if the list is not stictly just key-value pairs
 */
AttentionValuePtr SchemeSmob::get_av_from_list(SCM slist)
{
	while (scm_is_pair(slist))
	{
		SCM sval = SCM_CAR(slist);
		if (SCM_SMOB_PREDICATE(SchemeSmob::cog_misc_tag, sval))
		{
			scm_t_bits misctype = SCM_SMOB_FLAGS(sval);
			switch (misctype)
			{
				case COG_PROTOM: {
					ProtoAtomPtr pa(scm_to_protom(sval));
					AttentionValuePtr av(AttentionValueCast(pa));
					if (av) return av;
				}
				default:
					break;
			}
		}

		slist = SCM_CDR(slist);
	}

	return nullptr;
}

/* ============================================================== */

std::string SchemeSmob::av_to_string(const AttentionValuePtr& av)
{
#define BUFLEN 120
	char buff[BUFLEN];

	// We pretend that they are actually short integers.
	snprintf(buff, BUFLEN, "(av %f %f %f)",
	         av->getSTI(), av->getLTI(), av->getVLTI());

	return buff;
}

/* ============================================================== */

SCM SchemeSmob::av_to_scm (const AttentionValuePtr& av)
{
	return protom_to_scm(ProtoAtomCast(av));
}

/**
 * Create a new attention value, with indicated sti/lti/vlti
 */
SCM SchemeSmob::ss_new_av (SCM ssti, SCM slti, SCM svlti)
{
	if (!scm_is_real(ssti)) {
		scm_wrong_type_arg_msg("cog-new-av", 1, ssti, "double");
	}
	if (!scm_is_real(slti)) {
		scm_wrong_type_arg_msg("cog-new-av", 2, slti, "double");
	}
	if (!scm_is_real(svlti)) {
		scm_wrong_type_arg_msg("cog-new-av", 3, svlti, "double");
	}
	AttentionValue::sti_t sti = scm_to_double(ssti);
	AttentionValue::lti_t lti = scm_to_double(slti);
	AttentionValue::vlti_t vlti = scm_to_double(svlti);
	AttentionValuePtr av = AttentionValue::createAV(sti, lti, vlti);
	return av_to_scm(av);
}

/* ============================================================== */
/**
 * Return true if the scm is an attention value
 */
SCM SchemeSmob::ss_av_p (SCM s)
{
	ProtoAtomPtr pa(scm_to_protom(s));
	if (nullptr == pa) return SCM_BOOL_F;

	if (pa->get_type() == ATTENTION_VALUE)
		return SCM_BOOL_T;

	scm_remember_upto_here_1(s);
	return SCM_BOOL_F;
}

/* ============================================================== */

AttentionValuePtr SchemeSmob::verify_av(SCM sav, const char *subrname, int pos)
{
	ProtoAtomPtr pa(scm_to_protom(sav));
	AttentionValuePtr av(AttentionValueCast(pa));

	if (nullptr == av)
		scm_wrong_type_arg_msg(subrname, pos, sav, "opencog attention value");

	return av;
}

/**
 * Return association list holding contents of an attention value
 */
SCM SchemeSmob::ss_av_get_value (SCM s)
{
	AttentionValuePtr av = verify_av(s, "cog-av->alist");

	// Format AV values to two decimal pts.
	auto format = [](double n){
	char number_str[20];
	sprintf(number_str, "%.2f", n);
	return atof(number_str);
	};

	SCM sti =scm_from_double(format(av->getSTI()));
	SCM lti = scm_from_double(format(av->getLTI()));
	SCM vlti = scm_from_double(format(av->getVLTI()));

	SCM ssti = scm_from_utf8_symbol("sti");
	SCM slti = scm_from_utf8_symbol("lti");
	SCM svlti = scm_from_utf8_symbol("vlti");
	scm_remember_upto_here_1(s);

	SCM rc = SCM_EOL;
	rc = scm_acons(svlti, vlti, rc);
	rc = scm_acons(slti, lti, rc);
	rc = scm_acons(ssti, sti, rc);
	return rc;
}

/* ===================== END OF FILE ============================ */
<commit_msg>Remove formatting.It can be done from scheme.<commit_after>/*
 * SchemeSmobAV.c
 *
 * Scheme small objects (SMOBS) for attention values.
 *
 * Copyright (c) 2008,2009 Linas Vepstas <linas@linas.org>
 */

#include <cstddef>
#include <libguile.h>

#include <opencog/attentionbank/AttentionBank.h>
#include <opencog/truthvalue/AttentionValue.h>
#include <opencog/guile/SchemeSmob.h>

using namespace opencog;

/* ============================================================== */
/**
 * Search for an attention value in a list of values.
 * Return the attention value if found, else return null.
 * Throw errors if the list is not stictly just key-value pairs
 */
AttentionValuePtr SchemeSmob::get_av_from_list(SCM slist)
{
	while (scm_is_pair(slist))
	{
		SCM sval = SCM_CAR(slist);
		if (SCM_SMOB_PREDICATE(SchemeSmob::cog_misc_tag, sval))
		{
			scm_t_bits misctype = SCM_SMOB_FLAGS(sval);
			switch (misctype)
			{
				case COG_PROTOM: {
					ProtoAtomPtr pa(scm_to_protom(sval));
					AttentionValuePtr av(AttentionValueCast(pa));
					if (av) return av;
				}
				default:
					break;
			}
		}

		slist = SCM_CDR(slist);
	}

	return nullptr;
}

/* ============================================================== */

std::string SchemeSmob::av_to_string(const AttentionValuePtr& av)
{
#define BUFLEN 120
	char buff[BUFLEN];

	// We pretend that they are actually short integers.
	snprintf(buff, BUFLEN, "(av %f %f %f)",
	         av->getSTI(), av->getLTI(), av->getVLTI());

	return buff;
}

/* ============================================================== */

SCM SchemeSmob::av_to_scm (const AttentionValuePtr& av)
{
	return protom_to_scm(ProtoAtomCast(av));
}

/**
 * Create a new attention value, with indicated sti/lti/vlti
 */
SCM SchemeSmob::ss_new_av (SCM ssti, SCM slti, SCM svlti)
{
	if (!scm_is_real(ssti)) {
		scm_wrong_type_arg_msg("cog-new-av", 1, ssti, "double");
	}
	if (!scm_is_real(slti)) {
		scm_wrong_type_arg_msg("cog-new-av", 2, slti, "double");
	}
	if (!scm_is_real(svlti)) {
		scm_wrong_type_arg_msg("cog-new-av", 3, svlti, "double");
	}
	AttentionValue::sti_t sti = scm_to_double(ssti);
	AttentionValue::lti_t lti = scm_to_double(slti);
	AttentionValue::vlti_t vlti = scm_to_double(svlti);
	AttentionValuePtr av = AttentionValue::createAV(sti, lti, vlti);
	return av_to_scm(av);
}

/* ============================================================== */
/**
 * Return true if the scm is an attention value
 */
SCM SchemeSmob::ss_av_p (SCM s)
{
	ProtoAtomPtr pa(scm_to_protom(s));
	if (nullptr == pa) return SCM_BOOL_F;

	if (pa->get_type() == ATTENTION_VALUE)
		return SCM_BOOL_T;

	scm_remember_upto_here_1(s);
	return SCM_BOOL_F;
}

/* ============================================================== */

AttentionValuePtr SchemeSmob::verify_av(SCM sav, const char *subrname, int pos)
{
	ProtoAtomPtr pa(scm_to_protom(sav));
	AttentionValuePtr av(AttentionValueCast(pa));

	if (nullptr == av)
		scm_wrong_type_arg_msg(subrname, pos, sav, "opencog attention value");

	return av;
}

/**
 * Return association list holding contents of an attention value
 */
SCM SchemeSmob::ss_av_get_value (SCM s)
{
	AttentionValuePtr av = verify_av(s, "cog-av->alist");

	SCM sti =scm_from_double(av->getSTI());
	SCM lti = scm_from_double(av->getLTI());
	SCM vlti = scm_from_double(av->getVLTI());

	SCM ssti = scm_from_utf8_symbol("sti");
	SCM slti = scm_from_utf8_symbol("lti");
	SCM svlti = scm_from_utf8_symbol("vlti");
	scm_remember_upto_here_1(s);

	SCM rc = SCM_EOL;
	rc = scm_acons(svlti, vlti, rc);
	rc = scm_acons(slti, lti, rc);
	rc = scm_acons(ssti, sti, rc);
	return rc;
}

/* ===================== END OF FILE ============================ */
<|endoftext|>


<commit_before>// @(#)root/net:$Id$
// Author: Fons Rademakers   19/1/2001

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

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TPServerSocket                                                       //
//                                                                      //
// This class implements parallel server sockets. A parallel server     //
// socket waits for requests to come in over the network. It performs   //
// some operation based on that request and then possibly returns a     //
// full duplex parallel socket to the requester. The actual work is     //
// done via the TSystem class (either TUnixSystem or TWinNTSystem).     //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#include "TPServerSocket.h"
#include "TPSocket.h"
#include "TROOT.h"
#include "TVirtualMutex.h"

ClassImp(TPServerSocket)

//______________________________________________________________________________
TPServerSocket::TPServerSocket(Int_t port, Bool_t reuse, Int_t backlog,
                               Int_t tcpwindowsize) :
   TServerSocket(port, reuse, backlog, tcpwindowsize)
{
   // Create a parallel server socket object on a specified port. Set reuse
   // to true to force reuse of the server socket (i.e. do not wait for the
   // time out to pass). Using backlog one can set the desirable queue length
   // for pending connections.
   // Use tcpwindowsize to specify the size of the receive buffer, it has
   // to be specified here to make sure the window scale option is set (for
   // tcpwindowsize > 65KB and for platforms supporting window scaling).
   // Use IsValid() to check the validity of the
   // server socket. In case server socket is not valid use GetErrorCode()
   // to obtain the specific error value. These values are:
   //  0 = no error (socket is valid)
   // -1 = low level socket() call failed
   // -2 = low level bind() call failed
   // -3 = low level listen() call failed
   // Every valid server socket is added to the TROOT sockets list which
   // will make sure that any open sockets are properly closed on
   // program termination.

   fTcpWindowSize = tcpwindowsize;
   SetName("PServerSocket");
}

//______________________________________________________________________________
TPServerSocket::TPServerSocket(const char *service, Bool_t reuse, Int_t backlog,
                               Int_t tcpwindowsize) :
   TServerSocket(service, reuse, backlog, tcpwindowsize)
{
   // Create a parallel server socket object for a named service. Set reuse
   // to true to force reuse of the server socket (i.e. do not wait for the
   // time out to pass). Using backlog one can set the desirable queue length
   // for pending connections.
   // Use tcpwindowsize to specify the size of the receive buffer, it has
   // to be specified here to make sure the window scale option is set (for
   // tcpwindowsize > 65KB and for platforms supporting window scaling).
   // Use IsValid() to check the validity of the
   // server socket. In case server socket is not valid use GetErrorCode()
   // to obtain the specific error value. These values are:
   //  0 = no error (socket is valid)
   // -1 = low level socket() call failed
   // -2 = low level bind() call failed
   // -3 = low level listen() call failed
   // Every valid server socket is added to the TROOT sockets list which
   // will make sure that any open sockets are properly closed on
   // program termination.

   fTcpWindowSize = tcpwindowsize;
   SetName("PServerSocket");
}

//______________________________________________________________________________
TSocket *TPServerSocket::Accept(UChar_t Opt)
{
   // Accept a connection on a parallel server socket. Returns a full-duplex
   // parallel communication TPSocket object. If no pending connections are
   // present on the queue and nonblocking mode has not been enabled
   // with SetOption(kNoBlock,1) the call blocks until a connection is
   // present. The returned socket must be deleted by the user. The socket
   // is also added to the TROOT sockets list which will make sure that
   // any open sockets are properly closed on program termination.
   // In case of error 0 is returned and in case non-blocking I/O is
   // enabled and no connections are available -1 is returned.

   TSocket  *setupSocket = 0;
   TSocket  **pSockets;
   TPSocket *newPSocket = 0;

   Int_t size, port;

   // wait for the incoming connections to the server and accept them
   setupSocket = TServerSocket::Accept(Opt);

   if (setupSocket <= 0) return 0;

   // receive the port number and number of parallel sockets from the
   // client and establish 'n' connections
   if (setupSocket->Recv(port, size) < 0) {
      Error("Accept", "error receiving port number and number of sockets");
      return 0;
   }

   // Check if client is running in single mode
   if (size == 0) {
      pSockets = new TSocket*[1];

      pSockets[0] = setupSocket;

      // create TPSocket object with the original socket
      newPSocket = new TPSocket(pSockets, 1);

   } else {
      pSockets = new TSocket*[size];

      for (int i = 0; i < size; i++) {
         pSockets[i] = new TSocket(setupSocket->GetInetAddress(),
                                           port, fTcpWindowSize);
         R__LOCKGUARD2(gROOTMutex);
         gROOT->GetListOfSockets()->Remove(pSockets[i]);
      }

      // create TPSocket object with all the accepted sockets
      newPSocket = new TPSocket(pSockets, size);

   }

   // Transmit authentication information, if any
   if (setupSocket->IsAuthenticated())
      newPSocket->SetSecContext(setupSocket->GetSecContext());

   // clean up, if needed
   if (size > 0)
      delete setupSocket;

   // return the TSocket object
   return newPSocket;
}

<commit_msg>Suppress warning: ordered comparison of pointer with integer zero<commit_after>// @(#)root/net:$Id$
// Author: Fons Rademakers   19/1/2001

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

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TPServerSocket                                                       //
//                                                                      //
// This class implements parallel server sockets. A parallel server     //
// socket waits for requests to come in over the network. It performs   //
// some operation based on that request and then possibly returns a     //
// full duplex parallel socket to the requester. The actual work is     //
// done via the TSystem class (either TUnixSystem or TWinNTSystem).     //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#include "TPServerSocket.h"
#include "TPSocket.h"
#include "TROOT.h"
#include "TVirtualMutex.h"

ClassImp(TPServerSocket)

//______________________________________________________________________________
TPServerSocket::TPServerSocket(Int_t port, Bool_t reuse, Int_t backlog,
                               Int_t tcpwindowsize) :
   TServerSocket(port, reuse, backlog, tcpwindowsize)
{
   // Create a parallel server socket object on a specified port. Set reuse
   // to true to force reuse of the server socket (i.e. do not wait for the
   // time out to pass). Using backlog one can set the desirable queue length
   // for pending connections.
   // Use tcpwindowsize to specify the size of the receive buffer, it has
   // to be specified here to make sure the window scale option is set (for
   // tcpwindowsize > 65KB and for platforms supporting window scaling).
   // Use IsValid() to check the validity of the
   // server socket. In case server socket is not valid use GetErrorCode()
   // to obtain the specific error value. These values are:
   //  0 = no error (socket is valid)
   // -1 = low level socket() call failed
   // -2 = low level bind() call failed
   // -3 = low level listen() call failed
   // Every valid server socket is added to the TROOT sockets list which
   // will make sure that any open sockets are properly closed on
   // program termination.

   fTcpWindowSize = tcpwindowsize;
   SetName("PServerSocket");
}

//______________________________________________________________________________
TPServerSocket::TPServerSocket(const char *service, Bool_t reuse, Int_t backlog,
                               Int_t tcpwindowsize) :
   TServerSocket(service, reuse, backlog, tcpwindowsize)
{
   // Create a parallel server socket object for a named service. Set reuse
   // to true to force reuse of the server socket (i.e. do not wait for the
   // time out to pass). Using backlog one can set the desirable queue length
   // for pending connections.
   // Use tcpwindowsize to specify the size of the receive buffer, it has
   // to be specified here to make sure the window scale option is set (for
   // tcpwindowsize > 65KB and for platforms supporting window scaling).
   // Use IsValid() to check the validity of the
   // server socket. In case server socket is not valid use GetErrorCode()
   // to obtain the specific error value. These values are:
   //  0 = no error (socket is valid)
   // -1 = low level socket() call failed
   // -2 = low level bind() call failed
   // -3 = low level listen() call failed
   // Every valid server socket is added to the TROOT sockets list which
   // will make sure that any open sockets are properly closed on
   // program termination.

   fTcpWindowSize = tcpwindowsize;
   SetName("PServerSocket");
}

//______________________________________________________________________________
TSocket *TPServerSocket::Accept(UChar_t Opt)
{
   // Accept a connection on a parallel server socket. Returns a full-duplex
   // parallel communication TPSocket object. If no pending connections are
   // present on the queue and nonblocking mode has not been enabled
   // with SetOption(kNoBlock,1) the call blocks until a connection is
   // present. The returned socket must be deleted by the user. The socket
   // is also added to the TROOT sockets list which will make sure that
   // any open sockets are properly closed on program termination.
   // In case of error 0 is returned and in case non-blocking I/O is
   // enabled and no connections are available -1 is returned.

   TSocket  *setupSocket = 0;
   TSocket  **pSockets;
   TPSocket *newPSocket = 0;

   Int_t size, port;

   // wait for the incoming connections to the server and accept them
   setupSocket = TServerSocket::Accept(Opt);

   if (setupSocket == 0) return 0;

   // receive the port number and number of parallel sockets from the
   // client and establish 'n' connections
   if (setupSocket->Recv(port, size) < 0) {
      Error("Accept", "error receiving port number and number of sockets");
      return 0;
   }

   // Check if client is running in single mode
   if (size == 0) {
      pSockets = new TSocket*[1];

      pSockets[0] = setupSocket;

      // create TPSocket object with the original socket
      newPSocket = new TPSocket(pSockets, 1);

   } else {
      pSockets = new TSocket*[size];

      for (int i = 0; i < size; i++) {
         pSockets[i] = new TSocket(setupSocket->GetInetAddress(),
                                           port, fTcpWindowSize);
         R__LOCKGUARD2(gROOTMutex);
         gROOT->GetListOfSockets()->Remove(pSockets[i]);
      }

      // create TPSocket object with all the accepted sockets
      newPSocket = new TPSocket(pSockets, size);

   }

   // Transmit authentication information, if any
   if (setupSocket->IsAuthenticated())
      newPSocket->SetSecContext(setupSocket->GetSecContext());

   // clean up, if needed
   if (size > 0)
      delete setupSocket;

   // return the TSocket object
   return newPSocket;
}

<|endoftext|>


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

#include "src/traced/probes/probes_producer.h"

#include <stdio.h>
#include <sys/stat.h>
#include <queue>
#include <string>

#include "perfetto/base/logging.h"
#include "perfetto/base/weak_ptr.h"
#include "perfetto/traced/traced.h"
#include "perfetto/tracing/core/data_source_config.h"
#include "perfetto/tracing/core/data_source_descriptor.h"
#include "perfetto/tracing/core/ftrace_config.h"
#include "perfetto/tracing/core/trace_config.h"
#include "perfetto/tracing/core/trace_packet.h"
#include "src/traced/probes/filesystem/inode_file_data_source.h"

#include "perfetto/trace/filesystem/inode_file_map.pbzero.h"
#include "perfetto/trace/ftrace/ftrace_event_bundle.pbzero.h"
#include "perfetto/trace/trace_packet.pbzero.h"

namespace perfetto {
namespace {

uint64_t kInitialConnectionBackoffMs = 100;
uint64_t kMaxConnectionBackoffMs = 30 * 1000;
constexpr char kFtraceSourceName[] = "com.google.perfetto.ftrace";
constexpr char kProcessStatsSourceName[] = "com.google.perfetto.process_stats";
constexpr char kInodeMapSourceName[] = "com.google.perfetto.inode_file_map";

}  // namespace.

// State transition diagram:
//                    +----------------------------+
//                    v                            +
// NotStarted -> NotConnected -> Connecting -> Connected
//                    ^              +
//                    +--------------+
//

ProbesProducer::ProbesProducer() {}
ProbesProducer::~ProbesProducer() = default;

void ProbesProducer::OnConnect() {
  PERFETTO_DCHECK(state_ == kConnecting);
  state_ = kConnected;
  ResetConnectionBackoff();
  PERFETTO_LOG("Connected to the service");

  DataSourceDescriptor ftrace_descriptor;
  ftrace_descriptor.set_name(kFtraceSourceName);
  endpoint_->RegisterDataSource(ftrace_descriptor, [](DataSourceInstanceID) {});

  DataSourceDescriptor process_stats_descriptor;
  process_stats_descriptor.set_name(kProcessStatsSourceName);
  endpoint_->RegisterDataSource(process_stats_descriptor,
                                [](DataSourceInstanceID) {});

  DataSourceDescriptor inode_map_descriptor;
  inode_map_descriptor.set_name(kInodeMapSourceName);
  endpoint_->RegisterDataSource(inode_map_descriptor,
                                [](DataSourceInstanceID) {});
}

void ProbesProducer::OnDisconnect() {
  PERFETTO_DCHECK(state_ == kConnected || state_ == kConnecting);
  state_ = kNotConnected;
  PERFETTO_LOG("Disconnected from tracing service");
  IncreaseConnectionBackoff();

  // TODO(hjd): Erase all sinks and add e2e test for this.
  task_runner_->PostDelayedTask([this] { this->Connect(); },
                                connection_backoff_ms_);
}

void ProbesProducer::CreateDataSourceInstance(DataSourceInstanceID instance_id,
                                              const DataSourceConfig& config) {
  // TODO(hjd): This a hack since we don't actually know the session id. For
  // now we'll assume anything wit hthe same target buffer is in the same
  // session.
  TracingSessionID session_id = config.target_buffer();

  if (config.name() == kFtraceSourceName) {
    if (!CreateFtraceDataSourceInstance(session_id, instance_id, config))
      failed_sources_.insert(instance_id);
  } else if (config.name() == kInodeMapSourceName) {
    CreateInodeFileDataSourceInstance(session_id, instance_id, config);
  } else if (config.name() == kProcessStatsSourceName) {
    CreateProcessStatsDataSourceInstance(session_id, instance_id, config);
  } else {
    PERFETTO_ELOG("Data source name: %s not recognised.",
                  config.name().c_str());
    return;
  }

  std::map<TracingSessionID, InodeFileDataSource*> file_sources;
  std::map<TracingSessionID, ProcessStatsDataSource*> ps_sources;
  for (const auto& pair : file_map_sources_)
    file_sources[pair.second->session_id()] = pair.second.get();
  for (const auto& pair : process_stats_sources_)
    ps_sources[pair.second->session_id()] = pair.second.get();

  for (const auto& id_to_source : delegates_) {
    const std::unique_ptr<SinkDelegate>& source = id_to_source.second;
    if (session_id != source->session_id())
      continue;
    if (!source->ps_source() && ps_sources.count(session_id))
      source->set_ps_source(ps_sources[session_id]->GetWeakPtr());
    if (!source->file_source() && file_sources.count(session_id))
      source->set_file_source(file_sources[session_id]->GetWeakPtr());
  }
}

void ProbesProducer::AddWatchdogsTimer(DataSourceInstanceID id,
                                       const DataSourceConfig& config) {
  if (config.trace_duration_ms() != 0)
    watchdogs_.emplace(id, base::Watchdog::GetInstance()->CreateFatalTimer(
                               5000 + 2 * config.trace_duration_ms()));
}

bool ProbesProducer::CreateFtraceDataSourceInstance(
    TracingSessionID session_id,
    DataSourceInstanceID id,
    const DataSourceConfig& config) {
  // Don't retry if FtraceController::Create() failed once.
  // This can legitimately happen on user builds where we cannot access the
  // debug paths, e.g., because of SELinux rules.
  if (ftrace_creation_failed_)
    return false;

  // Lazily create on the first instance.
  if (!ftrace_) {
    ftrace_ = FtraceController::Create(task_runner_);

    if (!ftrace_) {
      PERFETTO_ELOG("Failed to create FtraceController");
      ftrace_creation_failed_ = true;
      return false;
    }

    ftrace_->DisableAllEvents();
    ftrace_->ClearTrace();
  }

  PERFETTO_LOG("Ftrace start (id=%" PRIu64 ", target_buf=%" PRIu32 ")", id,
               config.target_buffer());

  FtraceConfig proto_config = config.ftrace_config();

  // TODO(hjd): Static cast is bad, target_buffer() should return a BufferID.
  auto trace_writer = endpoint_->CreateTraceWriter(
      static_cast<BufferID>(config.target_buffer()));
  auto delegate = std::unique_ptr<SinkDelegate>(
      new SinkDelegate(session_id, task_runner_, std::move(trace_writer)));
  auto sink = ftrace_->CreateSink(std::move(proto_config), delegate.get());
  if (!sink) {
    PERFETTO_ELOG("Failed to start tracing (maybe someone else is using it?)");
    return false;
  }
  delegate->set_sink(std::move(sink));
  delegates_.emplace(id, std::move(delegate));
  AddWatchdogsTimer(id, config);
  return true;
}

void ProbesProducer::CreateInodeFileDataSourceInstance(
    TracingSessionID session_id,
    DataSourceInstanceID id,
    const DataSourceConfig& source_config) {
  PERFETTO_LOG("Inode file map start (id=%" PRIu64 ", target_buf=%" PRIu32 ")",
               id, source_config.target_buffer());
  auto trace_writer = endpoint_->CreateTraceWriter(
      static_cast<BufferID>(source_config.target_buffer()));
  if (system_inodes_.empty())
    CreateStaticDeviceToInodeMap("/system/", &system_inodes_);
  auto file_map_source =
      std::unique_ptr<InodeFileDataSource>(new InodeFileDataSource(
          session_id, &system_inodes_, &cache_, std::move(trace_writer)));
  file_map_sources_.emplace(id, std::move(file_map_source));
  AddWatchdogsTimer(id, source_config);
}

void ProbesProducer::CreateProcessStatsDataSourceInstance(
    TracingSessionID session_id,
    DataSourceInstanceID id,
    const DataSourceConfig& config) {
  PERFETTO_DCHECK(process_stats_sources_.count(id) == 0);
  auto trace_writer = endpoint_->CreateTraceWriter(
      static_cast<BufferID>(config.target_buffer()));
  auto source = std::unique_ptr<ProcessStatsDataSource>(
      new ProcessStatsDataSource(session_id, std::move(trace_writer)));
  auto it_and_inserted = process_stats_sources_.emplace(id, std::move(source));
  PERFETTO_DCHECK(it_and_inserted.second);
  it_and_inserted.first->second->WriteAllProcesses();
}

void ProbesProducer::TearDownDataSourceInstance(DataSourceInstanceID id) {
  PERFETTO_LOG("Producer stop (id=%" PRIu64 ")", id);
  // |id| could be the id of any of the datasources we handle:
  PERFETTO_DCHECK((failed_sources_.count(id) +
                   process_stats_sources_.count(id) +
                   file_map_sources_.count(id)) == 1);
  failed_sources_.erase(id);
  delegates_.erase(id);
  process_stats_sources_.erase(id);
  file_map_sources_.erase(id);
  watchdogs_.erase(id);
}

void ProbesProducer::OnTracingStart() {}
void ProbesProducer::OnTracingStop() {}

void ProbesProducer::ConnectWithRetries(const char* socket_name,
                                        base::TaskRunner* task_runner) {
  PERFETTO_DCHECK(state_ == kNotStarted);
  state_ = kNotConnected;

  ResetConnectionBackoff();
  socket_name_ = socket_name;
  task_runner_ = task_runner;
  Connect();
}

void ProbesProducer::Connect() {
  PERFETTO_DCHECK(state_ == kNotConnected);
  state_ = kConnecting;
  endpoint_ = ProducerIPCClient::Connect(socket_name_, this, task_runner_);
}

void ProbesProducer::IncreaseConnectionBackoff() {
  connection_backoff_ms_ *= 2;
  if (connection_backoff_ms_ > kMaxConnectionBackoffMs)
    connection_backoff_ms_ = kMaxConnectionBackoffMs;
}

void ProbesProducer::ResetConnectionBackoff() {
  connection_backoff_ms_ = kInitialConnectionBackoffMs;
}

ProbesProducer::SinkDelegate::SinkDelegate(TracingSessionID id,
                                           base::TaskRunner* task_runner,
                                           std::unique_ptr<TraceWriter> writer)
    : session_id_(id),
      task_runner_(task_runner),
      writer_(std::move(writer)),
      weak_factory_(this) {}

ProbesProducer::SinkDelegate::~SinkDelegate() = default;

ProbesProducer::FtraceBundleHandle
ProbesProducer::SinkDelegate::GetBundleForCpu(size_t) {
  trace_packet_ = writer_->NewTracePacket();
  return FtraceBundleHandle(trace_packet_->set_ftrace_events());
}

void ProbesProducer::SinkDelegate::OnBundleComplete(
    size_t,
    FtraceBundleHandle,
    const FtraceMetadata& metadata) {
  trace_packet_->Finalize();

  if (file_source_ && !metadata.inode_and_device.empty()) {
    auto inodes = metadata.inode_and_device;
    auto weak_file_source = file_source_;
    task_runner_->PostTask([weak_file_source, inodes] {
      if (weak_file_source)
        weak_file_source->OnInodes(inodes);
    });
  }
}

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

#include "src/traced/probes/probes_producer.h"

#include <stdio.h>
#include <sys/stat.h>
#include <queue>
#include <string>

#include "perfetto/base/logging.h"
#include "perfetto/base/weak_ptr.h"
#include "perfetto/traced/traced.h"
#include "perfetto/tracing/core/data_source_config.h"
#include "perfetto/tracing/core/data_source_descriptor.h"
#include "perfetto/tracing/core/ftrace_config.h"
#include "perfetto/tracing/core/trace_config.h"
#include "perfetto/tracing/core/trace_packet.h"
#include "src/traced/probes/filesystem/inode_file_data_source.h"

#include "perfetto/trace/filesystem/inode_file_map.pbzero.h"
#include "perfetto/trace/ftrace/ftrace_event_bundle.pbzero.h"
#include "perfetto/trace/trace_packet.pbzero.h"

namespace perfetto {
namespace {

uint64_t kInitialConnectionBackoffMs = 100;
uint64_t kMaxConnectionBackoffMs = 30 * 1000;
constexpr char kFtraceSourceName[] = "com.google.perfetto.ftrace";
constexpr char kProcessStatsSourceName[] = "com.google.perfetto.process_stats";
constexpr char kInodeMapSourceName[] = "com.google.perfetto.inode_file_map";

}  // namespace.

// State transition diagram:
//                    +----------------------------+
//                    v                            +
// NotStarted -> NotConnected -> Connecting -> Connected
//                    ^              +
//                    +--------------+
//

ProbesProducer::ProbesProducer() {}
ProbesProducer::~ProbesProducer() = default;

void ProbesProducer::OnConnect() {
  PERFETTO_DCHECK(state_ == kConnecting);
  state_ = kConnected;
  ResetConnectionBackoff();
  PERFETTO_LOG("Connected to the service");

  DataSourceDescriptor ftrace_descriptor;
  ftrace_descriptor.set_name(kFtraceSourceName);
  endpoint_->RegisterDataSource(ftrace_descriptor, [](DataSourceInstanceID) {});

  DataSourceDescriptor process_stats_descriptor;
  process_stats_descriptor.set_name(kProcessStatsSourceName);
  endpoint_->RegisterDataSource(process_stats_descriptor,
                                [](DataSourceInstanceID) {});

  DataSourceDescriptor inode_map_descriptor;
  inode_map_descriptor.set_name(kInodeMapSourceName);
  endpoint_->RegisterDataSource(inode_map_descriptor,
                                [](DataSourceInstanceID) {});
}

void ProbesProducer::OnDisconnect() {
  PERFETTO_DCHECK(state_ == kConnected || state_ == kConnecting);
  state_ = kNotConnected;
  PERFETTO_LOG("Disconnected from tracing service");
  IncreaseConnectionBackoff();

  // TODO(hjd): Erase all sinks and add e2e test for this.
  task_runner_->PostDelayedTask([this] { this->Connect(); },
                                connection_backoff_ms_);
}

void ProbesProducer::CreateDataSourceInstance(DataSourceInstanceID instance_id,
                                              const DataSourceConfig& config) {
  // TODO(hjd): This a hack since we don't actually know the session id. For
  // now we'll assume anything wit hthe same target buffer is in the same
  // session.
  TracingSessionID session_id = config.target_buffer();

  if (config.name() == kFtraceSourceName) {
    if (!CreateFtraceDataSourceInstance(session_id, instance_id, config))
      failed_sources_.insert(instance_id);
  } else if (config.name() == kInodeMapSourceName) {
    CreateInodeFileDataSourceInstance(session_id, instance_id, config);
  } else if (config.name() == kProcessStatsSourceName) {
    CreateProcessStatsDataSourceInstance(session_id, instance_id, config);
  } else {
    PERFETTO_ELOG("Data source name: %s not recognised.",
                  config.name().c_str());
    return;
  }

  std::map<TracingSessionID, InodeFileDataSource*> file_sources;
  std::map<TracingSessionID, ProcessStatsDataSource*> ps_sources;
  for (const auto& pair : file_map_sources_)
    file_sources[pair.second->session_id()] = pair.second.get();
  for (const auto& pair : process_stats_sources_)
    ps_sources[pair.second->session_id()] = pair.second.get();

  for (const auto& id_to_source : delegates_) {
    const std::unique_ptr<SinkDelegate>& source = id_to_source.second;
    if (session_id != source->session_id())
      continue;
    if (!source->ps_source() && ps_sources.count(session_id))
      source->set_ps_source(ps_sources[session_id]->GetWeakPtr());
    if (!source->file_source() && file_sources.count(session_id))
      source->set_file_source(file_sources[session_id]->GetWeakPtr());
  }
}

void ProbesProducer::AddWatchdogsTimer(DataSourceInstanceID id,
                                       const DataSourceConfig& config) {
  if (config.trace_duration_ms() != 0)
    watchdogs_.emplace(id, base::Watchdog::GetInstance()->CreateFatalTimer(
                               5000 + 2 * config.trace_duration_ms()));
}

bool ProbesProducer::CreateFtraceDataSourceInstance(
    TracingSessionID session_id,
    DataSourceInstanceID id,
    const DataSourceConfig& config) {
  // Don't retry if FtraceController::Create() failed once.
  // This can legitimately happen on user builds where we cannot access the
  // debug paths, e.g., because of SELinux rules.
  if (ftrace_creation_failed_)
    return false;

  // Lazily create on the first instance.
  if (!ftrace_) {
    ftrace_ = FtraceController::Create(task_runner_);

    if (!ftrace_) {
      PERFETTO_ELOG("Failed to create FtraceController");
      ftrace_creation_failed_ = true;
      return false;
    }

    ftrace_->DisableAllEvents();
    ftrace_->ClearTrace();
  }

  PERFETTO_LOG("Ftrace start (id=%" PRIu64 ", target_buf=%" PRIu32 ")", id,
               config.target_buffer());

  FtraceConfig proto_config = config.ftrace_config();

  // TODO(hjd): Static cast is bad, target_buffer() should return a BufferID.
  auto trace_writer = endpoint_->CreateTraceWriter(
      static_cast<BufferID>(config.target_buffer()));
  auto delegate = std::unique_ptr<SinkDelegate>(
      new SinkDelegate(session_id, task_runner_, std::move(trace_writer)));
  auto sink = ftrace_->CreateSink(std::move(proto_config), delegate.get());
  if (!sink) {
    PERFETTO_ELOG("Failed to start tracing (maybe someone else is using it?)");
    return false;
  }
  delegate->set_sink(std::move(sink));
  delegates_.emplace(id, std::move(delegate));
  AddWatchdogsTimer(id, config);
  return true;
}

void ProbesProducer::CreateInodeFileDataSourceInstance(
    TracingSessionID session_id,
    DataSourceInstanceID id,
    const DataSourceConfig& source_config) {
  PERFETTO_LOG("Inode file map start (id=%" PRIu64 ", target_buf=%" PRIu32 ")",
               id, source_config.target_buffer());
  auto trace_writer = endpoint_->CreateTraceWriter(
      static_cast<BufferID>(source_config.target_buffer()));
  if (system_inodes_.empty())
    CreateStaticDeviceToInodeMap("/system/", &system_inodes_);
  auto file_map_source =
      std::unique_ptr<InodeFileDataSource>(new InodeFileDataSource(
          session_id, &system_inodes_, &cache_, std::move(trace_writer)));
  file_map_sources_.emplace(id, std::move(file_map_source));
  AddWatchdogsTimer(id, source_config);
}

void ProbesProducer::CreateProcessStatsDataSourceInstance(
    TracingSessionID session_id,
    DataSourceInstanceID id,
    const DataSourceConfig& config) {
  PERFETTO_DCHECK(process_stats_sources_.count(id) == 0);
  auto trace_writer = endpoint_->CreateTraceWriter(
      static_cast<BufferID>(config.target_buffer()));
  auto source = std::unique_ptr<ProcessStatsDataSource>(
      new ProcessStatsDataSource(session_id, std::move(trace_writer)));
  auto it_and_inserted = process_stats_sources_.emplace(id, std::move(source));
  PERFETTO_DCHECK(it_and_inserted.second);
  it_and_inserted.first->second->WriteAllProcesses();
}

void ProbesProducer::TearDownDataSourceInstance(DataSourceInstanceID id) {
  PERFETTO_LOG("Producer stop (id=%" PRIu64 ")", id);
  // |id| could be the id of any of the datasources we handle:
  PERFETTO_DCHECK((failed_sources_.count(id) + delegates_.count(id) +
                   process_stats_sources_.count(id) +
                   file_map_sources_.count(id)) == 1);
  failed_sources_.erase(id);
  delegates_.erase(id);
  process_stats_sources_.erase(id);
  file_map_sources_.erase(id);
  watchdogs_.erase(id);
}

void ProbesProducer::OnTracingStart() {}
void ProbesProducer::OnTracingStop() {}

void ProbesProducer::ConnectWithRetries(const char* socket_name,
                                        base::TaskRunner* task_runner) {
  PERFETTO_DCHECK(state_ == kNotStarted);
  state_ = kNotConnected;

  ResetConnectionBackoff();
  socket_name_ = socket_name;
  task_runner_ = task_runner;
  Connect();
}

void ProbesProducer::Connect() {
  PERFETTO_DCHECK(state_ == kNotConnected);
  state_ = kConnecting;
  endpoint_ = ProducerIPCClient::Connect(socket_name_, this, task_runner_);
}

void ProbesProducer::IncreaseConnectionBackoff() {
  connection_backoff_ms_ *= 2;
  if (connection_backoff_ms_ > kMaxConnectionBackoffMs)
    connection_backoff_ms_ = kMaxConnectionBackoffMs;
}

void ProbesProducer::ResetConnectionBackoff() {
  connection_backoff_ms_ = kInitialConnectionBackoffMs;
}

ProbesProducer::SinkDelegate::SinkDelegate(TracingSessionID id,
                                           base::TaskRunner* task_runner,
                                           std::unique_ptr<TraceWriter> writer)
    : session_id_(id),
      task_runner_(task_runner),
      writer_(std::move(writer)),
      weak_factory_(this) {}

ProbesProducer::SinkDelegate::~SinkDelegate() = default;

ProbesProducer::FtraceBundleHandle
ProbesProducer::SinkDelegate::GetBundleForCpu(size_t) {
  trace_packet_ = writer_->NewTracePacket();
  return FtraceBundleHandle(trace_packet_->set_ftrace_events());
}

void ProbesProducer::SinkDelegate::OnBundleComplete(
    size_t,
    FtraceBundleHandle,
    const FtraceMetadata& metadata) {
  trace_packet_->Finalize();

  if (file_source_ && !metadata.inode_and_device.empty()) {
    auto inodes = metadata.inode_and_device;
    auto weak_file_source = file_source_;
    task_runner_->PostTask([weak_file_source, inodes] {
      if (weak_file_source)
        weak_file_source->OnInodes(inodes);
    });
  }
}

}  // namespace perfetto
<|endoftext|>


<commit_before>#include "rosplan_planning_system/PlannerInterface/PlannerInterface.h"

namespace KCL_rosplan {

	/*----------------------*/
	/* Problem subscription */
	/*----------------------*/

	void PlannerInterface::problemCallback(const std_msgs::String& problemInstance) {
		ROS_INFO("KCL: (%s) Problem received.", ros::this_node::getName().c_str());
		problem_instance_received = true;
		problem_instance_time = ros::WallTime::now().toSec();
		problem_instance = problemInstance.data;
	}

	/*--------------------*/
	/* Planning interface */
	/*--------------------*/

	/**
	 * planning system service method (1)
	 * loads parameters from param server
	 */
	bool PlannerInterface::runPlanningServerDefault(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {

		// defaults
		use_problem_topic = false;
		data_path = "common/";
		domain_path = "common/domain.pddl";
		problem_path = "common/problem.pddl";
		planner_command = "timeout 60 common/bin/popf -n DOMAIN PROBLEM";

		// load params
		node_handle->getParam("use_problem_topic", use_problem_topic);
		node_handle->getParam("domain_path", domain_path);
		node_handle->getParam("data_path", data_path);
		node_handle->getParam("problem_path", problem_path);
		node_handle->getParam("planner_command", planner_command);

		// call planning server
		runPlanningServer(domain_path, problem_path, data_path, planner_command, use_problem_topic);
		return true;
	}

	/**
	 * planning system service method (2)
	 * loads parameters from service request
	 */
	bool PlannerInterface::runPlanningServerParams(rosplan_dispatch_msgs::PlanningService::Request &req, rosplan_dispatch_msgs::PlanningService::Response &res) {
		// call planning server
		res.plan_found = runPlanningServer(req.domain_path, req.problem_path, req.data_path, req.planner_command, req.use_problem_topic);
		return true;
	}

	/**
	 * planning system service method (3)
	 * loads parameters from actionlib goal
	 */
	void PlannerInterface::runPlanningServerAction(const rosplan_dispatch_msgs::PlanGoalConstPtr& goal) {
		// call planning server
		if(runPlanningServer(goal->domain_path, goal->problem_path, goal->data_path, goal->planner_command, goal->use_problem_topic)) {
			plan_server->setSucceeded();
		} else {
			plan_server->setAborted();
		}
	}

	/**
	 * planning system; prepares planning; calls planner; parses plan.
	 */
	bool PlannerInterface::runPlanningServer(std::string domainPath, std::string problemPath, std::string dataPath, std::string plannerCommand, bool useProblemTopic) {

		// save parameters
		data_path = dataPath;
		domain_path = domainPath;
		problem_path = problemPath;
		planner_command = plannerCommand;
		use_problem_topic = useProblemTopic;

        // check if data_path ends in "/" and add "/" if not
        const char *last_char = &data_path.back();
        if (strcmp(last_char,"/") != 0)data_path = data_path + "/";

		// set problem name for ROS_INFO
		std::size_t lastDivide = problem_path.find_last_of("/\\");
		if(lastDivide != std::string::npos) {
			problem_name = problem_path.substr(lastDivide+1);
		} else {
			problem_name = problem_path;
		}

		if(use_problem_topic && !problem_instance_received) {
			ROS_INFO("KCL: (%s) (%s) Problem was not published yet.", ros::this_node::getName().c_str(), problem_name.c_str());
			return false;
		}

		bool success = runPlanner();

		// publish planner output
		if(success) {
			std_msgs::String planMsg;
			planMsg.data = planner_output;
			plan_publisher.publish(planMsg);
		}

		return success;
	}

} // close namespace
<commit_msg>added planner interface return value until we can get a proper service in place<commit_after>#include "rosplan_planning_system/PlannerInterface/PlannerInterface.h"

namespace KCL_rosplan {

	/*----------------------*/
	/* Problem subscription */
	/*----------------------*/

	void PlannerInterface::problemCallback(const std_msgs::String& problemInstance) {
		ROS_INFO("KCL: (%s) Problem received.", ros::this_node::getName().c_str());
		problem_instance_received = true;
		problem_instance_time = ros::WallTime::now().toSec();
		problem_instance = problemInstance.data;
	}

	/*--------------------*/
	/* Planning interface */
	/*--------------------*/

	/**
	 * planning system service method (1)
	 * loads parameters from param server
	 */
	bool PlannerInterface::runPlanningServerDefault(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {

		// defaults
		use_problem_topic = false;
		data_path = "common/";
		domain_path = "common/domain.pddl";
		problem_path = "common/problem.pddl";
		planner_command = "timeout 60 common/bin/popf -n DOMAIN PROBLEM";

		// load params
		node_handle->getParam("use_problem_topic", use_problem_topic);
		node_handle->getParam("domain_path", domain_path);
		node_handle->getParam("data_path", data_path);
		node_handle->getParam("problem_path", problem_path);
		node_handle->getParam("planner_command", planner_command);

		// call planning server
		return runPlanningServer(domain_path, problem_path, data_path, planner_command, use_problem_topic);
	}

	/**
	 * planning system service method (2)
	 * loads parameters from service request
	 */
	bool PlannerInterface::runPlanningServerParams(rosplan_dispatch_msgs::PlanningService::Request &req, rosplan_dispatch_msgs::PlanningService::Response &res) {
		// call planning server
		res.plan_found = runPlanningServer(req.domain_path, req.problem_path, req.data_path, req.planner_command, req.use_problem_topic);
		return true;
	}

	/**
	 * planning system service method (3)
	 * loads parameters from actionlib goal
	 */
	void PlannerInterface::runPlanningServerAction(const rosplan_dispatch_msgs::PlanGoalConstPtr& goal) {
		// call planning server
		if(runPlanningServer(goal->domain_path, goal->problem_path, goal->data_path, goal->planner_command, goal->use_problem_topic)) {
			plan_server->setSucceeded();
		} else {
			plan_server->setAborted();
		}
	}

	/**
	 * planning system; prepares planning; calls planner; parses plan.
	 */
	bool PlannerInterface::runPlanningServer(std::string domainPath, std::string problemPath, std::string dataPath, std::string plannerCommand, bool useProblemTopic) {

		// save parameters
		data_path = dataPath;
		domain_path = domainPath;
		problem_path = problemPath;
		planner_command = plannerCommand;
		use_problem_topic = useProblemTopic;

        // check if data_path ends in "/" and add "/" if not
        const char *last_char = &data_path.back();
        if (strcmp(last_char,"/") != 0)data_path = data_path + "/";

		// set problem name for ROS_INFO
		std::size_t lastDivide = problem_path.find_last_of("/\\");
		if(lastDivide != std::string::npos) {
			problem_name = problem_path.substr(lastDivide+1);
		} else {
			problem_name = problem_path;
		}

		if(use_problem_topic && !problem_instance_received) {
			ROS_INFO("KCL: (%s) (%s) Problem was not published yet.", ros::this_node::getName().c_str(), problem_name.c_str());
			return false;
		}

		bool success = runPlanner();

		// publish planner output
		if(success) {
			std_msgs::String planMsg;
			planMsg.data = planner_output;
			plan_publisher.publish(planMsg);
		}

		return success;
	}

} // close namespace
<|endoftext|>


<commit_before>#pragma once

#include <iterator>

#include <cstdlib> //malloc, realloc
#include <cstring> //memcpy, memmove
#include <exception> //bad_alloc
#include <type_traits>
#include <utility>

namespace tyti {
template<typename T>
class any_iterator : std::iterator<std::bidirectional_iterator_tag, T>
{
    //functionpointer save structure
    struct TypeInfos
    {
        void(*const inc_fn)(void*);
        void(*const dec_fn)(void*);
        const bool(*const equal_fn)(const void*,const void*);
        const T*(*const deref_fn)(const void*);
        void(*const dtor_fn)(void*);
        void(*const copy_ctor_fn)(void*,const void*);
        void(*const move_ctor_fn)(void*, const void*);
        const size_t size;
    };

    template<typename IterType>
    TypeInfos* getFunctionInfos()
    {
#if !defined(_MSC_VER) || (_MSC_VER > 1600) // skip vs 2010. It doesnt implement "is_copy_constructible"
        static_assert(std::is_copy_constructible<IterType>::value, "any_iterator requires a copy constructable type");
        static_assert(std::is_destructible<IterType>::value, "any_iterator requires a destructable type");
#endif
        static TypeInfos ti =
        {
            &any_iterator::inc<IterType>,
            &any_iterator::dec<IterType>,
            &any_iterator::equal<IterType>,
            &any_iterator::deref<IterType>,
            //(std::is_trivially_destructible<IterType>::value) ?
            //static_cast<void(*)(void*)>(nullptr) :
            &any_iterator::dtor<IterType>,
            //(std::is_trivially_copy_constructible<IterType>::value) ?
            //static_cast<void(*)(void*,const void*)>(nullptr) :
            &any_iterator::copyConstructor<IterType>,
            &any_iterator::moveConstructor<IterType>,
            sizeof(IterType)
        };
        return &ti;
    }

    //access functions
    template<typename Iter>
    static void inc(void* _ptr)
    {
        ++(*reinterpret_cast<Iter*>(_ptr));
    }

    template<typename Iter>
    static void dec(void* _ptr)
    {
        --(*reinterpret_cast<Iter*>(_ptr));
    }

    template<typename Iter>
    static const T* deref(const void* _ptr)
    {
        return &(*(*reinterpret_cast<const Iter*>(_ptr)));
    }

    template<typename Iter>
    static const bool equal(const void* _lhs, const void* _rhs)
    {
        return *reinterpret_cast<const Iter*>(_lhs) == *reinterpret_cast<const Iter*>(_rhs);
    }
    template<typename Iter>
    static void dtor(void* _ptr)
    {
        reinterpret_cast<Iter*>(_ptr)->~Iter();
    }
    template<typename Iter>
    static void copyConstructor(void* _dst, const void* _src)
    {
        new (_dst) Iter(*reinterpret_cast<const Iter*>(_src));
    }
    template<typename Iter>
    static void moveConstructor(void* _dst, const void* _src)
    {
        new (_dst) Iter(std::move(*reinterpret_cast<const Iter*>(_src)));
    }

    // helper functions
    inline void destruct()
    {
        ti_->dtor_fn(ptr_);
    }

    void switch_type(const TypeInfos* _newType)
    {
        destruct();
        if (ti_->size < _newType->size)
        {
            ptr_ = std::realloc(ptr_, _newType->size);
            if (!ptr_)
                throw std::bad_alloc();
        }
        ti_ = _newType;
    }

    inline void copy_and_assign(const void* _src)
    {
        ti_->copy_ctor_fn(ptr_, _src);
    }

    inline void move_iter(const void* _src)
    {
        ti_->move_ctor_fn(ptr_, _src);
    }

    // member variables
    void* ptr_;
    const TypeInfos* ti_;

    /// Interface
public:
    template <typename IterType, class = typename std::enable_if<!std::is_rvalue_reference<IterType>::value>::type>
    explicit any_iterator(const IterType& _iter)
        : ptr_(std::malloc(sizeof(IterType))), ti_(getFunctionInfos<IterType>())
    {
        if (!ptr_)
            throw std::bad_alloc();
        copy_and_assign(&_iter);
    }

    template<typename IterType, class = typename std::enable_if<std::is_rvalue_reference<IterType>::value>::type>
        explicit any_iterator(IterType&& _iter)
        : ptr_(std::malloc(_iter.ti_->size)), ti_(std::move(_iter.ti_))
    {
        if (!ptr_)
            throw std::bad_alloc();
        move_iter(_iter.ptr_);
    }

    any_iterator(const any_iterator& _iter)
        : ptr_(std::malloc(_iter.ti_->size)), ti_(_iter.ti_)
    {
        if (!ptr_)
            throw std::bad_alloc();
        copy_and_assign(_iter.ptr_);
    }

    any_iterator(any_iterator&& _iter)
        : ptr_(_iter.ptr), ti_(_iter.ti_)
    {
        _iter.ptr_ = nullptr;
        _iter.ti_ = getFunctionInfos<int>();//dont call dtor
    }

    template <typename IterType, class = typename std::enable_if<!std::is_rvalue_reference<IterType>::value>::type >
    const any_iterator& operator=(const IterType& _iter)
    {
        switch_type(getFunctionInfos<IterType>());
        copy_and_assign(&_iter);
        return *this;
    }

    template <typename IterType, class = typename std::enable_if<std::is_rvalue_reference<IterType>::value>::type>
        const any_iterator& operator=(IterType&& _iter)
    {
        switch_type(getFunctionInfos<IterType>());
        move_iter(&_iter);
        return *this;
    }

    const any_iterator& operator=(const any_iterator& _iter)
    {
        switch_type(_iter.ti_);
        copy_and_assign(_iter.ptr_);
        return *this;
    }

    const any_iterator& operator=(any_iterator&& _iter)
    {
        ti_ = _iter.ti_;
        ptr_ = _iter.ptr_;
        _iter.ti_ = getFunctionInfos<int>();
        _iter.ptr_ = nullptr;
        return *this;
    }

    ~any_iterator()
    {
        destruct();
        std::free(ptr_);
    }

    bool operator==(const any_iterator& _rhs) const
    {
        if (ti_ != _rhs.ti_) //different types
            return false;
        return ti_->equal_fn(ptr_, _rhs.ptr_);
    }

    template <typename IterType>
    bool operator==(const IterType& _rhs) const {
        return ti_->equal_fn(ptr_, reinterpret_cast<const void*>(&_rhs));
    }

    template <typename IterType>
    bool operator!=(const IterType& _rhs) const{
        return !operator==(_rhs);
    }

    /// Standard pre-increment operator
    any_iterator& operator++() {
        ti_->inc_fn(ptr_);
        return *this;
    }

    /// Standard post-increment operator
    any_iterator operator++(int) {
        any_iterator cpy(*this);
        ti_->inc_fn(ptr_);
        return cpy;
    }

    /// Standard pre-decrement operator
    any_iterator& operator--() {
        ti_->dec_fn(ptr_);
        return *this;
    }

    /// Standard post-decrement operator
    any_iterator operator--(int) {
        any_iterator cpy(*this);
        ti_->dec_fn(ptr_);
        return cpy;
    }

    const T& operator*() const {
        return *(ti_->deref_fn(ptr_));
    }

    /// Standard pointer operator.
    const T* operator->() const {
        return ti_->deref_fn(ptr_);
    }
};

// C++17 todo:
//if msvc >= 2017 or gcc with c++17 support
//#include <any>
//using AnyIterator = AnyIteratorT<std::any>;
//endif

} // end namespace tyti

template<typename IterT, typename T>
bool operator==(const IterT&& _lhs, const tyti::any_iterator<T>&& _rhs)
{
    return _rhs.operator==(std::forward<IterT>(_lhs));
}

template<typename IterT, typename T>
bool operator!=(IterT&& _lhs, tyti::any_iterator<T>&& _rhs)
{
    return _rhs.operator!=(std::forward<IterT>(_lhs));
}<commit_msg>removes some redundant constraint checking since it is already included as a requirement in the iterator_tag<commit_after>#pragma once

#include <iterator>

#include <cstdlib> //malloc, realloc
#include <cstring> //memcpy, memmove
#include <exception> //bad_alloc
#include <type_traits>
#include <utility>

namespace tyti {
template<typename T>
class any_iterator : std::iterator<std::bidirectional_iterator_tag, T>
{
    //functionpointer save structure
    struct TypeInfos
    {
        void(*const inc_fn)(void*);
        void(*const dec_fn)(void*);
        const bool(*const equal_fn)(const void*,const void*);
        const T*(*const deref_fn)(const void*);
        void(*const dtor_fn)(void*);
        void(*const copy_ctor_fn)(void*,const void*);
        void(*const move_ctor_fn)(void*, const void*);
        const size_t size;
    };

    template<typename IterType>
    TypeInfos* getFunctionInfos()
    {
        static TypeInfos ti =
        {
            &any_iterator::inc<IterType>,
            &any_iterator::dec<IterType>,
            &any_iterator::equal<IterType>,
            &any_iterator::deref<IterType>,
            //(std::is_trivially_destructible<IterType>::value) ?
            //static_cast<void(*)(void*)>(nullptr) :
            &any_iterator::dtor<IterType>,
            //(std::is_trivially_copy_constructible<IterType>::value) ?
            //static_cast<void(*)(void*,const void*)>(nullptr) :
            &any_iterator::copyConstructor<IterType>,
            &any_iterator::moveConstructor<IterType>,
            sizeof(IterType)
        };
        return &ti;
    }

    //access functions
    template<typename Iter>
    static void inc(void* _ptr)
    {
        ++(*reinterpret_cast<Iter*>(_ptr));
    }

    template<typename Iter>
    static void dec(void* _ptr)
    {
        --(*reinterpret_cast<Iter*>(_ptr));
    }

    template<typename Iter>
    static const T* deref(const void* _ptr)
    {
        return &(*(*reinterpret_cast<const Iter*>(_ptr)));
    }

    template<typename Iter>
    static const bool equal(const void* _lhs, const void* _rhs)
    {
        return *reinterpret_cast<const Iter*>(_lhs) == *reinterpret_cast<const Iter*>(_rhs);
    }
    template<typename Iter>
    static void dtor(void* _ptr)
    {
        reinterpret_cast<Iter*>(_ptr)->~Iter();
    }
    template<typename Iter>
    static void copyConstructor(void* _dst, const void* _src)
    {
        new (_dst) Iter(*reinterpret_cast<const Iter*>(_src));
    }
    template<typename Iter>
    static void moveConstructor(void* _dst, const void* _src)
    {
        new (_dst) Iter(std::move(*reinterpret_cast<const Iter*>(_src)));
    }

    // helper functions
    inline void destruct()
    {
        ti_->dtor_fn(ptr_);
    }

    void switch_type(const TypeInfos* _newType)
    {
        destruct();
        if (ti_->size < _newType->size)
        {
            ptr_ = std::realloc(ptr_, _newType->size);
            if (!ptr_)
                throw std::bad_alloc();
        }
        ti_ = _newType;
    }

    inline void copy_and_assign(const void* _src)
    {
        ti_->copy_ctor_fn(ptr_, _src);
    }

    inline void move_iter(const void* _src)
    {
        ti_->move_ctor_fn(ptr_, _src);
    }

    // member variables
    void* ptr_;
    const TypeInfos* ti_;

    /// Interface
public:
    template <typename IterType, class = typename std::enable_if<!std::is_rvalue_reference<IterType>::value>::type>
    explicit any_iterator(const IterType& _iter)
        : ptr_(std::malloc(sizeof(IterType))), ti_(getFunctionInfos<IterType>())
    {
        if (!ptr_)
            throw std::bad_alloc();
        copy_and_assign(&_iter);
    }

    template<typename IterType, class = typename std::enable_if<std::is_rvalue_reference<IterType>::value>::type>
        explicit any_iterator(IterType&& _iter)
        : ptr_(std::malloc(_iter.ti_->size)), ti_(std::move(_iter.ti_))
    {
        if (!ptr_)
            throw std::bad_alloc();
        move_iter(_iter.ptr_);
    }

    any_iterator(const any_iterator& _iter)
        : ptr_(std::malloc(_iter.ti_->size)), ti_(_iter.ti_)
    {
        if (!ptr_)
            throw std::bad_alloc();
        copy_and_assign(_iter.ptr_);
    }

    any_iterator(any_iterator&& _iter)
        : ptr_(_iter.ptr), ti_(_iter.ti_)
    {
        _iter.ptr_ = nullptr;
        _iter.ti_ = getFunctionInfos<int>();//dont call dtor
    }

    template <typename IterType, class = typename std::enable_if<!std::is_rvalue_reference<IterType>::value>::type >
    const any_iterator& operator=(const IterType& _iter)
    {
        switch_type(getFunctionInfos<IterType>());
        copy_and_assign(&_iter);
        return *this;
    }

    template <typename IterType, class = typename std::enable_if<std::is_rvalue_reference<IterType>::value>::type>
        const any_iterator& operator=(IterType&& _iter)
    {
        switch_type(getFunctionInfos<IterType>());
        move_iter(&_iter);
        return *this;
    }

    const any_iterator& operator=(const any_iterator& _iter)
    {
        switch_type(_iter.ti_);
        copy_and_assign(_iter.ptr_);
        return *this;
    }

    const any_iterator& operator=(any_iterator&& _iter)
    {
        ti_ = _iter.ti_;
        ptr_ = _iter.ptr_;
        _iter.ti_ = getFunctionInfos<int>();
        _iter.ptr_ = nullptr;
        return *this;
    }

    ~any_iterator()
    {
        destruct();
        std::free(ptr_);
    }

    bool operator==(const any_iterator& _rhs) const
    {
        if (ti_ != _rhs.ti_) //different types
            return false;
        return ti_->equal_fn(ptr_, _rhs.ptr_);
    }

    template <typename IterType>
    bool operator==(const IterType& _rhs) const {
        return ti_->equal_fn(ptr_, reinterpret_cast<const void*>(&_rhs));
    }

    template <typename IterType>
    bool operator!=(const IterType& _rhs) const{
        return !operator==(_rhs);
    }

    /// Standard pre-increment operator
    any_iterator& operator++() {
        ti_->inc_fn(ptr_);
        return *this;
    }

    /// Standard post-increment operator
    any_iterator operator++(int) {
        any_iterator cpy(*this);
        ti_->inc_fn(ptr_);
        return cpy;
    }

    /// Standard pre-decrement operator
    any_iterator& operator--() {
        ti_->dec_fn(ptr_);
        return *this;
    }

    /// Standard post-decrement operator
    any_iterator operator--(int) {
        any_iterator cpy(*this);
        ti_->dec_fn(ptr_);
        return cpy;
    }

    const T& operator*() const {
        return *(ti_->deref_fn(ptr_));
    }

    /// Standard pointer operator.
    const T* operator->() const {
        return ti_->deref_fn(ptr_);
    }
};

// C++17 todo:
//if msvc >= 2017 or gcc with c++17 support
//#include <any>
//using AnyIterator = AnyIteratorT<std::any>;
//endif

} // end namespace tyti

template<typename IterT, typename T>
bool operator==(const IterT&& _lhs, const tyti::any_iterator<T>&& _rhs)
{
    return _rhs.operator==(std::forward<IterT>(_lhs));
}

template<typename IterT, typename T>
bool operator!=(IterT&& _lhs, tyti::any_iterator<T>&& _rhs)
{
    return _rhs.operator!=(std::forward<IterT>(_lhs));
}<|endoftext|>


<commit_before>#pragma once

#include <hop_bits/optimisationAlgorithm.hpp>

namespace hop {
  class CovarianceMatrixAdaptationEvolutionStrategy : public OptimisationAlgorithm {
    public:
      //TODO: which input parameters should be added to the constructor?
      explicit CovarianceMatrixAdaptationEvolutionStrategy(
      	const std::shared_ptr<OptimisationProblem> optimisationProblem);

      CovarianceMatrixAdaptationEvolutionStrategy(const CovarianceMatrixAdaptationEvolutionStrategy&) = delete;
      CovarianceMatrixAdaptationEvolutionStrategy& operator=(const CovarianceMatrixAdaptationEvolutionStrategy&) = delete;

      std::string to_string() const override;

    protected:
      void optimiseImplementation() override;
      // input parameters
      // N
      unsigned int numberOfDimensions_;
      // xmean
      arma::Col<double> objectiveValues_;
      double sigma_;
      // stopfitness
      double stopValue_;

      // selection parameters
      int lambda_;
      double mu_;
      arma::Col<double> weights_;
      double mueff_;

      // adaptation parameters
      double cc_;
      double cs_;
      double c1_;
      double cmu_;
      double damps_;

      // dynamic parameters and constants
      arma::Col<double> pc_;
      arma::Col<double> ps_;
      arma::Mat<double> B_;
      arma::Mat<double> D_;
      arma::Mat<double> C_;
      arma::Mat<double> invsqrtC_;
      double eigeneval_;
      double chiN_;

      //function to use
      double frosenbrock(arma::Col<double> x);
  };
}
<commit_msg>fix: The parameter D_ should be a column vector<commit_after>#pragma once

#include <hop_bits/optimisationAlgorithm.hpp>

namespace hop {
  class CovarianceMatrixAdaptationEvolutionStrategy : public OptimisationAlgorithm {
    public:
      //TODO: which input parameters should be added to the constructor?
      explicit CovarianceMatrixAdaptationEvolutionStrategy(
      	const std::shared_ptr<OptimisationProblem> optimisationProblem);

      CovarianceMatrixAdaptationEvolutionStrategy(const CovarianceMatrixAdaptationEvolutionStrategy&) = delete;
      CovarianceMatrixAdaptationEvolutionStrategy& operator=(const CovarianceMatrixAdaptationEvolutionStrategy&) = delete;

      std::string to_string() const override;

    protected:
      void optimiseImplementation() override;
      // input parameters
      // N
      unsigned int numberOfDimensions_;
      // xmean
      arma::Col<double> objectiveValues_;
      double sigma_;
      // stopfitness
      double stopValue_;

      // selection parameters
      int lambda_;
      double mu_;
      arma::Col<double> weights_;
      double mueff_;

      // adaptation parameters
      double cc_;
      double cs_;
      double c1_;
      double cmu_;
      double damps_;

      // dynamic parameters and constants
      arma::Col<double> pc_;
      arma::Col<double> ps_;
      arma::Mat<double> B_;
      arma::Col<double> D_;
      arma::Mat<double> C_;
      arma::Mat<double> invsqrtC_;
      double eigeneval_;
      double chiN_;

      //function to use
      double frosenbrock(arma::Col<double> x);
  };
}
<|endoftext|>


<commit_before>// win32.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <cstdio>

#include "test.h"

int _tmain(int argc, _TCHAR* argv[])
{
  Test t;
  int i = t.function_one(400, true, "Hello World!");
  printf("function_one: %i\n", i);
  bool b = t.function_two(4.2e14, (void *)(0x100), 0.125f);
  printf("function_two: %i\n", b);
  double d = t.function_three(256);
  printf("function_three: %f\n", d);
  unsigned __int64 u = t.function_four();
  printf("function_four: %llu\n", u);
	return 0;
}

<commit_msg>Fixed the new test calls.<commit_after>// win32.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <cstdio>

#include "test.h"

int _tmain(int argc, _TCHAR* argv[])
{
  Test t;
  int i = t.function_one(400, true, "Hello World!");
  printf("function_one: %i\n", i);
  bool b = t.function_two(4.2e14, (void *)(0x100), 0.125f);
  printf("function_two: %i\n", b);
  double d = t.function_three(256);
  printf("function_three: %f\n", d);
  unsigned __int64 u = t.function_four();
  printf("function_four: %llu\n", u);
  float f = t.function_five(4294967306);
  printf("function_five: %f\n", f);
	return 0;
}

<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/memory/lib/phy/adr32s.H $  */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
<commit_msg>Add DLL Calibration<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/memory/lib/phy/adr32s.H $  */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */

///
/// @file adr32s.H
/// @brief Subroutines for the PHY ADR32S registers
///
// *HWP HWP Owner: Brian Silver <bsilver@us.ibm.com>
// *HWP HWP Backup: Andre Marin <aamarin@us.ibm.com>
// *HWP Team: Memory
// *HWP Level: 2
// *HWP Consumed by: FSP:HB

#ifndef _MSS_ADR32S_H_
#define _MSS_ADR32S_H_

#include <fapi2.H>
#include <p9_mc_scom_addresses.H>
#include <p9_mc_scom_addresses_fld.H>

#include <lib/shared/mss_const.H>
#include <lib/utils/scom.H>

namespace mss
{

// I have a dream that the PHY code can be shared among controllers. So, I drive the
// engine from a set of traits. This might be folly. Allow me to dream. BRS

///
/// @class adr32sTraits
/// @brief a collection of traits associated with the PHY ADR32S interface
/// @tparam T fapi2::TargetType representing the PHY
///
template< fapi2::TargetType T >
class adr32sTraits;

///
/// @class adr32sTraits
/// @brief a collection of traits associated with the Centaur PHY ADR32S interface
///
template<>
class adr32sTraits<fapi2::TARGET_TYPE_MBA>
{
};

///
/// @class adr32sTraits
/// @brief a collection of traits associated with the Nimbus PHY ADR32S
///
template<>
class adr32sTraits<fapi2::TARGET_TYPE_MCA>
{
    public:

        // Number of ADR32S units
        static constexpr uint64_t ADR32S_COUNT = 2;

        // MCA ADR32S control registers all come in pairs.
        static const std::vector<uint64_t> DLL_CNFG_REG;

        enum
        {
            DLL_CNTL_INIT_RXDLL_CAL_RESET = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_INIT_RXDLL_CAL_RESET,
            DLL_CNTL_INIT_RXDLL_CAL_UPDATE = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_INIT_RXDLL_CAL_UPDATE,
            DLL_CNTL_REGS_RXDLL_CAL_SKIP = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_REGS_RXDLL_CAL_SKIP,
            DLL_CNTL_REGS_RXDLL_CAL_SKIP_LEN = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_REGS_RXDLL_CAL_SKIP_LEN,
            DLL_CNTL_REGS_RXDLL_COARSE_ADJ_BY2 = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_REGS_RXDLL_COARSE_ADJ_BY2,
            DLL_CNTL_RXREG_FILTER_LENGTH_DC = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_RXREG_FILTER_LENGTH_DC,
            DLL_CNTL_RXREG_FILTER_LENGTH_DC_LEN = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_RXREG_FILTER_LENGTH_DC_LEN,
            DLL_CNTL_RXREG_LEAD_LAG_SEPARATION_DC = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_RXREG_LEAD_LAG_SEPARATION_DC,
            DLL_CNTL_RXREG_LEAD_LAG_SEPARATION_DC_LEN =
                MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_RXREG_LEAD_LAG_SEPARATION_DC_LEN,
            DLL_CNTL_EN_DRIVER_INVFB_DC = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_EN_DRIVER_INVFB_DC,
            DLL_CNTL_RXREG_FINECAL_2XILSB_DC = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_RXREG_FINECAL_2XILSB_DC,
            DLL_CNTL_CAL_GOOD = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_CAL_GOOD,
            DLL_CNTL_CAL_ERROR = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_CAL_ERROR,
            DLL_CNTL_CAL_ERROR_FINE = MCA_DDRPHY_ADR_DLL_CNTL_P0_ADR32S0_ADR0_CAL_ERROR_FINE,

        };

};

namespace adr32s
{

///
/// @brief Read DLL_CNTL
/// @tparam I ADR32S instance e.g., 0 or 1 for a 64 bit implementation of the PHY
/// @tparam T fapi2 Target Type - derived
/// @tparam TT traits type defaults to adr32sTraits<T>
/// @param[in] i_target the fapi2 target of the port
/// @param[out] o_data the value of the register
/// @return fapi2::ReturnCode FAPI2_RC_SUCCESS if ok
///
template< uint64_t I, fapi2::TargetType T, typename TT = adr32sTraits<T> >
inline fapi2::ReturnCode read_dll_cntl( const fapi2::Target<T>& i_target, fapi2::buffer<uint64_t>& o_data )
{
    static_assert( I < TT::ADR32S_COUNT, "adr32s instance out of range");
    FAPI_TRY( mss::getScom(i_target, TT::DLL_CNFG_REG[I], o_data) );
    FAPI_INF("dll_cntl adrs32%d: 0x%016lx", I, o_data);
fapi_try_exit:
    return fapi2::current_err;
}

///
/// @brief Write DLL_CNTL_ADR32S0
/// @tparam I ADR32S instance e.g., 0 or 1 for a 64 bit implementation of the PHY
/// @tparam T fapi2 Target Type - derived
/// @tparam TT traits type defaults to adr32sTraits<T>
/// @param[in] i_target the fapi2 target of the port
/// @param[in] i_data the value of the register
/// @return fapi2::ReturnCode FAPI2_RC_SUCCESS if ok
///
template< uint64_t I, fapi2::TargetType T, typename TT = adr32sTraits<T> >
inline fapi2::ReturnCode write_dll_cntl( const fapi2::Target<T>& i_target, const fapi2::buffer<uint64_t>& i_data )
{
    static_assert( I < TT::ADR32S_COUNT, "adr32s instance out of range");
    FAPI_INF("dll_cntl adr32s%d: 0x%016lx", I, i_data);
    FAPI_TRY( mss::putScom(i_target, TT::DLL_CNFG_REG[I], i_data) );
fapi_try_exit:
    return fapi2::current_err;
}

//
// Reseting the DLL registers TODO RTC:156518
//

///
/// @brief Set the DLL cal reset (begins DLL cal operations)
/// @tparam T fapi2 Target Type - derived
/// @tparam TT traits type defaults to adr32sTraits<T>
/// @param[in] o_data the value of the register
/// @param[in] i_state mss::LOW or mss::HIGH representing the state of the bit
/// @note Default state is 'low' as writing a 0 forces the cal to begin.
///
template< fapi2::TargetType T = fapi2::TARGET_TYPE_MCA, typename TT = adr32sTraits<T> >
inline void set_dll_cal_reset( fapi2::buffer<uint64_t>& o_data, const states i_state = mss::LOW )
{
    FAPI_INF("set_dll_cal_reset %s", (i_state == mss::LOW ? "low" : "high"));
    o_data.writeBit<TT::DLL_CNTL_INIT_RXDLL_CAL_RESET>(i_state);
}

} // close namespace adr

} // close namespace mss

#endif
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2017 Fondazione Istituto Italiano di Tecnologia
 * Authors: Silvio Traversaro
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include <iDynTree/InverseKinematics.h>
#include <iDynTree/KinDynComputations.h>

#include <iDynTree/Core/TestUtils.h>
#include <iDynTree/Model/JointState.h>
#include <iDynTree/Model/ModelTestUtils.h>

#include "testModels.h"

#include <cstdio>
#include <cstdlib>

#include <ctime>

/**
 * Return the current time in seconds, with respect
 * to an arbitrary point in time.
 */
inline double clockInSec()
{
    clock_t ret = clock();
    return ((double)ret)/((double)CLOCKS_PER_SEC);
}

iDynTree::JointPosDoubleArray getRandomJointPositions(const iDynTree::Model & model)
{
    iDynTree::JointPosDoubleArray sRandom(model);
    getRandomJointPositions(sRandom,model);
    return sRandom;
}

struct simpleChainIKOptions
{
    iDynTree::InverseKinematicsRotationParametrization rotationParametrization;
    bool useDesiredJointPositionsToActualValue;
    bool useDesiredJointPositionsToRandomValue;
};

void simpleChainIK(int minNrOfJoints, int maxNrOfJoints, const iDynTree::InverseKinematicsRotationParametrization rotationParametrization)
{
    // Solve a simple IK problem for a chain, with no constraints
    for (int i = minNrOfJoints; i <= maxNrOfJoints; i++)
    {
        assert(i >= 2);

        std::cerr << "~~~~~~~> simpleChainIK with " << i << " dofs " << std::endl;
        bool noFixedJoints = true;
        iDynTree::Model chain = iDynTree::getRandomChain(i,10,noFixedJoints);

        ASSERT_EQUAL_DOUBLE(i, chain.getNrOfDOFs());

        // Name of the targetFrame the leaf added by getRandomChain
        std::string targetFrame = "link" + iDynTree::int2string(i - 1);

        // Create IK
        iDynTree::InverseKinematics ik;
        bool ok = ik.setModel(chain);
        ASSERT_IS_TRUE(ok);
        // Always express the target as cost
        ik.setTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone);

        // Use the requested parametrization
        ik.setRotationParametrization(rotationParametrization);

        // Create also a KinDyn object to perform forward kinematics for the desired values and the optimized ones
        iDynTree::KinDynComputations kinDynDes;
        ok = kinDynDes.loadRobotModel(ik.model());
        ASSERT_IS_TRUE(ok);
        iDynTree::KinDynComputations kinDynOpt;
        ok = kinDynOpt.loadRobotModel(ik.model());
        ASSERT_IS_TRUE(ok);

        // Create a random vector of internal joint positions
        // used to get reasonable values for the constraints and the targets
        iDynTree::JointPosDoubleArray s = getRandomJointPositions(kinDynDes.model());
        ok = kinDynDes.setJointPos(s);
        ASSERT_IS_TRUE(ok);

        // Add the fixed base constraint
        ok = ik.addFrameConstraint("link1", kinDynDes.getWorldTransform("link1"));
        //ok = ik.addFrameRotationConstraint("link1", iDynTree::Transform::Identity().getRotation());
        //ok = ik.addFramePositionConstraint("link1", iDynTree::Transform::Identity().getPosition());
        ASSERT_IS_TRUE(ok);

        // Add target
        ok = ik.addPositionTarget(targetFrame, kinDynDes.getWorldTransform(targetFrame));
        ASSERT_IS_TRUE(ok);

        // Add a random initial guess
        iDynTree::Transform baseDes = kinDynDes.getWorldBaseTransform();
        iDynTree::JointPosDoubleArray sInitial = getRandomJointPositions(ik.model());
        ik.setInitialCondition(&(baseDes),&(sInitial));

        // Add a random desired position
        iDynTree::JointPosDoubleArray sDesired = getRandomJointPositions(ik.model());
        ik.setDesiredJointConfiguration(sDesired,1e-12);

        // Start from the initial one
        double tic = clockInSec();
        ok = ik.solve();
        double toc = clockInSec();

        ASSERT_IS_TRUE(ok);

        // Get the solution
        iDynTree::Transform baseOpt = iDynTree::Transform::Identity();
        iDynTree::JointPosDoubleArray sOpt(ik.model());
        ik.getSolution(baseOpt,sOpt);
        iDynTree::Twist dummyVel;
        dummyVel.zero();
        iDynTree::Vector3 dummyGrav;
        dummyGrav.zero();
        iDynTree::JointDOFsDoubleArray dummyJointVel(ik.model());
        dummyJointVel.zero();

        kinDynOpt.setRobotState(baseOpt,sOpt,dummyVel,dummyJointVel,dummyGrav);
        double tolConstraints = 1e-7;
        double tolTargets     = 1e-6;

        // Check if the base link constraint is still valid
        ASSERT_EQUAL_TRANSFORM_TOL(kinDynOpt.getWorldTransform("link1"), kinDynDes.getWorldTransform("link1"), tolConstraints);

        // Check if the target is realized
        ASSERT_EQUAL_VECTOR_TOL(kinDynDes.getWorldTransform(targetFrame).getPosition(), kinDynOpt.getWorldTransform(targetFrame).getPosition(), tolTargets);
    }

}

// Check the consistency of a simple humanoid wholebody IK test case
void simpleHumanoidWholeBodyIKConsistency(const iDynTree::InverseKinematicsRotationParametrization rotationParametrization)
{
    iDynTree::InverseKinematics ik;

    bool ok = ik.loadModelFromFile(getAbsModelPath("iCubGenova02.urdf"));
    ASSERT_IS_TRUE(ok);

    //ik.setFloatingBaseOnFrameNamed("l_foot");

    ik.setRotationParametrization(rotationParametrization);

    // Create also a KinDyn object to perform forward kinematics for the desired values
    iDynTree::KinDynComputations kinDynDes;
    ok = kinDynDes.loadRobotModel(ik.model());
    ASSERT_IS_TRUE(ok);

    // Create a random vector of internal joint positions
    // used to get reasonable values for the constraints and the targets
    iDynTree::JointPosDoubleArray s = getRandomJointPositions(kinDynDes.model());
    ok = kinDynDes.setJointPos(s);
    ASSERT_IS_TRUE(ok);

    // Create a simple IK problem with the foot constraint

    // The l_sole frame is our world absolute frame (i.e. {}^A H_{l_sole} = identity
    ok = ik.addFrameConstraint("l_foot",kinDynDes.getWorldTransform("l_foot"));
    ASSERT_IS_TRUE(ok);

    std::cerr << "kinDynDes.getWorldTransform(l_foot) : " << kinDynDes.getWorldTransform("l_foot").toString() << std::endl;

    // The relative position of the r_sole should be the initial one
    ok = ik.addFrameConstraint("r_sole",kinDynDes.getWorldTransform("r_sole"));
    ASSERT_IS_TRUE(ok);

    // The two cartesian targets should be reasonable values
    ik.setTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone);
    ok = ik.addPositionTarget("l_elbow_1",kinDynDes.getWorldTransform("l_elbow_1"));
    ASSERT_IS_TRUE(ok);

    //ok = ik.addPositionTarget("r_elbow_1",kinDynDes.getRelativeTransform("l_sole","r_elbow_1").getPosition());
    //ASSERT_IS_TRUE(ok);

    iDynTree::Transform initialH = kinDynDes.getWorldBaseTransform();

    ik.setInitialCondition(&initialH,&s);
    ik.setDesiredJointConfiguration(s,1e-15);

    // Solve the optimization problem
    double tic = clockInSec();
    ok = ik.solve();
    double toc = clockInSec();
    std::cerr << "Inverse Kinematics solved in " << toc-tic << " seconds. " << std::endl;
    ASSERT_IS_TRUE(ok);

    iDynTree::Transform basePosOptimized;
    iDynTree::JointPosDoubleArray sOptimized(ik.model());
    sOptimized.zero();

    ik.getSolution(basePosOptimized,sOptimized);

    // We create a new KinDyn object to perform forward kinematics for the optimized values
    iDynTree::KinDynComputations kinDynOpt;
    kinDynOpt.loadRobotModel(ik.model());
    iDynTree::Twist dummyVel;
    dummyVel.zero();
    iDynTree::Vector3 dummyGrav;
    dummyGrav.zero();
    iDynTree::JointDOFsDoubleArray dummyJointVel(ik.model());
    dummyJointVel.zero();
    kinDynOpt.setRobotState(basePosOptimized, sOptimized, dummyVel, dummyJointVel, dummyGrav);

    // Check that the contraint and the targets are respected
    double tolConstraints = 1e-7;
    double tolTargets     = 1e-6;
    ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("l_foot"),kinDynOpt.getWorldTransform("l_foot"),tolConstraints);
    ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("r_sole"),kinDynOpt.getWorldTransform("r_sole"),tolConstraints);
    ASSERT_EQUAL_VECTOR_TOL(kinDynDes.getWorldTransform("l_elbow_1").getPosition(),
                            kinDynOpt.getWorldTransform("l_elbow_1").getPosition(),tolTargets);

    return;
}

int main()
{
    // Improve repetability (at least in the same platform)
    srand(0);

    simpleChainIK(2,13,iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw);

    // This is not working at the moment, there is some problem with quaternion constraints
    //simpleChainIK(10,iDynTree::InverseKinematicsRotationParametrizationQuaternion);

    simpleHumanoidWholeBodyIKConsistency(iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw);

    return EXIT_SUCCESS;
}
<commit_msg>Improve robustness of Inverse Kinematics test<commit_after>/*
 * Copyright (C) 2017 Fondazione Istituto Italiano di Tecnologia
 * Authors: Silvio Traversaro
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include <iDynTree/InverseKinematics.h>
#include <iDynTree/KinDynComputations.h>

#include <iDynTree/Core/TestUtils.h>
#include <iDynTree/Model/JointState.h>
#include <iDynTree/Model/ModelTestUtils.h>

#include "testModels.h"

#include <cstdio>
#include <cstdlib>

#include <ctime>

/**
 * Return the current time in seconds, with respect
 * to an arbitrary point in time.
 */
inline double clockInSec()
{
    clock_t ret = clock();
    return ((double)ret)/((double)CLOCKS_PER_SEC);
}

iDynTree::JointPosDoubleArray getRandomJointPositions(const iDynTree::Model & model)
{
    iDynTree::JointPosDoubleArray sRandom(model);
    getRandomJointPositions(sRandom,model);
    return sRandom;
}

inline iDynTree::JointPosDoubleArray getRandomJointPositionsCloseTo(const iDynTree::Model& model, iDynTree::VectorDynSize s, double maxDelta)
{
    iDynTree::JointPosDoubleArray vec(model);

    assert(vec.size() == model.getNrOfPosCoords());
    for(iDynTree::JointIndex jntIdx=0; jntIdx < model.getNrOfJoints(); jntIdx++)
    {
        iDynTree::IJointConstPtr jntPtr = model.getJoint(jntIdx);
        if( jntPtr->hasPosLimits() )
        {
            for(int i=0; i < jntPtr->getNrOfPosCoords(); i++)
            {
                int dofIndex = jntPtr->getDOFsOffset()+i;
                double max = std::min(jntPtr->getMaxPosLimit(i),s(dofIndex)+maxDelta);
                double min = std::max(jntPtr->getMinPosLimit(i),s(dofIndex)-maxDelta);
                vec(dofIndex) = iDynTree::getRandomDouble(min,max);
            }
        }
        else
        {
            for(int i=0; i < jntPtr->getNrOfPosCoords(); i++)
            {
                int dofIndex = jntPtr->getDOFsOffset()+i;
                vec(jntPtr->getDOFsOffset()+i) = iDynTree::getRandomDouble(s(dofIndex)-maxDelta,s(dofIndex)+maxDelta);
            }
        }
    }

    return vec;
}

struct simpleChainIKOptions
{
    iDynTree::InverseKinematicsRotationParametrization rotationParametrization;
    bool useDesiredJointPositionsToActualValue;
    bool useDesiredJointPositionsToRandomValue;
};

void simpleChainIK(int minNrOfJoints, int maxNrOfJoints, const iDynTree::InverseKinematicsRotationParametrization rotationParametrization)
{
    // Solve a simple IK problem for a chain, with no constraints
    for (int i = minNrOfJoints; i <= maxNrOfJoints; i++)
    {
        assert(i >= 2);

        std::cerr << "~~~~~~~> simpleChainIK with " << i << " dofs " << std::endl;
        bool noFixedJoints = true;
        iDynTree::Model chain = iDynTree::getRandomChain(i,10,noFixedJoints);

        ASSERT_EQUAL_DOUBLE(i, chain.getNrOfDOFs());

        // Name of the targetFrame the leaf added by getRandomChain
        std::string targetFrame = "link" + iDynTree::int2string(i - 1);

        // Create IK
        iDynTree::InverseKinematics ik;
        bool ok = ik.setModel(chain);
        ASSERT_IS_TRUE(ok);
        // Always express the target as cost
        ik.setTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone);

        // Use the requested parametrization
        ik.setRotationParametrization(rotationParametrization);

        ik.setTol(1e-6);
        ik.setConstrTol(1e-7);

        // Create also a KinDyn object to perform forward kinematics for the desired values and the optimized ones
        iDynTree::KinDynComputations kinDynDes;
        ok = kinDynDes.loadRobotModel(ik.model());
        ASSERT_IS_TRUE(ok);
        iDynTree::KinDynComputations kinDynOpt;
        ok = kinDynOpt.loadRobotModel(ik.model());
        ASSERT_IS_TRUE(ok);

        // Create a random vector of internal joint positions
        // used to get reasonable values for the constraints and the targets
        iDynTree::JointPosDoubleArray s = getRandomJointPositions(kinDynDes.model());
        ok = kinDynDes.setJointPos(s);
        ASSERT_IS_TRUE(ok);

        // Add the fixed base constraint
        ok = ik.addFrameConstraint("link1", kinDynDes.getWorldTransform("link1"));
        //ok = ik.addFrameRotationConstraint("link1", iDynTree::Transform::Identity().getRotation());
        //ok = ik.addFramePositionConstraint("link1", iDynTree::Transform::Identity().getPosition());
        ASSERT_IS_TRUE(ok);

        // Add target
        ok = ik.addPositionTarget(targetFrame, kinDynDes.getWorldTransform(targetFrame));
        ASSERT_IS_TRUE(ok);

        // Add a random initial guess
        iDynTree::Transform baseDes = kinDynDes.getWorldBaseTransform();
        // Set a max delta to avoid local minima in testing
        double maxDelta = 0.01;
        iDynTree::JointPosDoubleArray sInitial = getRandomJointPositionsCloseTo(ik.model(),s,maxDelta);
        ik.setInitialCondition(&(baseDes),&(sInitial));

        // Add a random desired position
        iDynTree::JointPosDoubleArray sDesired = getRandomJointPositions(ik.model());
        ik.setDesiredJointConfiguration(sDesired,1e-12);

        // Start from the initial one
        double tic = clockInSec();
        ok = ik.solve();
        double toc = clockInSec();

        ASSERT_IS_TRUE(ok);

        // Get the solution
        iDynTree::Transform baseOpt = iDynTree::Transform::Identity();
        iDynTree::JointPosDoubleArray sOpt(ik.model());
        ik.getSolution(baseOpt,sOpt);
        iDynTree::Twist dummyVel;
        dummyVel.zero();
        iDynTree::Vector3 dummyGrav;
        dummyGrav.zero();
        iDynTree::JointDOFsDoubleArray dummyJointVel(ik.model());
        dummyJointVel.zero();

        kinDynOpt.setRobotState(baseOpt,sOpt,dummyVel,dummyJointVel,dummyGrav);
        double tolConstraints = 1e-6;
        double tolTargets     = 1e-3;

        // Check if the base link constraint is still valid
        ASSERT_EQUAL_TRANSFORM_TOL(kinDynOpt.getWorldTransform("link1"), kinDynDes.getWorldTransform("link1"), tolConstraints);

        // Check if the target is realized
        ASSERT_EQUAL_VECTOR_TOL(kinDynDes.getWorldTransform(targetFrame).getPosition(), kinDynOpt.getWorldTransform(targetFrame).getPosition(), tolTargets);
    }

}

// Check the consistency of a simple humanoid wholebody IK test case
void simpleHumanoidWholeBodyIKConsistency(const iDynTree::InverseKinematicsRotationParametrization rotationParametrization)
{
    iDynTree::InverseKinematics ik;

    bool ok = ik.loadModelFromFile(getAbsModelPath("iCubGenova02.urdf"));
    ASSERT_IS_TRUE(ok);

    //ik.setFloatingBaseOnFrameNamed("l_foot");

    ik.setRotationParametrization(rotationParametrization);

    // Create also a KinDyn object to perform forward kinematics for the desired values
    iDynTree::KinDynComputations kinDynDes;
    ok = kinDynDes.loadRobotModel(ik.model());
    ASSERT_IS_TRUE(ok);

    // Create a random vector of internal joint positions
    // used to get reasonable values for the constraints and the targets
    iDynTree::JointPosDoubleArray s = getRandomJointPositions(kinDynDes.model());
    ok = kinDynDes.setJointPos(s);
    ASSERT_IS_TRUE(ok);

    // Create a simple IK problem with the foot constraint

    // The l_sole frame is our world absolute frame (i.e. {}^A H_{l_sole} = identity
    ok = ik.addFrameConstraint("l_foot",kinDynDes.getWorldTransform("l_foot"));
    ASSERT_IS_TRUE(ok);

    std::cerr << "kinDynDes.getWorldTransform(l_foot) : " << kinDynDes.getWorldTransform("l_foot").toString() << std::endl;

    // The relative position of the r_sole should be the initial one
    ok = ik.addFrameConstraint("r_sole",kinDynDes.getWorldTransform("r_sole"));
    ASSERT_IS_TRUE(ok);

    // The two cartesian targets should be reasonable values
    ik.setTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone);
    ok = ik.addPositionTarget("l_elbow_1",kinDynDes.getWorldTransform("l_elbow_1"));
    ASSERT_IS_TRUE(ok);

    //ok = ik.addPositionTarget("r_elbow_1",kinDynDes.getRelativeTransform("l_sole","r_elbow_1").getPosition());
    //ASSERT_IS_TRUE(ok);

    iDynTree::Transform initialH = kinDynDes.getWorldBaseTransform();

    ik.setInitialCondition(&initialH,&s);
    ik.setDesiredJointConfiguration(s,1e-15);

    // Solve the optimization problem
    double tic = clockInSec();
    ok = ik.solve();
    double toc = clockInSec();
    std::cerr << "Inverse Kinematics solved in " << toc-tic << " seconds. " << std::endl;
    ASSERT_IS_TRUE(ok);

    iDynTree::Transform basePosOptimized;
    iDynTree::JointPosDoubleArray sOptimized(ik.model());
    sOptimized.zero();

    ik.getSolution(basePosOptimized,sOptimized);

    // We create a new KinDyn object to perform forward kinematics for the optimized values
    iDynTree::KinDynComputations kinDynOpt;
    kinDynOpt.loadRobotModel(ik.model());
    iDynTree::Twist dummyVel;
    dummyVel.zero();
    iDynTree::Vector3 dummyGrav;
    dummyGrav.zero();
    iDynTree::JointDOFsDoubleArray dummyJointVel(ik.model());
    dummyJointVel.zero();
    kinDynOpt.setRobotState(basePosOptimized, sOptimized, dummyVel, dummyJointVel, dummyGrav);

    // Check that the contraint and the targets are respected
    double tolConstraints = 1e-7;
    double tolTargets     = 1e-6;
    ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("l_foot"),kinDynOpt.getWorldTransform("l_foot"),tolConstraints);
    ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("r_sole"),kinDynOpt.getWorldTransform("r_sole"),tolConstraints);
    ASSERT_EQUAL_VECTOR_TOL(kinDynDes.getWorldTransform("l_elbow_1").getPosition(),
                            kinDynOpt.getWorldTransform("l_elbow_1").getPosition(),tolTargets);

    return;
}

int main()
{
    // Improve repetability (at least in the same platform)
    srand(0);

    simpleChainIK(2,13,iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw);

    // This is not working at the moment, there is some problem with quaternion constraints
    //simpleChainIK(10,iDynTree::InverseKinematicsRotationParametrizationQuaternion);

    simpleHumanoidWholeBodyIKConsistency(iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw);

    return EXIT_SUCCESS;
}
<|endoftext|>