<commit_before>/*
 * This istream filter catches fatal errors and attempts to ignore
 * them.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "istream_catch.hxx"
#include "istream_internal.hxx"
#include "istream_forward.hxx"
#include "util/Cast.hxx"

#include <daemon/log.h>

#include <assert.h>

struct CatchIstream {
    struct istream output;
    struct istream *input;
    off_t available;

    GError *(*callback)(GError *error, void *ctx);
    void *callback_ctx;

    void SendSpace();
};

static constexpr char space[] =
    "                                "
    "                                "
    "                                "
    "                                ";

void
CatchIstream::SendSpace()
{
    assert(input == nullptr);
    assert(available > 0);

    do {
        size_t length;
        if (available >= (off_t)sizeof(space) - 1)
            length = sizeof(space) - 1;
        else
            length = (size_t)available;

        size_t nbytes = istream_invoke_data(&output, space, length);
        if (nbytes == 0)
            return;

        available -= nbytes;
        if (nbytes < length)
            return;
    } while (available > 0);

    istream_deinit_eof(&output);
}

/*
 * istream handler
 *
 */

static size_t
catch_input_data(const void *data, size_t length, void *ctx)
{
    auto *c = (CatchIstream *)ctx;

    size_t nbytes = istream_invoke_data(&c->output, data, length);
    if (nbytes > 0) {
        if ((off_t)nbytes < c->available)
            c->available -= (off_t)nbytes;
        else
            c->available = 0;
    }

    return nbytes;
}

static ssize_t
catch_input_direct(enum istream_direct type, int fd, size_t max_length,
                   void *ctx)
{
    auto *c = (CatchIstream *)ctx;

    ssize_t nbytes = istream_invoke_direct(&c->output, type, fd, max_length);
    if (nbytes > 0) {
        if ((off_t)nbytes < c->available)
            c->available -= (off_t)nbytes;
        else
            c->available = 0;
    }

    return nbytes;
}

static void
catch_input_abort(GError *error, void *ctx)
{
    auto *c = (CatchIstream *)ctx;

    error = c->callback(error, c->callback_ctx);
    if (error != nullptr) {
        /* forward error to our handler */
        istream_deinit_abort(&c->output, error);
        return;
    }

    /* the error has been handled by the callback, and he has disposed
       it */

    c->input = nullptr;

    if (c->available > 0) {
        /* according to a previous call to method "available", there
           is more data which we must provide - fill that with space
           characters */
        c->input = nullptr;
        c->SendSpace();
    } else
        istream_deinit_eof(&c->output);
}

static const struct istream_handler catch_input_handler = {
    .data = catch_input_data,
    .direct = catch_input_direct,
    .eof = istream_forward_eof,
    .abort = catch_input_abort,
};

/*
 * istream implementation
 *
 */

static inline CatchIstream *
istream_to_catch(struct istream *istream)
{
    return &ContainerCast2(*istream, &CatchIstream::output);
}

static off_t
istream_catch_available(struct istream *istream, bool partial)
{
    CatchIstream *c = istream_to_catch(istream);

    if (c->input != nullptr) {
        off_t available = istream_available(c->input, partial);
        if (available != (off_t)-1 && available > c->available)
            c->available = available;

        return available;
    } else
        return c->available;
}

static void
istream_catch_read(struct istream *istream)
{
    CatchIstream *c = istream_to_catch(istream);

    if (c->input != nullptr) {
        istream_handler_set_direct(c->input, c->output.handler_direct);
        istream_read(c->input);
    } else
        c->SendSpace();
}

static void
istream_catch_close(struct istream *istream)
{
    CatchIstream *c = istream_to_catch(istream);

    if (c->input != nullptr)
        istream_free_handler(&c->input);

    istream_deinit(&c->output);
}

static const struct istream_class istream_catch = {
    .available = istream_catch_available,
    .read = istream_catch_read,
    .close = istream_catch_close,
};

/*
 * constructor
 *
 */

struct istream *
istream_catch_new(struct pool *pool, struct istream *input,
                  GError *(*callback)(GError *error, void *ctx), void *ctx)
{
    assert(input != nullptr);
    assert(!istream_has_handler(input));
    assert(callback != nullptr);

    auto *c = NewFromPool<CatchIstream>(*pool);
    istream_init(&c->output, &istream_catch, pool);

    istream_assign_handler(&c->input, input,
                           &catch_input_handler, c,
                           0);
    c->available = 0;
    c->callback = callback;
    c->callback_ctx = ctx;

    return &c->output;
}
<commit_msg>istream_catch: use the OO API<commit_after>/*
 * This istream filter catches fatal errors and attempts to ignore
 * them.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "istream_catch.hxx"
#include "istream_internal.hxx"
#include "istream_forward.hxx"
#include "util/Cast.hxx"

#include <daemon/log.h>

#include <assert.h>

class CatchIstream final : public ForwardIstream {
    off_t available = 0;

    GError *(*const callback)(GError *error, void *ctx);
    void *const callback_ctx;

public:
    CatchIstream(struct pool &pool, struct istream &_input,
                 GError *(*_callback)(GError *error, void *ctx), void *ctx)
        :ForwardIstream(pool, _input,
                        MakeIstreamHandler<CatchIstream>::handler, this),
         callback(_callback), callback_ctx(ctx) {}

    void SendSpace();

    /* virtual methods from class Istream */

    off_t GetAvailable(bool partial) override;

    off_t Skip(off_t length) override {
        off_t nbytes = ForwardIstream::Skip(length);
        if (nbytes > 0) {
            if (nbytes < available)
                available -= nbytes;
            else
                available = 0;
        }

        return nbytes;
    }

    void Read() override;
    void Close() override;

    /* handler */

    size_t OnData(const void *data, size_t length);
    ssize_t OnDirect(enum istream_direct type, int fd, size_t max_length);
    void OnError(GError *error);
};

static constexpr char space[] =
    "                                "
    "                                "
    "                                "
    "                                ";

void
CatchIstream::SendSpace()
{
    assert(!HasInput());
    assert(available > 0);

    do {
        size_t length;
        if (available >= (off_t)sizeof(space) - 1)
            length = sizeof(space) - 1;
        else
            length = (size_t)available;

        size_t nbytes = ForwardIstream::OnData(space, length);
        if (nbytes == 0)
            return;

        available -= nbytes;
        if (nbytes < length)
            return;
    } while (available > 0);

    DestroyEof();
}

/*
 * istream handler
 *
 */

size_t
CatchIstream::OnData(const void *data, size_t length)
{
    size_t nbytes = ForwardIstream::OnData(data, length);
    if (nbytes > 0) {
        if ((off_t)nbytes < available)
            available -= (off_t)nbytes;
        else
            available = 0;
    }

    return nbytes;
}

ssize_t
CatchIstream::OnDirect(enum istream_direct type, int fd, size_t max_length)
{
    ssize_t nbytes = ForwardIstream::OnDirect(type, fd, max_length);
    if (nbytes > 0) {
        if ((off_t)nbytes < available)
            available -= (off_t)nbytes;
        else
            available = 0;
    }

    return nbytes;
}

void
CatchIstream::OnError(GError *error)
{
    error = callback(error, callback_ctx);
    if (error != nullptr) {
        /* forward error to our handler */
        ForwardIstream::OnError(error);
        return;
    }

    /* the error has been handled by the callback, and he has disposed
       it */

    ClearInput();

    if (available > 0)
        /* according to a previous call to method "available", there
           is more data which we must provide - fill that with space
           characters */
        SendSpace();
    else
        DestroyEof();
}

/*
 * istream implementation
 *
 */

off_t
CatchIstream::GetAvailable(bool partial)
{
    if (HasInput()) {
        off_t result = ForwardIstream::GetAvailable(partial);
        if (result > available)
            available = result;

        return result;
    } else
        return available;
}

void
CatchIstream::Read()
{
    if (HasInput())
        ForwardIstream::Read();
    else
        SendSpace();
}

void
CatchIstream::Close()
{
    if (HasInput())
        input.CloseHandler();

    Destroy();
}

/*
 * constructor
 *
 */

struct istream *
istream_catch_new(struct pool *pool, struct istream *input,
                  GError *(*callback)(GError *error, void *ctx), void *ctx)
{
    assert(input != nullptr);
    assert(!istream_has_handler(input));
    assert(callback != nullptr);

    return NewIstream<CatchIstream>(*pool, *input, callback, ctx);
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2005-2019 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * 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 "otbWrapperOutputImageParameter.h"
#include "otbClampImageFilter.h"
#include "otbImageIOFactory.h"
#include "itksys/SystemTools.hxx"

#ifdef OTB_USE_MPI

#include "otbMPIConfig.h"
#include "otbMPIVrtWriter.h"

#ifdef OTB_USE_SPTW
#include "otbSimpleParallelTiffWriter.h"
#endif

#endif

#define CAST_IMAGE_BASE( T, image_base )		\
  {							\
    T * img = dynamic_cast< T * >( image_base );	\
							\
    if( img )						\
      SwitchInput< T >( img );				\
    							\
    return;						\
  }

namespace otb
{
namespace Wrapper
{

// Declare specialisation for UInt8RGBAImageType
template<>
void OutputImageParameter::SwitchInput(UInt8RGBAImageType * );

// Declare specialisation for UInt8RGBImageType
template <>
void OutputImageParameter::SwitchInput( UInt8RGBImageType * );

OutputImageParameter::OutputImageParameter()
  : m_PixelType(ImagePixelType_float),
    m_DefaultPixelType(ImagePixelType_float),
    m_RAMValue(0)
{
  SetName("Output Image");
  SetKey("out");
}

OutputImageParameter::~OutputImageParameter()
{
}

std::string OutputImageParameter::ConvertPixelTypeToString(ImagePixelType type)
{
  // TODO: Could be replaced by constant static string array e.g.:
  // return PIXEL_TYPE_NAME[ type ];

  std::string ret;
  switch(type)
    {
    case ImagePixelType_uint8:
      {
      ret = "uint8";
      break;
      }
    case ImagePixelType_int16:
      {
      ret = "int16";
      break;
      }
    case ImagePixelType_uint16:
      {
      ret = "uint16";
      break;
      }
    case ImagePixelType_int32:
      {
      ret = "int32";
      break;
      }
    case ImagePixelType_uint32:
      {
      ret = "uint32";
      break;
      }
    case ImagePixelType_float:
      {
      ret = "float";
      break;
      }
    case ImagePixelType_double:
      {
      ret = "double";
      break;
      }
    case ImagePixelType_cint16:
      {
      ret = "cint16";
      break;
      }
    case ImagePixelType_cint32:
      {
      ret = "cint32";
      break;
      }
    case ImagePixelType_cfloat:
      {
      ret = "cfloat";
      break;
      }
    case ImagePixelType_cdouble:
      {
      ret = "cdouble";
      break;
      }
    }
  return ret;
}

bool
OutputImageParameter::ConvertStringToPixelType(const std::string &value, ImagePixelType &type)
{
  // TODO: Could be replaced std::find_if() in constant static string
  // array (see ::ConvertPixelTypeToString().

  if (value == "uint8")
    type = ImagePixelType_uint8;
  else if (value == "int16")
    type = ImagePixelType_int16;
  else if (value == "uint16")
    type = ImagePixelType_uint16;
  else if (value == "int32")
    type = ImagePixelType_int32;
  else if (value == "uint32")
    type = ImagePixelType_uint32;
  else if (value == "float")
    type = ImagePixelType_float;
  else if (value == "double")
    type = ImagePixelType_double;
  else if (value == "cint16")
    type = ImagePixelType_cint16;
  else if (value == "cint32")
    type = ImagePixelType_cint32;
  else if (value == "cfloat")
    type = ImagePixelType_cfloat;
  else if (value == "cdouble")
    type = ImagePixelType_cdouble;
  else
    return false;
  return true;
}

void
OutputImageParameter
::InitializeWriters()
{
  ImageBaseType * image = m_Image.GetPointer();

  CAST_IMAGE_BASE( UInt8VectorImageType, image );
  CAST_IMAGE_BASE( Int16VectorImageType, image );
  CAST_IMAGE_BASE( UInt16VectorImageType, image );
  CAST_IMAGE_BASE( Int32VectorImageType, image );
  CAST_IMAGE_BASE( UInt32VectorImageType, image );

  CAST_IMAGE_BASE( FloatVectorImageType, image );
  CAST_IMAGE_BASE( DoubleVectorImageType, image );

  CAST_IMAGE_BASE( ComplexInt16VectorImageType, image );
  CAST_IMAGE_BASE( ComplexInt32VectorImageType, image );
  CAST_IMAGE_BASE( ComplexFloatVectorImageType, image );
  CAST_IMAGE_BASE( ComplexDoubleVectorImageType, image );

  CAST_IMAGE_BASE( UInt8ImageType, image );
  CAST_IMAGE_BASE( Int16ImageType, image );
  CAST_IMAGE_BASE( UInt16ImageType, image );
  CAST_IMAGE_BASE( Int32ImageType, image );
  CAST_IMAGE_BASE( UInt32ImageType, image );

  CAST_IMAGE_BASE( FloatImageType, image );
  CAST_IMAGE_BASE( DoubleImageType, image );

  CAST_IMAGE_BASE( ComplexInt16ImageType, image );
  CAST_IMAGE_BASE( ComplexInt32ImageType, image );
  CAST_IMAGE_BASE( ComplexFloatImageType, image );
  CAST_IMAGE_BASE( ComplexDoubleImageType, image );

  CAST_IMAGE_BASE( UInt8RGBImageType, image );
  CAST_IMAGE_BASE( UInt8RGBAImageType, image );

  itkExceptionMacro( "Unknown image-base type." );
}

template< typename TOutputImage,
	  typename TInputImage >
void
OutputImageParameter
::ClampAndWriteVectorImage( TInputImage * in )
{
  assert( !m_FileName.empty() );

  auto icif =
    ClampImageFilter< TInputImage, DoubleVectorImageType >::New();

  auto clampFilter =
    ClampImageFilter< DoubleVectorImageType , TOutputImage >::New();

  icif->SetInput( in );

  clampFilter->SetInput( icif->GetOutput() );

#ifdef OTB_USE_MPI

  otb::MPIConfig::Pointer mpiConfig = otb::MPIConfig::Instance();

  if( mpiConfig->GetNbProcs() > 1 )
    {
    std::string extension =
      itksys::SystemTools::GetFilenameExtension( m_Filename );

    if(extension == ".vrt")
      {
      // Use the MPIVrtWriter

      auto vrtWriter =
	otb::MPIVrtWriter< TOutputImage >::New();

      vrtWriter->SetInput( clampFilter->GetOutput() );
      vrtWriter->SetFileName( m_FileName );
      vrtWriter->SetAvailableRAM( m_RAMValue);

      // Change internal state only when everything has been setup
      // without raising exception.

      m_InputCaster = icif;
      m_OutputCaster = clampFilter;

      m_Writer = vrtWriter;

      return;
      }

#ifdef OTB_USE_SPTW

    else if (extension == ".tif")
      {
      // Use simple parallel tiff writer

      auto sptWriter =
	otb::SimpleParallelTiffWriter< TOutputImage >::New();

      sptWriter->SetFileName( m_FileName );
      sptWriter->SetInput( clampFilter->GetOutput() );
      sptWriter->GetStreamingManager()->SetDefaultRAM( m_RAMValue );

      // Change internal state only when everything has been setup
      // without raising exception.

      m_InputCaster = icif;
      m_OutputCaster = clampFilter;

      m_Writer = sptWriter;

      return;
      }

#endif // OTB_USE_SPTW

    else
      {
      itkGenericExceptionMacro(
	"File format "
	<< extension
	<< " not supported for parallel writing with MPI. Supported formats are "
	   ".vrt and .tif. Extended filenames are not supported."
      );
      }
    }

#endif // OTB_USE_MPI

  //
  // Use default OTB writer.

  auto writer = otb::ImageFileWriter< TOutputImage >::New();

  writer->SetFileName( m_FileName );
  writer->SetInput( clampFilter->GetOutput() );
  writer->GetStreamingManager()->SetDefaultRAM( m_RAMValue );

  // Change internal state only when everything has been setup
  // without raising exception.

  m_InputCaster = icif;
  m_OutputCaster = clampFilter;

  m_Writer = writer;
}

template< typename TInputImage >
void
OutputImageParameter
::SwitchInput( TInputImage * image )
{
  assert( image );

  switch( m_PixelType )
    {
    case ImagePixelType_uint8:
      ClampAndWriteVectorImage< UInt8VectorImageType >( image );
      break;

    case ImagePixelType_int16:
      ClampAndWriteVectorImage< Int16VectorImageType >( image );
      break;

    case ImagePixelType_uint16:
      ClampAndWriteVectorImage< UInt16VectorImageType >( image );
      break;

    case ImagePixelType_int32:

      ClampAndWriteVectorImage< Int32VectorImageType >( image );
      break;

    case ImagePixelType_uint32:
      ClampAndWriteVectorImage< UInt32VectorImageType >( image );
      break;

    case ImagePixelType_float:
      ClampAndWriteVectorImage< FloatVectorImageType >( image );
      break;

    case ImagePixelType_double:
      ClampAndWriteVectorImage< DoubleVectorImageType >( image );
      break;

    case ImagePixelType_cint16:
      ClampAndWriteVectorImage < ComplexInt16VectorImageType >( image );
      break;

    case ImagePixelType_cint32:
      ClampAndWriteVectorImage < ComplexInt32VectorImageType >( image );
      break;

    case ImagePixelType_cfloat:
      ClampAndWriteVectorImage< ComplexFloatVectorImageType >( image );
    break;

    case ImagePixelType_cdouble:
      ClampAndWriteVectorImage < ComplexDoubleVectorImageType >( image );
    break;

    default:
      assert( false && "Unexpected image-type." );
      break;
    }
}

void
OutputImageParameter::Write()
{
  m_Writer->Update();

  // Clean internal filters
  m_InputCaster = nullptr;
  m_OutputCaster = nullptr;

  m_Writer = nullptr;
}

itk::ProcessObject*
OutputImageParameter::GetWriter()
{
  return m_Writer;
}

ImageBaseType*
OutputImageParameter::GetValue( )
{
  return m_Image;
}

void
OutputImageParameter::SetValue(ImageBaseType* image)
{
  m_Image = image;
  SetActive(true);
}

bool
OutputImageParameter
::HasValue() const
{
  return !m_FileName.empty();
}

std::string
OutputImageParameter::CheckFileName(bool fixMissingExtension)
{
  std::string ret("");
  // Check that there is an ImageIO capable of writing the file
  otb::ExtendedFilenameToWriterOptions::Pointer filenameHelper =
    otb::ExtendedFilenameToWriterOptions::New();
  filenameHelper->SetExtendedFileName(this->GetFileName());
  std::string simpleFilename = filenameHelper->GetSimpleFileName();
  // TODO : check if simpleFilename is empty

  otb::ImageIOBase::Pointer imageIO =
    otb::ImageIOFactory::CreateImageIO(simpleFilename.c_str(),
                                       otb::ImageIOFactory::WriteMode);
  if(imageIO.IsNull())
    {
    // check for missing extension
    std::string outExt = itksys::SystemTools::GetFilenameLastExtension(simpleFilename);
    if (outExt.empty())
      {
      if (fixMissingExtension)
        {
        // try with .tif
        std::string fullFileName(this->GetFileName());
        std::string extendedPart = fullFileName.substr(simpleFilename.size());
        this->SetFileName(simpleFilename+std::string(".tif")+extendedPart);
        ret += std::string("no extension detected, using TIF as default.");
        }
      else
        {
        // TODO : call exception here?
        }
      }
    else
      {
      // TODO : call exception here?
      }
    }
  return ret;
}

// Specialization for UInt8RGBAImageType

template<>
void
OutputImageParameter
::SwitchInput( UInt8RGBAImageType * img )
{
  assert( img );

  if( m_PixelType != ImagePixelType_uint8 )
     itkExceptionMacro("Unknown PixelType for RGBA Image. Only uint8 is supported.");

  auto writer =
    otb::ImageFileWriter< UInt8RGBAImageType >::New();

  writer->SetFileName( GetFileName() );
  writer->SetInput( img );
  writer->GetStreamingManager()->SetDefaultRAM( m_RAMValue );

  m_Writer = writer;
}

// Specialization for UInt8RGBImageType
template<>
void
OutputImageParameter
::SwitchInput( UInt8RGBImageType * img )
{
  if( m_PixelType != ImagePixelType_uint8 )
    itkExceptionMacro("Unknown PixelType for RGB Image. Only uint8 is supported.");

  auto writer = otb::ImageFileWriter< UInt8RGBImageType >::New();

  writer->SetFileName( GetFileName() );
  writer->SetInput( img );
  writer->GetStreamingManager()->SetDefaultRAM( m_RAMValue );

  m_Writer = writer;
}

void OutputImageParameter::SetFileName (const char* filename)
{
  this->SetFileName(std::string(filename));
}

void OutputImageParameter::SetFileName (const std::string& filename)
{
  m_FileName = filename;
  SetActive(true);
}

}
}
<commit_msg>ENH: 1659: Fixed typo in identifier (conditional code).<commit_after>/*
 * Copyright (C) 2005-2019 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * 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 "otbWrapperOutputImageParameter.h"
#include "otbClampImageFilter.h"
#include "otbImageIOFactory.h"
#include "itksys/SystemTools.hxx"

#ifdef OTB_USE_MPI

#include "otbMPIConfig.h"
#include "otbMPIVrtWriter.h"

#ifdef OTB_USE_SPTW
#include "otbSimpleParallelTiffWriter.h"
#endif

#endif

#define CAST_IMAGE_BASE( T, image_base )		\
  {							\
    T * img = dynamic_cast< T * >( image_base );	\
							\
    if( img )						\
      SwitchInput< T >( img );				\
    							\
    return;						\
  }

namespace otb
{
namespace Wrapper
{

// Declare specialisation for UInt8RGBAImageType
template<>
void OutputImageParameter::SwitchInput(UInt8RGBAImageType * );

// Declare specialisation for UInt8RGBImageType
template <>
void OutputImageParameter::SwitchInput( UInt8RGBImageType * );

OutputImageParameter::OutputImageParameter()
  : m_PixelType(ImagePixelType_float),
    m_DefaultPixelType(ImagePixelType_float),
    m_RAMValue(0)
{
  SetName("Output Image");
  SetKey("out");
}

OutputImageParameter::~OutputImageParameter()
{
}

std::string OutputImageParameter::ConvertPixelTypeToString(ImagePixelType type)
{
  // TODO: Could be replaced by constant static string array e.g.:
  // return PIXEL_TYPE_NAME[ type ];

  std::string ret;
  switch(type)
    {
    case ImagePixelType_uint8:
      {
      ret = "uint8";
      break;
      }
    case ImagePixelType_int16:
      {
      ret = "int16";
      break;
      }
    case ImagePixelType_uint16:
      {
      ret = "uint16";
      break;
      }
    case ImagePixelType_int32:
      {
      ret = "int32";
      break;
      }
    case ImagePixelType_uint32:
      {
      ret = "uint32";
      break;
      }
    case ImagePixelType_float:
      {
      ret = "float";
      break;
      }
    case ImagePixelType_double:
      {
      ret = "double";
      break;
      }
    case ImagePixelType_cint16:
      {
      ret = "cint16";
      break;
      }
    case ImagePixelType_cint32:
      {
      ret = "cint32";
      break;
      }
    case ImagePixelType_cfloat:
      {
      ret = "cfloat";
      break;
      }
    case ImagePixelType_cdouble:
      {
      ret = "cdouble";
      break;
      }
    }
  return ret;
}

bool
OutputImageParameter::ConvertStringToPixelType(const std::string &value, ImagePixelType &type)
{
  // TODO: Could be replaced std::find_if() in constant static string
  // array (see ::ConvertPixelTypeToString().

  if (value == "uint8")
    type = ImagePixelType_uint8;
  else if (value == "int16")
    type = ImagePixelType_int16;
  else if (value == "uint16")
    type = ImagePixelType_uint16;
  else if (value == "int32")
    type = ImagePixelType_int32;
  else if (value == "uint32")
    type = ImagePixelType_uint32;
  else if (value == "float")
    type = ImagePixelType_float;
  else if (value == "double")
    type = ImagePixelType_double;
  else if (value == "cint16")
    type = ImagePixelType_cint16;
  else if (value == "cint32")
    type = ImagePixelType_cint32;
  else if (value == "cfloat")
    type = ImagePixelType_cfloat;
  else if (value == "cdouble")
    type = ImagePixelType_cdouble;
  else
    return false;
  return true;
}

void
OutputImageParameter
::InitializeWriters()
{
  ImageBaseType * image = m_Image.GetPointer();

  CAST_IMAGE_BASE( UInt8VectorImageType, image );
  CAST_IMAGE_BASE( Int16VectorImageType, image );
  CAST_IMAGE_BASE( UInt16VectorImageType, image );
  CAST_IMAGE_BASE( Int32VectorImageType, image );
  CAST_IMAGE_BASE( UInt32VectorImageType, image );

  CAST_IMAGE_BASE( FloatVectorImageType, image );
  CAST_IMAGE_BASE( DoubleVectorImageType, image );

  CAST_IMAGE_BASE( ComplexInt16VectorImageType, image );
  CAST_IMAGE_BASE( ComplexInt32VectorImageType, image );
  CAST_IMAGE_BASE( ComplexFloatVectorImageType, image );
  CAST_IMAGE_BASE( ComplexDoubleVectorImageType, image );

  CAST_IMAGE_BASE( UInt8ImageType, image );
  CAST_IMAGE_BASE( Int16ImageType, image );
  CAST_IMAGE_BASE( UInt16ImageType, image );
  CAST_IMAGE_BASE( Int32ImageType, image );
  CAST_IMAGE_BASE( UInt32ImageType, image );

  CAST_IMAGE_BASE( FloatImageType, image );
  CAST_IMAGE_BASE( DoubleImageType, image );

  CAST_IMAGE_BASE( ComplexInt16ImageType, image );
  CAST_IMAGE_BASE( ComplexInt32ImageType, image );
  CAST_IMAGE_BASE( ComplexFloatImageType, image );
  CAST_IMAGE_BASE( ComplexDoubleImageType, image );

  CAST_IMAGE_BASE( UInt8RGBImageType, image );
  CAST_IMAGE_BASE( UInt8RGBAImageType, image );

  itkExceptionMacro( "Unknown image-base type." );
}

template< typename TOutputImage,
	  typename TInputImage >
void
OutputImageParameter
::ClampAndWriteVectorImage( TInputImage * in )
{
  assert( !m_FileName.empty() );

  auto icif =
    ClampImageFilter< TInputImage, DoubleVectorImageType >::New();

  auto clampFilter =
    ClampImageFilter< DoubleVectorImageType , TOutputImage >::New();

  icif->SetInput( in );

  clampFilter->SetInput( icif->GetOutput() );

#ifdef OTB_USE_MPI

  otb::MPIConfig::Pointer mpiConfig = otb::MPIConfig::Instance();

  if( mpiConfig->GetNbProcs() > 1 )
    {
    std::string extension =
      itksys::SystemTools::GetFilenameExtension( m_FileName );

    if(extension == ".vrt")
      {
      // Use the MPIVrtWriter

      auto vrtWriter =
	otb::MPIVrtWriter< TOutputImage >::New();

      vrtWriter->SetInput( clampFilter->GetOutput() );
      vrtWriter->SetFileName( m_FileName );
      vrtWriter->SetAvailableRAM( m_RAMValue);

      // Change internal state only when everything has been setup
      // without raising exception.

      m_InputCaster = icif;
      m_OutputCaster = clampFilter;

      m_Writer = vrtWriter;

      return;
      }

#ifdef OTB_USE_SPTW

    else if (extension == ".tif")
      {
      // Use simple parallel tiff writer

      auto sptWriter =
	otb::SimpleParallelTiffWriter< TOutputImage >::New();

      sptWriter->SetFileName( m_FileName );
      sptWriter->SetInput( clampFilter->GetOutput() );
      sptWriter->GetStreamingManager()->SetDefaultRAM( m_RAMValue );

      // Change internal state only when everything has been setup
      // without raising exception.

      m_InputCaster = icif;
      m_OutputCaster = clampFilter;

      m_Writer = sptWriter;

      return;
      }

#endif // OTB_USE_SPTW

    else
      {
      itkGenericExceptionMacro(
	"File format "
	<< extension
	<< " not supported for parallel writing with MPI. Supported formats are "
	   ".vrt and .tif. Extended filenames are not supported."
      );
      }
    }

#endif // OTB_USE_MPI

  //
  // Use default OTB writer.

  auto writer = otb::ImageFileWriter< TOutputImage >::New();

  writer->SetFileName( m_FileName );
  writer->SetInput( clampFilter->GetOutput() );
  writer->GetStreamingManager()->SetDefaultRAM( m_RAMValue );

  // Change internal state only when everything has been setup
  // without raising exception.

  m_InputCaster = icif;
  m_OutputCaster = clampFilter;

  m_Writer = writer;
}

template< typename TInputImage >
void
OutputImageParameter
::SwitchInput( TInputImage * image )
{
  assert( image );

  switch( m_PixelType )
    {
    case ImagePixelType_uint8:
      ClampAndWriteVectorImage< UInt8VectorImageType >( image );
      break;

    case ImagePixelType_int16:
      ClampAndWriteVectorImage< Int16VectorImageType >( image );
      break;

    case ImagePixelType_uint16:
      ClampAndWriteVectorImage< UInt16VectorImageType >( image );
      break;

    case ImagePixelType_int32:

      ClampAndWriteVectorImage< Int32VectorImageType >( image );
      break;

    case ImagePixelType_uint32:
      ClampAndWriteVectorImage< UInt32VectorImageType >( image );
      break;

    case ImagePixelType_float:
      ClampAndWriteVectorImage< FloatVectorImageType >( image );
      break;

    case ImagePixelType_double:
      ClampAndWriteVectorImage< DoubleVectorImageType >( image );
      break;

    case ImagePixelType_cint16:
      ClampAndWriteVectorImage < ComplexInt16VectorImageType >( image );
      break;

    case ImagePixelType_cint32:
      ClampAndWriteVectorImage < ComplexInt32VectorImageType >( image );
      break;

    case ImagePixelType_cfloat:
      ClampAndWriteVectorImage< ComplexFloatVectorImageType >( image );
    break;

    case ImagePixelType_cdouble:
      ClampAndWriteVectorImage < ComplexDoubleVectorImageType >( image );
    break;

    default:
      assert( false && "Unexpected image-type." );
      break;
    }
}

void
OutputImageParameter::Write()
{
  m_Writer->Update();

  // Clean internal filters
  m_InputCaster = nullptr;
  m_OutputCaster = nullptr;

  m_Writer = nullptr;
}

itk::ProcessObject*
OutputImageParameter::GetWriter()
{
  return m_Writer;
}

ImageBaseType*
OutputImageParameter::GetValue( )
{
  return m_Image;
}

void
OutputImageParameter::SetValue(ImageBaseType* image)
{
  m_Image = image;
  SetActive(true);
}

bool
OutputImageParameter
::HasValue() const
{
  return !m_FileName.empty();
}

std::string
OutputImageParameter::CheckFileName(bool fixMissingExtension)
{
  std::string ret("");
  // Check that there is an ImageIO capable of writing the file
  otb::ExtendedFilenameToWriterOptions::Pointer filenameHelper =
    otb::ExtendedFilenameToWriterOptions::New();
  filenameHelper->SetExtendedFileName(this->GetFileName());
  std::string simpleFilename = filenameHelper->GetSimpleFileName();
  // TODO : check if simpleFilename is empty

  otb::ImageIOBase::Pointer imageIO =
    otb::ImageIOFactory::CreateImageIO(simpleFilename.c_str(),
                                       otb::ImageIOFactory::WriteMode);
  if(imageIO.IsNull())
    {
    // check for missing extension
    std::string outExt = itksys::SystemTools::GetFilenameLastExtension(simpleFilename);
    if (outExt.empty())
      {
      if (fixMissingExtension)
        {
        // try with .tif
        std::string fullFileName(this->GetFileName());
        std::string extendedPart = fullFileName.substr(simpleFilename.size());
        this->SetFileName(simpleFilename+std::string(".tif")+extendedPart);
        ret += std::string("no extension detected, using TIF as default.");
        }
      else
        {
        // TODO : call exception here?
        }
      }
    else
      {
      // TODO : call exception here?
      }
    }
  return ret;
}

// Specialization for UInt8RGBAImageType

template<>
void
OutputImageParameter
::SwitchInput( UInt8RGBAImageType * img )
{
  assert( img );

  if( m_PixelType != ImagePixelType_uint8 )
     itkExceptionMacro("Unknown PixelType for RGBA Image. Only uint8 is supported.");

  auto writer =
    otb::ImageFileWriter< UInt8RGBAImageType >::New();

  writer->SetFileName( GetFileName() );
  writer->SetInput( img );
  writer->GetStreamingManager()->SetDefaultRAM( m_RAMValue );

  m_Writer = writer;
}

// Specialization for UInt8RGBImageType
template<>
void
OutputImageParameter
::SwitchInput( UInt8RGBImageType * img )
{
  if( m_PixelType != ImagePixelType_uint8 )
    itkExceptionMacro("Unknown PixelType for RGB Image. Only uint8 is supported.");

  auto writer = otb::ImageFileWriter< UInt8RGBImageType >::New();

  writer->SetFileName( GetFileName() );
  writer->SetInput( img );
  writer->GetStreamingManager()->SetDefaultRAM( m_RAMValue );

  m_Writer = writer;
}

void OutputImageParameter::SetFileName (const char* filename)
{
  this->SetFileName(std::string(filename));
}

void OutputImageParameter::SetFileName (const std::string& filename)
{
  m_FileName = filename;
  SetActive(true);
}

}
}
<|endoftext|>


<commit_before>//
// Copyright (c) 2015 The ANGLE Project Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
//

#include "Matrix.h"
#include "SampleApplication.h"
#include "random_utils.h"
#include "shader_utils.h"

#include <vector>
#include <algorithm>

class MultiWindowSample : public SampleApplication
{
  public:
    MultiWindowSample()
        : SampleApplication("MultiWindow", 256, 256)
    {
    }

    virtual bool initialize()
    {
        const std::string vs = SHADER_SOURCE
        (
            attribute vec4 vPosition;
            void main()
            {
                gl_Position = vPosition;
            }
        );

        const std::string fs = SHADER_SOURCE
        (
            precision mediump float;
            void main()
            {
                gl_FragColor = vec4(1.0, 0.0, 0.0, 1.0);
            }
        );

        mProgram = CompileProgram(vs, fs);
        if (!mProgram)
        {
            return false;
        }

        // Set an initial rotation
        mRotation = 45.0f;

        glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

        window rootWindow;
        rootWindow.osWindow = getWindow();
        rootWindow.surface = getSurface();
        mWindows.push_back(rootWindow);

        const size_t numWindows = 5;
        for (size_t i = 1; i < numWindows; i++)
        {
            window window;

            window.osWindow = CreateOSWindow();
            if (!window.osWindow->initialize("MultiWindow", 256, 256))
            {
                return false;
            }

            window.surface = eglCreateWindowSurface(getDisplay(), getConfig(), window.osWindow->getNativeWindow(), nullptr);
            if (window.surface == EGL_NO_SURFACE)
            {
                return false;
            }

            window.osWindow->setVisible(true);

            mWindows.push_back(window);
        }

        for (size_t i = 1; i < mWindows.size(); i++)
        {
            int x = rootWindow.osWindow->getX() + static_cast<int>(RandomBetween(0, 512));
            int y = rootWindow.osWindow->getY() + static_cast<int>(RandomBetween(0, 512));
            int width = static_cast<int>(RandomBetween(128, 512));
            int height = static_cast<int>(RandomBetween(128, 512));
            mWindows[i].osWindow->setPosition(x, y);
            mWindows[i].osWindow->resize(width, height);
        }

        return true;
    }

    virtual void destroy()
    {
        glDeleteProgram(mProgram);
    }

    virtual void step(float dt, double totalTime)
    {
        mRotation = fmod(mRotation + (dt * 40.0f), 360.0f);
    }

    virtual void draw()
    {
        OSWindow* rootWindow = mWindows[0].osWindow;
        int left = rootWindow->getX();
        int right = rootWindow->getX() + rootWindow->getWidth();
        int top = rootWindow->getY();
        int bottom = rootWindow->getY() + rootWindow->getHeight();
        for (size_t i = 1; i < mWindows.size(); i++)
        {
            OSWindow* window = mWindows[i].osWindow;
            left = std::min(left, window->getX());
            right = std::max(right, window->getX() + window->getWidth());
            top = std::min(top, window->getY());
            bottom = std::max(bottom, window->getY() + window->getHeight());
        }

        float midX = (left + right) * 0.5f;
        float midY = (top + bottom) * 0.5f;

        Matrix4 modelMatrix = Matrix4::translate(Vector3(midX, midY, 0.0f)) *
                              Matrix4::rotate(mRotation, Vector3(0.0f, 0.0f, 1.0f)) *
                              Matrix4::translate(Vector3(-midX, -midY, 0.0f));
        Matrix4 viewMatrix = Matrix4::identity();

        for (size_t i = 0; i < mWindows.size(); i++)
        {
            OSWindow* window = mWindows[i].osWindow;
            EGLSurface surface = mWindows[i].surface;

            eglMakeCurrent(getDisplay(), surface, surface, getContext());

            Matrix4 orthoMatrix = Matrix4::ortho(static_cast<float>(window->getX()), static_cast<float>(window->getX() + window->getWidth()),
                                                 static_cast<float>(window->getY() + window->getHeight()), static_cast<float>(window->getY()),
                                                 0.0f, 1.0f);
            Matrix4 mvpMatrix = orthoMatrix * viewMatrix * modelMatrix;

            Vector3 vertices[] =
            {
                Matrix4::transform(mvpMatrix, Vector4(midX, static_cast<float>(top), 0.0f, 1.0f)),
                Matrix4::transform(mvpMatrix, Vector4(static_cast<float>(left), static_cast<float>(bottom), 0.0f, 1.0f)),
                Matrix4::transform(mvpMatrix, Vector4(static_cast<float>(right), static_cast<float>(bottom), 0.0f, 1.0f)),
            };

            // Set the viewport
            glViewport(0, 0, window->getWidth(), window->getHeight());

            // Clear the color buffer
            glClear(GL_COLOR_BUFFER_BIT);

            // Use the program object
            glUseProgram(mProgram);

            // Load the vertex data
            glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, vertices[0].data);
            glEnableVertexAttribArray(0);

            glDrawArrays(GL_TRIANGLES, 0, 3);

            eglSwapBuffers(getDisplay(), surface);
        }
    }

  private:
    // Handle to a program object
    GLuint mProgram;

    // Current rotation
    float mRotation;

    // Window and surface data
    struct window
    {
        OSWindow* osWindow;
        EGLSurface surface;
    };
    std::vector<window> mWindows;
};

int main(int argc, char **argv)
{
    MultiWindowSample app;
    return app.run();
}
<commit_msg>Poll all windows in the multi window sample<commit_after>//
// Copyright (c) 2015 The ANGLE Project Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
//

#include "Matrix.h"
#include "SampleApplication.h"
#include "random_utils.h"
#include "shader_utils.h"

#include <vector>
#include <algorithm>

class MultiWindowSample : public SampleApplication
{
  public:
    MultiWindowSample()
        : SampleApplication("MultiWindow", 256, 256)
    {
    }

    virtual bool initialize()
    {
        const std::string vs = SHADER_SOURCE
        (
            attribute vec4 vPosition;
            void main()
            {
                gl_Position = vPosition;
            }
        );

        const std::string fs = SHADER_SOURCE
        (
            precision mediump float;
            void main()
            {
                gl_FragColor = vec4(1.0, 0.0, 0.0, 1.0);
            }
        );

        mProgram = CompileProgram(vs, fs);
        if (!mProgram)
        {
            return false;
        }

        // Set an initial rotation
        mRotation = 45.0f;

        glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

        window rootWindow;
        rootWindow.osWindow = getWindow();
        rootWindow.surface = getSurface();
        mWindows.push_back(rootWindow);

        const size_t numWindows = 5;
        for (size_t i = 1; i < numWindows; i++)
        {
            window window;

            window.osWindow = CreateOSWindow();
            if (!window.osWindow->initialize("MultiWindow", 256, 256))
            {
                return false;
            }

            window.surface = eglCreateWindowSurface(getDisplay(), getConfig(), window.osWindow->getNativeWindow(), nullptr);
            if (window.surface == EGL_NO_SURFACE)
            {
                return false;
            }

            window.osWindow->setVisible(true);

            mWindows.push_back(window);
        }

        for (size_t i = 1; i < mWindows.size(); i++)
        {
            int x = rootWindow.osWindow->getX() + static_cast<int>(RandomBetween(0, 512));
            int y = rootWindow.osWindow->getY() + static_cast<int>(RandomBetween(0, 512));
            int width = static_cast<int>(RandomBetween(128, 512));
            int height = static_cast<int>(RandomBetween(128, 512));
            mWindows[i].osWindow->setPosition(x, y);
            mWindows[i].osWindow->resize(width, height);
        }

        return true;
    }

    virtual void destroy()
    {
        glDeleteProgram(mProgram);
    }

    virtual void step(float dt, double totalTime)
    {
        mRotation = fmod(mRotation + (dt * 40.0f), 360.0f);

        for (size_t i = 1; i < mWindows.size(); i++)
        {
            mWindows[i].osWindow->messageLoop();
        }
    }

    virtual void draw()
    {
        OSWindow* rootWindow = mWindows[0].osWindow;
        int left = rootWindow->getX();
        int right = rootWindow->getX() + rootWindow->getWidth();
        int top = rootWindow->getY();
        int bottom = rootWindow->getY() + rootWindow->getHeight();
        for (size_t i = 1; i < mWindows.size(); i++)
        {
            OSWindow* window = mWindows[i].osWindow;
            left = std::min(left, window->getX());
            right = std::max(right, window->getX() + window->getWidth());
            top = std::min(top, window->getY());
            bottom = std::max(bottom, window->getY() + window->getHeight());
        }

        float midX = (left + right) * 0.5f;
        float midY = (top + bottom) * 0.5f;

        Matrix4 modelMatrix = Matrix4::translate(Vector3(midX, midY, 0.0f)) *
                              Matrix4::rotate(mRotation, Vector3(0.0f, 0.0f, 1.0f)) *
                              Matrix4::translate(Vector3(-midX, -midY, 0.0f));
        Matrix4 viewMatrix = Matrix4::identity();

        for (size_t i = 0; i < mWindows.size(); i++)
        {
            OSWindow* window = mWindows[i].osWindow;
            EGLSurface surface = mWindows[i].surface;

            eglMakeCurrent(getDisplay(), surface, surface, getContext());

            Matrix4 orthoMatrix = Matrix4::ortho(static_cast<float>(window->getX()), static_cast<float>(window->getX() + window->getWidth()),
                                                 static_cast<float>(window->getY() + window->getHeight()), static_cast<float>(window->getY()),
                                                 0.0f, 1.0f);
            Matrix4 mvpMatrix = orthoMatrix * viewMatrix * modelMatrix;

            Vector3 vertices[] =
            {
                Matrix4::transform(mvpMatrix, Vector4(midX, static_cast<float>(top), 0.0f, 1.0f)),
                Matrix4::transform(mvpMatrix, Vector4(static_cast<float>(left), static_cast<float>(bottom), 0.0f, 1.0f)),
                Matrix4::transform(mvpMatrix, Vector4(static_cast<float>(right), static_cast<float>(bottom), 0.0f, 1.0f)),
            };

            // Set the viewport
            glViewport(0, 0, window->getWidth(), window->getHeight());

            // Clear the color buffer
            glClear(GL_COLOR_BUFFER_BIT);

            // Use the program object
            glUseProgram(mProgram);

            // Load the vertex data
            glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, vertices[0].data);
            glEnableVertexAttribArray(0);

            glDrawArrays(GL_TRIANGLES, 0, 3);

            eglSwapBuffers(getDisplay(), surface);
        }
    }

  private:
    // Handle to a program object
    GLuint mProgram;

    // Current rotation
    float mRotation;

    // Window and surface data
    struct window
    {
        OSWindow* osWindow;
        EGLSurface surface;
    };
    std::vector<window> mWindows;
};

int main(int argc, char **argv)
{
    MultiWindowSample app;
    return app.run();
}
<|endoftext|>


<commit_before>/*
Copyright (C) 2001-present by Serge Lamikhov-Center

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

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

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

#ifndef ELFIO_UTILS_HPP
#define ELFIO_UTILS_HPP

#include <cstdint>
#include <ostream>

#define ELFIO_GET_ACCESS_DECL( TYPE, NAME ) virtual TYPE get_##NAME() const = 0

#define ELFIO_SET_ACCESS_DECL( TYPE, NAME ) \
    virtual void set_##NAME( TYPE value ) = 0

#define ELFIO_GET_SET_ACCESS_DECL( TYPE, NAME ) \
    virtual TYPE get_##NAME() const       = 0;  \
    virtual void set_##NAME( TYPE value ) = 0

#define ELFIO_GET_ACCESS( TYPE, NAME, FIELD ) \
    TYPE get_##NAME() const override { return ( *convertor )( FIELD ); }

#define ELFIO_SET_ACCESS( TYPE, NAME, FIELD ) \
    void set_##NAME( TYPE value ) override    \
    {                                         \
        FIELD = value;                        \
        FIELD = ( *convertor )( FIELD );      \
    }
#define ELFIO_GET_SET_ACCESS( TYPE, NAME, FIELD )                        \
    TYPE get_##NAME() const override { return ( *convertor )( FIELD ); } \
    void set_##NAME( TYPE value ) override                               \
    {                                                                    \
        FIELD = value;                                                   \
        FIELD = ( *convertor )( FIELD );                                 \
    }

namespace ELFIO {

//------------------------------------------------------------------------------
class endianess_convertor
{
  public:
    //------------------------------------------------------------------------------
    endianess_convertor() { need_conversion = false; }

    //------------------------------------------------------------------------------
    void setup( unsigned char elf_file_encoding )
    {
        need_conversion = ( elf_file_encoding != get_host_encoding() );
    }

    //------------------------------------------------------------------------------
    uint64_t operator()( uint64_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        value = ( ( value & 0x00000000000000FFull ) << 56 ) |
                ( ( value & 0x000000000000FF00ull ) << 40 ) |
                ( ( value & 0x0000000000FF0000ull ) << 24 ) |
                ( ( value & 0x00000000FF000000ull ) << 8 ) |
                ( ( value & 0x000000FF00000000ull ) >> 8 ) |
                ( ( value & 0x0000FF0000000000ull ) >> 24 ) |
                ( ( value & 0x00FF000000000000ull ) >> 40 ) |
                ( ( value & 0xFF00000000000000ull ) >> 56 );

        return value;
    }

    //------------------------------------------------------------------------------
    int64_t operator()( int64_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        return (int64_t)(*this)( (uint64_t)value );
    }

    //------------------------------------------------------------------------------
    uint32_t operator()( uint32_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        value =
            ( ( value & 0x000000FF ) << 24 ) | ( ( value & 0x0000FF00 ) << 8 ) |
            ( ( value & 0x00FF0000 ) >> 8 ) | ( ( value & 0xFF000000 ) >> 24 );

        return value;
    }

    //------------------------------------------------------------------------------
    int32_t operator()( int32_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        return (int32_t)(*this)( (uint32_t)value );
    }

    //------------------------------------------------------------------------------
    uint16_t operator()( uint16_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        value = ( ( value & 0x00FF ) << 8 ) | ( ( value & 0xFF00 ) >> 8 );

        return value;
    }

    //------------------------------------------------------------------------------
    int16_t operator()( int16_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        return (int16_t)(*this)( (uint16_t)value );
    }

    //------------------------------------------------------------------------------
    int8_t operator()( int8_t value ) const { return value; }

    //------------------------------------------------------------------------------
    uint8_t operator()( uint8_t value ) const { return value; }

    //------------------------------------------------------------------------------
  private:
    //------------------------------------------------------------------------------
    unsigned char get_host_encoding() const
    {
        static const int tmp = 1;
        if ( 1 == *reinterpret_cast<const char*>( &tmp ) ) {
            return ELFDATA2LSB;
        }
        else {
            return ELFDATA2MSB;
        }
    }

    //------------------------------------------------------------------------------
    bool need_conversion;
};

//------------------------------------------------------------------------------
struct address_translation
{
    address_translation( uint64_t start, uint64_t end, uint64_t map_to )
        : start( start ), end( end ), map_to( map_to ){};
    std::streampos start;
    std::streampos end;
    std::streampos map_to;
};

//------------------------------------------------------------------------------
class address_translator
{
  public:
    //------------------------------------------------------------------------------
    void set_address_translation( std::vector<address_translation>& addr_trans )
    {
        translation = addr_trans;
    }

    //------------------------------------------------------------------------------
    std::streampos operator()( std::streampos value ) const
    {
        if ( translation.empty() ) {
            return value;
        }

        for ( auto& t : translation ) {
            if ( t.map_to <= value &&
                 ( ( value - t.map_to ) < ( t.end - t.start ) ) ) {
                std::cout << std::hex << t.start - t.map_to + value << " "
                          << value << std::endl;
                return t.start - t.map_to + value;
            }
        }

        return value;
    }

    bool empty() const { return translation.empty(); }

  private:
    std::vector<address_translation> translation;
};

//------------------------------------------------------------------------------
inline uint32_t elf_hash( const unsigned char* name )
{
    uint32_t h = 0, g = 0;
    while ( *name != '\0' ) {
        h = ( h << 4 ) + *name++;
        g = h & 0xf0000000;
        if ( g != 0 )
            h ^= g >> 24;
        h &= ~g;
    }
    return h;
}

//------------------------------------------------------------------------------
inline uint32_t elf_gnu_hash( const unsigned char* s )
{
    uint32_t h = 0x1505;
    for ( unsigned char c = *s; c != '\0'; c = *++s )
        h = ( h << 5 ) + h + c;
    return h;
}

//------------------------------------------------------------------------------
inline std::string to_hex_string( uint64_t value )
{
    std::string str;

    while ( value ) {
        auto digit = value & 0xF;
        if ( digit < 0xA ) {
            str = char( '0' + digit ) + str;
        }
        else {
            str = char( 'A' + digit - 0xA ) + str;
        }
        value >>= 4;
    }

    return "0x" + str;
}

//------------------------------------------------------------------------------
inline void adjust_stream_size( std::ostream& stream, std::streamsize offset )
{
    stream.seekp( 0, std::ios_base::end );
    if ( stream.tellp() < offset ) {
        std::streamsize size = offset - stream.tellp();
        stream.write( std::string( size, '\0' ).c_str(), size );
    }
    stream.seekp( offset );
}

} // namespace ELFIO

#endif // ELFIO_UTILS_HPP
<commit_msg>Remove debug output<commit_after>/*
Copyright (C) 2001-present by Serge Lamikhov-Center

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

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

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

#ifndef ELFIO_UTILS_HPP
#define ELFIO_UTILS_HPP

#include <cstdint>
#include <ostream>

#define ELFIO_GET_ACCESS_DECL( TYPE, NAME ) virtual TYPE get_##NAME() const = 0

#define ELFIO_SET_ACCESS_DECL( TYPE, NAME ) \
    virtual void set_##NAME( TYPE value ) = 0

#define ELFIO_GET_SET_ACCESS_DECL( TYPE, NAME ) \
    virtual TYPE get_##NAME() const       = 0;  \
    virtual void set_##NAME( TYPE value ) = 0

#define ELFIO_GET_ACCESS( TYPE, NAME, FIELD ) \
    TYPE get_##NAME() const override { return ( *convertor )( FIELD ); }

#define ELFIO_SET_ACCESS( TYPE, NAME, FIELD ) \
    void set_##NAME( TYPE value ) override    \
    {                                         \
        FIELD = value;                        \
        FIELD = ( *convertor )( FIELD );      \
    }
#define ELFIO_GET_SET_ACCESS( TYPE, NAME, FIELD )                        \
    TYPE get_##NAME() const override { return ( *convertor )( FIELD ); } \
    void set_##NAME( TYPE value ) override                               \
    {                                                                    \
        FIELD = value;                                                   \
        FIELD = ( *convertor )( FIELD );                                 \
    }

namespace ELFIO {

//------------------------------------------------------------------------------
class endianess_convertor
{
  public:
    //------------------------------------------------------------------------------
    endianess_convertor() { need_conversion = false; }

    //------------------------------------------------------------------------------
    void setup( unsigned char elf_file_encoding )
    {
        need_conversion = ( elf_file_encoding != get_host_encoding() );
    }

    //------------------------------------------------------------------------------
    uint64_t operator()( uint64_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        value = ( ( value & 0x00000000000000FFull ) << 56 ) |
                ( ( value & 0x000000000000FF00ull ) << 40 ) |
                ( ( value & 0x0000000000FF0000ull ) << 24 ) |
                ( ( value & 0x00000000FF000000ull ) << 8 ) |
                ( ( value & 0x000000FF00000000ull ) >> 8 ) |
                ( ( value & 0x0000FF0000000000ull ) >> 24 ) |
                ( ( value & 0x00FF000000000000ull ) >> 40 ) |
                ( ( value & 0xFF00000000000000ull ) >> 56 );

        return value;
    }

    //------------------------------------------------------------------------------
    int64_t operator()( int64_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        return (int64_t)(*this)( (uint64_t)value );
    }

    //------------------------------------------------------------------------------
    uint32_t operator()( uint32_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        value =
            ( ( value & 0x000000FF ) << 24 ) | ( ( value & 0x0000FF00 ) << 8 ) |
            ( ( value & 0x00FF0000 ) >> 8 ) | ( ( value & 0xFF000000 ) >> 24 );

        return value;
    }

    //------------------------------------------------------------------------------
    int32_t operator()( int32_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        return (int32_t)(*this)( (uint32_t)value );
    }

    //------------------------------------------------------------------------------
    uint16_t operator()( uint16_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        value = ( ( value & 0x00FF ) << 8 ) | ( ( value & 0xFF00 ) >> 8 );

        return value;
    }

    //------------------------------------------------------------------------------
    int16_t operator()( int16_t value ) const
    {
        if ( !need_conversion ) {
            return value;
        }
        return (int16_t)(*this)( (uint16_t)value );
    }

    //------------------------------------------------------------------------------
    int8_t operator()( int8_t value ) const { return value; }

    //------------------------------------------------------------------------------
    uint8_t operator()( uint8_t value ) const { return value; }

    //------------------------------------------------------------------------------
  private:
    //------------------------------------------------------------------------------
    unsigned char get_host_encoding() const
    {
        static const int tmp = 1;
        if ( 1 == *reinterpret_cast<const char*>( &tmp ) ) {
            return ELFDATA2LSB;
        }
        else {
            return ELFDATA2MSB;
        }
    }

    //------------------------------------------------------------------------------
    bool need_conversion;
};

//------------------------------------------------------------------------------
struct address_translation
{
    address_translation( uint64_t start, uint64_t end, uint64_t map_to )
        : start( start ), end( end ), map_to( map_to ){};
    std::streampos start;
    std::streampos end;
    std::streampos map_to;
};

//------------------------------------------------------------------------------
class address_translator
{
  public:
    //------------------------------------------------------------------------------
    void set_address_translation( std::vector<address_translation>& addr_trans )
    {
        translation = addr_trans;
    }

    //------------------------------------------------------------------------------
    std::streampos operator()( std::streampos value ) const
    {
        if ( translation.empty() ) {
            return value;
        }

        for ( auto& t : translation ) {
            if ( t.map_to <= value &&
                 ( ( value - t.map_to ) < ( t.end - t.start ) ) ) {
                return t.start - t.map_to + value;
            }
        }

        return value;
    }

    bool empty() const { return translation.empty(); }

  private:
    std::vector<address_translation> translation;
};

//------------------------------------------------------------------------------
inline uint32_t elf_hash( const unsigned char* name )
{
    uint32_t h = 0, g = 0;
    while ( *name != '\0' ) {
        h = ( h << 4 ) + *name++;
        g = h & 0xf0000000;
        if ( g != 0 )
            h ^= g >> 24;
        h &= ~g;
    }
    return h;
}

//------------------------------------------------------------------------------
inline uint32_t elf_gnu_hash( const unsigned char* s )
{
    uint32_t h = 0x1505;
    for ( unsigned char c = *s; c != '\0'; c = *++s )
        h = ( h << 5 ) + h + c;
    return h;
}

//------------------------------------------------------------------------------
inline std::string to_hex_string( uint64_t value )
{
    std::string str;

    while ( value ) {
        auto digit = value & 0xF;
        if ( digit < 0xA ) {
            str = char( '0' + digit ) + str;
        }
        else {
            str = char( 'A' + digit - 0xA ) + str;
        }
        value >>= 4;
    }

    return "0x" + str;
}

//------------------------------------------------------------------------------
inline void adjust_stream_size( std::ostream& stream, std::streamsize offset )
{
    stream.seekp( 0, std::ios_base::end );
    if ( stream.tellp() < offset ) {
        std::streamsize size = offset - stream.tellp();
        stream.write( std::string( size, '\0' ).c_str(), size );
    }
    stream.seekp( offset );
}

} // namespace ELFIO

#endif // ELFIO_UTILS_HPP
<|endoftext|>


<commit_before><commit_msg>INTEGRATION: CWS changefileheader (1.20.216); FILE MERGED 2008/04/01 15:09:24 thb 1.20.216.3: #i85898# Stripping all external header guards 2008/04/01 10:53:48 thb 1.20.216.2: #i85898# Stripping all external header guards 2008/03/28 15:24:40 rt 1.20.216.1: #i87441# Change license header to LPGL v3.<commit_after><|endoftext|>


<commit_before>/**
 * @file PathPlanner.cpp
 *
 * @author Yigit Can Akcay
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"

PathPlanner::PathPlanner()
:
step_buffer({}),
foot_to_use(Foot::RIGHT),
last_stepRequestID(getMotionStatus().stepControl.stepRequestID + 1),      // WalkRequest stepRequestID starts at 0, we have to start at 1
kick_planned(false)
{
  DEBUG_REQUEST_REGISTER("PathPlanner:walk_to_ball", "Walks to the ball from far.", false);

  getDebugParameterList().add(&params);
}

PathPlanner::~PathPlanner()
{
  getDebugParameterList().remove(&params);
}

void PathPlanner::execute()
{
  // --- DEBUG REQUESTS ---
  DEBUG_REQUEST("PathPlanner:walk_to_ball",
                if (getBallModel().positionPreview.x > 250)
                {
                  walk_to_ball(Foot::NONE);
                }
                );
  // --- DEBUG REQUESTS ---

  getPathModel().kick_executed = false;

  STOPWATCH_START("PathPlanner");
  // Always executed first
  manage_step_buffer();

  // The kick has been executed
  // Tells XABSL to jump into next state
  if (kick_planned && step_buffer.empty()) {
    getPathModel().kick_executed = true;
  }

  switch (getPathModel().path_routine)
  {
  case PathModel::PathRoutine::NONE:
    // There is no kick planned, since the kick has been executed
    // and XABSL is in a different state now
    if (kick_planned) {
      kick_planned = false;
    }

    if (step_buffer.empty()) {
      return;
    }
    break;
  case PathModel::PathRoutine::GO_TO_BALL_FAST:
    walk_to_ball(Foot::NONE , true);
    break;
  case PathModel::PathRoutine::GO_TO_BALL_SLOW:
    walk_to_ball(Foot::NONE);
    break;
  case PathModel::PathRoutine::MOVE_AROUND_BALL:
    move_around_ball(getPathModel().direction, getPathModel().radius);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_LEFT:
    approach_ball(Foot::LEFT);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_RIGHT:
    approach_ball(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_LEFT:
    short_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_RIGHT:
    short_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::LONG_KICK_LEFT:
    long_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::LONG_KICK_RIGHT:
    long_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SIDEKICK_LEFT:
    sidekick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SIDEKICK_RIGHT:
    sidekick(Foot::RIGHT);
    break;
  }

  // Always executed last
  execute_step_buffer();

  STOPWATCH_STOP("PathPlanner");
}

void PathPlanner::walk_to_ball(const Foot foot, const bool go_fast)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ballPos    = getBallModel().positionPreview;
      coordinate = WalkRequest::Hip;
      break;
  }
  double ballRotation = ballPos.angle();

  Pose2D pose           = { ballRotation, 0.7*(ballPos.x - getPathModel().distance), ballPos.y };

  if (step_buffer.empty())
  {
    StepType type         = StepType::WALKSTEP;
    double scale          = 1.0;
    double speed_direction = 0.0;

    if (go_fast)
    {
      double character = 1.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
    else
    {
      double character = 0.3;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::move_around_ball(const double direction, const double radius)
{
  Vector2d ballPos                   = getBallModel().positionPreview;
  double ballRotation                = ballPos.angle();
  double ballDistance                = ballPos.abs();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;

  double min1;
  double min2;
  double max1;
  double max2;
  if (direction <= 0)
  {
    min1 = 0.0;
    min2 = 0.0;
    max1 = 45.0;
    max2 = 100.0;
  }
  else {
    min1 = -45;
    min2 = -100;
    max1 = 0;
    max2 = 0;
  }

  double stepX = (ballDistance - radius) * std::cos(ballRotation);
  double stepY = Math::clamp(radius * std::tan(Math::clamp(Math::toDegrees(-direction), min1, max1)), min2, max2) * std::cos(ballRotation);

  Pose2D pose           = { ballRotation, stepX, stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    Foot foot              = Foot::NONE;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::approach_ball(const Foot foot)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  double stepX                       = 0.0;
  double stepY                       = 0.0;
  double ballRadius                  = getFieldInfo().ballRadius;
  double stepRotation                = ballPos.abs() > 250 ? ballPos.angle() : 0;

  switch (foot) {
  case Foot::LEFT:
    ballPos    = getBallModel().positionPreviewInLFoot;
    coordinate = WalkRequest::LFoot;
    stepX      = ballPos.x - std::abs(ballPos.y - getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y - getPathModel().yOffset;
    break;
  case Foot::RIGHT:
    ballPos    = getBallModel().positionPreviewInRFoot;
    coordinate = WalkRequest::RFoot;
    stepX      = ballPos.x - std::abs(ballPos.y + getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y + getPathModel().yOffset;
    break;
  case Foot::NONE:
    ASSERT(false);
  }

  Pose2D pose = { stepRotation, 0.7 * stepX, 0.7 * stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, Foot::NONE, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::short_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500 , 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::long_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500, 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::sidekick(const Foot foot)
{
  if (!kick_planned)
  {
    double speed_direction             = 0.0;
    double stepY                       = 0.0;
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate     = WalkRequest::LFoot;
      speed_direction = 90;
      stepY          = 100;
      break;
    case Foot::RIGHT:
      coordinate     = WalkRequest::RFoot;
      speed_direction = -90;
      stepY          = -100;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, 500, stepY };
      StepType type = StepType::KICKSTEP;
      double character = 1.0;
      Foot step_foot = foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT;
      double scale = params.sidekick_scale;

      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

// Stepcontrol
void PathPlanner::add_step(Pose2D &pose, const StepType &type, const WalkRequest::Coordinate &coordinate, const double character, const Foot foot, const double scale, const double speedDirection, const WalkRequest::StepControlRequest::RestrictionMode restriction, bool isProtected)
{
  step_buffer.push_back(Step_Buffer_Element({ pose,
                                              speedDirection,
                                              type,
                                              type == StepType::KICKSTEP ? params.kick_time : 250,
                                              character,
                                              scale,
                                              foot,
                                              coordinate,
                                              restriction,
                                              isProtected}));
}
void PathPlanner::update_step(Pose2D &pose)
{
  ASSERT(step_buffer.size() > 0);

  step_buffer.front().pose = pose;
}

void PathPlanner::manage_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  // requested step has been accepted
  if (last_stepRequestID == getMotionStatus().stepControl.stepRequestID)
  {
    step_buffer.erase(step_buffer.begin());
    last_stepRequestID = getMotionStatus().stepControl.stepRequestID + 1;
  }
}

void PathPlanner::execute_step_buffer()
{
  STOPWATCH_START("PathPlanner:execute_steplist");

  if (step_buffer.empty()) {
    return;
  }

  getMotionRequest().id                                        = motion::walk;
  getMotionRequest().walkRequest.coordinate                    = step_buffer.front().coordinate;
  getMotionRequest().walkRequest.character                     = step_buffer.front().character;
  getMotionRequest().walkRequest.stepControl.scale             = step_buffer.front().scale;
  getMotionRequest().walkRequest.stepControl.stepID            = getMotionStatus().stepControl.stepID;
  getMotionRequest().walkRequest.stepControl.type              = step_buffer.front().type;
  getMotionRequest().walkRequest.stepControl.time              = step_buffer.front().time;
  getMotionRequest().walkRequest.stepControl.speedDirection    = step_buffer.front().speedDirection;
  getMotionRequest().walkRequest.stepControl.target            = step_buffer.front().pose;
  getMotionRequest().walkRequest.stepControl.restriction       = step_buffer.front().restriction;
  getMotionRequest().walkRequest.stepControl.isProtected       = step_buffer.front().isProtected;
  getMotionRequest().walkRequest.stepControl.stepRequestID     = last_stepRequestID;

  // normal walking WALKSTEPs use Foot::NONE, for KICKSTEPs the foot to use has to be specified
  if (step_buffer.front().foot == Foot::NONE)
  {
    switch (getMotionStatus().stepControl.moveableFoot)
    {
    case MotionStatus::StepControlStatus::LEFT:
      foot_to_use = Foot::LEFT;
      break;
    case MotionStatus::StepControlStatus::RIGHT:
      foot_to_use = Foot::RIGHT;
      break;
    case MotionStatus::StepControlStatus::BOTH:
      if (step_buffer.front().pose.translation.y > 0.0f || step_buffer.front().pose.rotation > 0.0f)
      {
        foot_to_use = Foot::LEFT;
      }
      else
      {
        foot_to_use = Foot::RIGHT;
      }
      break;
    case MotionStatus::StepControlStatus::NONE:
      foot_to_use = Foot::RIGHT;
      break;
    }
  }
  else
  {
    foot_to_use = step_buffer.front().foot;
  }
  // false means right foot
  getMotionRequest().walkRequest.stepControl.moveLeftFoot = (foot_to_use != Foot::RIGHT);
  STOPWATCH_STOP("PathPlanner:execute_steplist");
}
<commit_msg>bugfix: stop path execution is a forced motion request is issued by xabsl<commit_after>/**
 * @file PathPlanner.cpp
 *
 * @author Yigit Can Akcay
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"

PathPlanner::PathPlanner()
:
step_buffer({}),
foot_to_use(Foot::RIGHT),
last_stepRequestID(getMotionStatus().stepControl.stepRequestID + 1),      // WalkRequest stepRequestID starts at 0, we have to start at 1
kick_planned(false)
{
  DEBUG_REQUEST_REGISTER("PathPlanner:walk_to_ball", "Walks to the ball from far.", false);

  getDebugParameterList().add(&params);
}

PathPlanner::~PathPlanner()
{
  getDebugParameterList().remove(&params);
}

void PathPlanner::execute()
{
  // --- DEBUG REQUESTS ---
  DEBUG_REQUEST("PathPlanner:walk_to_ball",
    if (getBallModel().positionPreview.x > 250) {
      walk_to_ball(Foot::NONE);
    }
  );
  // --- DEBUG REQUESTS ---

  getPathModel().kick_executed = false;

  STOPWATCH_START("PathPlanner");
  // Always executed first
  manage_step_buffer();

  // The kick has been executed
  // Tells XABSL to jump into next state
  if (kick_planned && step_buffer.empty()) {
    getPathModel().kick_executed = true;
  }

  // HACK: xabsl set a firced motion request => clear everything
  if(getPathModel().path_routine == PathModel::PathRoutine::NONE && getMotionRequest().forced) {
    step_buffer.clear();
    return;
  }

  switch (getPathModel().path_routine)
  {
  case PathModel::PathRoutine::NONE:
    // There is no kick planned, since the kick has been executed
    // and XABSL is in a different state now
    if (kick_planned) {
      kick_planned = false;
    }

    if (step_buffer.empty()) {
      return;
    }
    break;
  case PathModel::PathRoutine::GO_TO_BALL_FAST:
    walk_to_ball(Foot::NONE , true);
    break;
  case PathModel::PathRoutine::GO_TO_BALL_SLOW:
    walk_to_ball(Foot::NONE);
    break;
  case PathModel::PathRoutine::MOVE_AROUND_BALL:
    move_around_ball(getPathModel().direction, getPathModel().radius);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_LEFT:
    approach_ball(Foot::LEFT);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_RIGHT:
    approach_ball(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_LEFT:
    short_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_RIGHT:
    short_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::LONG_KICK_LEFT:
    long_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::LONG_KICK_RIGHT:
    long_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SIDEKICK_LEFT:
    sidekick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SIDEKICK_RIGHT:
    sidekick(Foot::RIGHT);
    break;
  }

  // Always executed last
  execute_step_buffer();

  STOPWATCH_STOP("PathPlanner");
}

void PathPlanner::walk_to_ball(const Foot foot, const bool go_fast)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ballPos    = getBallModel().positionPreview;
      coordinate = WalkRequest::Hip;
      break;
  }
  double ballRotation = ballPos.angle();

  Pose2D pose           = { ballRotation, 0.7*(ballPos.x - getPathModel().distance), ballPos.y };

  if (step_buffer.empty())
  {
    StepType type         = StepType::WALKSTEP;
    double scale          = 1.0;
    double speed_direction = 0.0;

    if (go_fast)
    {
      double character = 1.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
    else
    {
      double character = 0.3;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::move_around_ball(const double direction, const double radius)
{
  Vector2d ballPos                   = getBallModel().positionPreview;
  double ballRotation                = ballPos.angle();
  double ballDistance                = ballPos.abs();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;

  double min1;
  double min2;
  double max1;
  double max2;
  if (direction <= 0)
  {
    min1 = 0.0;
    min2 = 0.0;
    max1 = 45.0;
    max2 = 100.0;
  }
  else {
    min1 = -45;
    min2 = -100;
    max1 = 0;
    max2 = 0;
  }

  double stepX = (ballDistance - radius) * std::cos(ballRotation);
  double stepY = Math::clamp(radius * std::tan(Math::clamp(Math::toDegrees(-direction), min1, max1)), min2, max2) * std::cos(ballRotation);

  Pose2D pose           = { ballRotation, stepX, stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    Foot foot              = Foot::NONE;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::approach_ball(const Foot foot)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  double stepX                       = 0.0;
  double stepY                       = 0.0;
  double ballRadius                  = getFieldInfo().ballRadius;
  double stepRotation                = ballPos.abs() > 250 ? ballPos.angle() : 0;

  switch (foot) {
  case Foot::LEFT:
    ballPos    = getBallModel().positionPreviewInLFoot;
    coordinate = WalkRequest::LFoot;
    stepX      = ballPos.x - std::abs(ballPos.y - getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y - getPathModel().yOffset;
    break;
  case Foot::RIGHT:
    ballPos    = getBallModel().positionPreviewInRFoot;
    coordinate = WalkRequest::RFoot;
    stepX      = ballPos.x - std::abs(ballPos.y + getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y + getPathModel().yOffset;
    break;
  case Foot::NONE:
    ASSERT(false);
  }

  Pose2D pose = { stepRotation, 0.7 * stepX, 0.7 * stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, Foot::NONE, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::short_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500 , 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::long_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500, 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::sidekick(const Foot foot)
{
  if (!kick_planned)
  {
    double speed_direction             = 0.0;
    double stepY                       = 0.0;
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate     = WalkRequest::LFoot;
      speed_direction = 90;
      stepY          = 100;
      break;
    case Foot::RIGHT:
      coordinate     = WalkRequest::RFoot;
      speed_direction = -90;
      stepY          = -100;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, 500, stepY };
      StepType type = StepType::KICKSTEP;
      double character = 1.0;
      Foot step_foot = foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT;
      double scale = params.sidekick_scale;

      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

// Stepcontrol
void PathPlanner::add_step(Pose2D &pose, const StepType &type, const WalkRequest::Coordinate &coordinate, const double character, const Foot foot, const double scale, const double speedDirection, const WalkRequest::StepControlRequest::RestrictionMode restriction, bool isProtected)
{
  step_buffer.push_back(Step_Buffer_Element({ pose,
                                              speedDirection,
                                              type,
                                              type == StepType::KICKSTEP ? params.kick_time : 250,
                                              character,
                                              scale,
                                              foot,
                                              coordinate,
                                              restriction,
                                              isProtected}));
}
void PathPlanner::update_step(Pose2D &pose)
{
  ASSERT(step_buffer.size() > 0);

  step_buffer.front().pose = pose;
}

void PathPlanner::manage_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  // requested step has been accepted
  if (last_stepRequestID == getMotionStatus().stepControl.stepRequestID)
  {
    step_buffer.erase(step_buffer.begin());
    last_stepRequestID = getMotionStatus().stepControl.stepRequestID + 1;
  }
}

void PathPlanner::execute_step_buffer()
{
  STOPWATCH_START("PathPlanner:execute_steplist");

  if (step_buffer.empty()) {
    return;
  }

  getMotionRequest().id                                        = motion::walk;
  getMotionRequest().walkRequest.coordinate                    = step_buffer.front().coordinate;
  getMotionRequest().walkRequest.character                     = step_buffer.front().character;
  getMotionRequest().walkRequest.stepControl.scale             = step_buffer.front().scale;
  getMotionRequest().walkRequest.stepControl.stepID            = getMotionStatus().stepControl.stepID;
  getMotionRequest().walkRequest.stepControl.type              = step_buffer.front().type;
  getMotionRequest().walkRequest.stepControl.time              = step_buffer.front().time;
  getMotionRequest().walkRequest.stepControl.speedDirection    = step_buffer.front().speedDirection;
  getMotionRequest().walkRequest.stepControl.target            = step_buffer.front().pose;
  getMotionRequest().walkRequest.stepControl.restriction       = step_buffer.front().restriction;
  getMotionRequest().walkRequest.stepControl.isProtected       = step_buffer.front().isProtected;
  getMotionRequest().walkRequest.stepControl.stepRequestID     = last_stepRequestID;

  // normal walking WALKSTEPs use Foot::NONE, for KICKSTEPs the foot to use has to be specified
  if (step_buffer.front().foot == Foot::NONE)
  {
    switch (getMotionStatus().stepControl.moveableFoot)
    {
    case MotionStatus::StepControlStatus::LEFT:
      foot_to_use = Foot::LEFT;
      break;
    case MotionStatus::StepControlStatus::RIGHT:
      foot_to_use = Foot::RIGHT;
      break;
    case MotionStatus::StepControlStatus::BOTH:
      if (step_buffer.front().pose.translation.y > 0.0f || step_buffer.front().pose.rotation > 0.0f)
      {
        foot_to_use = Foot::LEFT;
      }
      else
      {
        foot_to_use = Foot::RIGHT;
      }
      break;
    case MotionStatus::StepControlStatus::NONE:
      foot_to_use = Foot::RIGHT;
      break;
    }
  }
  else
  {
    foot_to_use = step_buffer.front().foot;
  }
  // false means right foot
  getMotionRequest().walkRequest.stepControl.moveLeftFoot = (foot_to_use != Foot::RIGHT);
  STOPWATCH_STOP("PathPlanner:execute_steplist");
}
<|endoftext|>