<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.
 */

#ifndef otbFastICAImageFilter_hxx
#define otbFastICAImageFilter_hxx

#include "otbFastICAImageFilter.h"

#include "itkNumericTraits.h"
#include "itkProgressReporter.h"

#include <vnl/vnl_matrix.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_generalized_eigensystem.h>

namespace otb
{

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::FastICAImageFilter ()
{
  this->SetNumberOfRequiredInputs(1);

  m_NumberOfPrincipalComponentsRequired = 0;

  m_GivenTransformationMatrix = false;
  m_IsTransformationForward = true;

  m_NumberOfIterations = 50;
  m_ConvergenceThreshold = 1E-4;

  m_NonLinearity = [](double x) {return std::tanh(x);};
  m_NonLinearityDerivative = [](double x) {return 1-std::pow( std::tanh(x), 2. );};

  m_Mu = 1.;

  m_PCAFilter = PCAFilterType::New();
  m_PCAFilter->SetUseNormalization(true);
  m_PCAFilter->SetUseVarianceForNormalization(false);

  m_TransformFilter = TransformFilterType::New();
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::GenerateOutputInformation()
// throw itk::ExceptionObject
{
  Superclass::GenerateOutputInformation();

  switch ( static_cast<int>(DirectionOfTransformation) )
  {
  case static_cast<int>(Transform::FORWARD):
    {
      if ( m_NumberOfPrincipalComponentsRequired == 0
          || m_NumberOfPrincipalComponentsRequired
            > this->GetInput()->GetNumberOfComponentsPerPixel() )
      {
        m_NumberOfPrincipalComponentsRequired =
          this->GetInput()->GetNumberOfComponentsPerPixel();
      }

      this->GetOutput()->SetNumberOfComponentsPerPixel(
        m_NumberOfPrincipalComponentsRequired );
      break;
    }
  case static_cast<int>(Transform::INVERSE):
    {
      unsigned int theOutputDimension = 0;
      if ( m_GivenTransformationMatrix )
      {
        theOutputDimension = m_TransformationMatrix.Rows() >= m_TransformationMatrix.Cols() ?
          m_TransformationMatrix.Rows() : m_TransformationMatrix.Cols();
      }
      else
      {
        throw itk::ExceptionObject(__FILE__, __LINE__,
          "Mixture matrix is required to know the output size",
          ITK_LOCATION);
      }

      this->GetOutput()->SetNumberOfComponentsPerPixel( theOutputDimension );

      break;
    }
    default:
      throw itk::ExceptionObject(__FILE__, __LINE__,
          "Class should be templeted with FORWARD or INVERSE only...",
          ITK_LOCATION );
  }

  switch ( static_cast<int>(DirectionOfTransformation) )
    {
    case static_cast<int>(Transform::FORWARD):
    {
    ForwardGenerateOutputInformation();
    break;
    }
    case static_cast<int>(Transform::INVERSE):
    {
    ReverseGenerateOutputInformation();
    break;
    }
    }
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::ForwardGenerateOutputInformation()
{
  typename InputImageType::Pointer inputImgPtr
    = const_cast<InputImageType*>( this->GetInput() );

  m_PCAFilter->SetInput( inputImgPtr );
  m_PCAFilter->GetOutput()->UpdateOutputInformation();

  if ( !m_GivenTransformationMatrix )
  {
    GenerateTransformationMatrix();
  }
  else if ( !m_IsTransformationForward )
  {
    // prevent from multiple inversion in the pipelines
    m_IsTransformationForward = true;
    vnl_svd< MatrixElementType > invertor ( m_TransformationMatrix.GetVnlMatrix() );
    m_TransformationMatrix = invertor.pinverse();
  }

  if ( m_TransformationMatrix.GetVnlMatrix().empty() )
  {
    throw itk::ExceptionObject( __FILE__, __LINE__,
          "Empty transformation matrix",
          ITK_LOCATION);
  }

  m_TransformFilter->SetInput( m_PCAFilter->GetOutput() );
  m_TransformFilter->SetMatrix( m_TransformationMatrix.GetVnlMatrix() );
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::ReverseGenerateOutputInformation()
{
 if ( !m_GivenTransformationMatrix )
  {
    throw itk::ExceptionObject( __FILE__, __LINE__,
                                "No Transformation matrix given",
                                ITK_LOCATION );
  }

  if ( m_TransformationMatrix.GetVnlMatrix().empty() )
  {
    throw itk::ExceptionObject( __FILE__, __LINE__,
          "Empty transformation matrix",
          ITK_LOCATION);
  }

  if ( m_IsTransformationForward )
  {
    // prevent from multiple inversion in the pipelines
    m_IsTransformationForward = false;
    vnl_svd< MatrixElementType > invertor ( m_TransformationMatrix.GetVnlMatrix() );
    m_TransformationMatrix = invertor.pinverse();
  }

  m_TransformFilter->SetInput( this->GetInput() );
  m_TransformFilter->SetMatrix( m_TransformationMatrix.GetVnlMatrix() );

  /*
   * PCA filter may throw exception if
   * the mean, stdDev and transformation matrix
   * have not been given at this point
   */
  m_PCAFilter->SetInput( m_TransformFilter->GetOutput() );
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::GenerateData ()
{
  switch ( static_cast<int>(DirectionOfTransformation) )
    {
    case static_cast<int>(Transform::FORWARD):
      return ForwardGenerateData();
    case static_cast<int>(Transform::INVERSE):
      return ReverseGenerateData();
    default:
      throw itk::ExceptionObject(__FILE__, __LINE__,
                                 "Class should be templated with FORWARD or INVERSE only...",
                                 ITK_LOCATION );
    }
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::ForwardGenerateData ()
{
  m_TransformFilter->GraftOutput( this->GetOutput() );
  m_TransformFilter->Update();

  this->GraftOutput( m_TransformFilter->GetOutput() );
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::ReverseGenerateData ()
{
  m_PCAFilter->GraftOutput( this->GetOutput() );
  m_PCAFilter->Update();
  this->GraftOutput( m_PCAFilter->GetOutput() );
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::GenerateTransformationMatrix ()
{
  itk::ProgressReporter reporter ( this, 0, GetNumberOfIterations(), GetNumberOfIterations() );

  double convergence = itk::NumericTraits<double>::max();
  unsigned int iteration = 0;

  const unsigned int size = this->GetInput()->GetNumberOfComponentsPerPixel();

  // transformation matrix
  InternalMatrixType W ( size, size, vnl_matrix_identity );

  while ( iteration++ < GetNumberOfIterations()
          && convergence > GetConvergenceThreshold() )
  {
    InternalMatrixType W_old ( W );

    typename InputImageType::Pointer img = const_cast<InputImageType*>( m_PCAFilter->GetOutput() );
    TransformFilterPointerType transformer = TransformFilterType::New();
    if ( !W.is_identity() )
    {
      transformer->SetInput( GetPCAFilter()->GetOutput() );
      transformer->SetMatrix( W );
      transformer->Update();
      img = const_cast<InputImageType*>( transformer->GetOutput() );
    }

    for ( unsigned int band = 0; band < size; band++ )
    {
      otbMsgDebugMacro( << "Iteration " << iteration << ", bande " << band
                        << ", convergence " << convergence );

      InternalOptimizerPointerType optimizer = InternalOptimizerType::New();
      optimizer->SetInput( 0, m_PCAFilter->GetOutput() );
      optimizer->SetInput( 1, img );
      optimizer->SetW( W );
      optimizer->SetNonLinearity( this->GetNonLinearity(),
                                      this->GetNonLinearityDerivative() );
      optimizer->SetCurrentBandForLoop( band );

      MeanEstimatorFilterPointerType estimator = MeanEstimatorFilterType::New();
      estimator->SetInput( optimizer->GetOutput() );

      // Here we have a pipeline of two persistent filters, we have to manually
      // call Reset() and Synthetize () on the first one (optimizer).
      optimizer->Reset();
      estimator->Update();
      optimizer->Synthetize();

      double norm = 0.;
      for ( unsigned int bd = 0; bd < size; bd++ )
      {
        W(bd, band) -= m_Mu * ( estimator->GetMean()[bd]
                                - optimizer->GetBeta() * W(bd, band) )
                            / optimizer->GetDen();
        norm += std::pow( W(bd, band), 2. );
      }
      for ( unsigned int bd = 0; bd < size; bd++ )
        W(bd, band) /= std::sqrt( norm );
    }

    // Decorrelation of the W vectors
    InternalMatrixType W_tmp = W * W.transpose();
    vnl_svd< MatrixElementType > solver ( W_tmp );
    InternalMatrixType valP = solver.W();
    for ( unsigned int i = 0; i < valP.rows(); ++i )
      valP(i, i) = 1. / std::sqrt( static_cast<double>( valP(i, i) ) ); // Watch for 0 or neg
    InternalMatrixType transf = solver.U();
    W_tmp = transf * valP * transf.transpose();
    W = W_tmp * W;

    // Convergence evaluation
    convergence = 0.;
    for ( unsigned int i = 0; i < W.rows(); ++i )
      for ( unsigned int j = 0; j < W.cols(); ++j )
        convergence += std::abs( W(i, j) - W_old(i, j) );

    reporter.CompletedPixel();
  } // end of while loop

  if ( size != this->GetNumberOfPrincipalComponentsRequired() )
    {
    this->m_TransformationMatrix = W.get_n_columns( 0, this->GetNumberOfPrincipalComponentsRequired() );
    }
  else
    {
    this->m_TransformationMatrix = W;
    }

  otbMsgDebugMacro( << "Final convergence " << convergence
    << " after " << iteration << " iterations" );
}

} // end of namespace otb

#endif

<commit_msg>ENH: the dimensioality reduction is now done in the pca step<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.
 */

#ifndef otbFastICAImageFilter_hxx
#define otbFastICAImageFilter_hxx

#include "otbFastICAImageFilter.h"

#include "itkNumericTraits.h"
#include "itkProgressReporter.h"

#include <vnl/vnl_matrix.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_generalized_eigensystem.h>

namespace otb
{

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::FastICAImageFilter ()
{
  this->SetNumberOfRequiredInputs(1);

  m_NumberOfPrincipalComponentsRequired = 0;

  m_GivenTransformationMatrix = false;
  m_IsTransformationForward = true;

  m_NumberOfIterations = 50;
  m_ConvergenceThreshold = 1E-4;

  m_NonLinearity = [](double x) {return std::tanh(x);};
  m_NonLinearityDerivative = [](double x) {return 1-std::pow( std::tanh(x), 2. );};

  m_Mu = 1.;

  m_PCAFilter = PCAFilterType::New();
  m_PCAFilter->SetUseNormalization(true);
  m_PCAFilter->SetUseVarianceForNormalization(false);

  m_TransformFilter = TransformFilterType::New();
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::GenerateOutputInformation()
// throw itk::ExceptionObject
{
  Superclass::GenerateOutputInformation();

  switch ( static_cast<int>(DirectionOfTransformation) )
  {
  case static_cast<int>(Transform::FORWARD):
    {
      if ( m_NumberOfPrincipalComponentsRequired == 0
          || m_NumberOfPrincipalComponentsRequired
            > this->GetInput()->GetNumberOfComponentsPerPixel() )
      {
        m_NumberOfPrincipalComponentsRequired =
          this->GetInput()->GetNumberOfComponentsPerPixel();
      }
      m_PCAFilter->SetNumberOfPrincipalComponentsRequired(
        m_NumberOfPrincipalComponentsRequired);
      this->GetOutput()->SetNumberOfComponentsPerPixel(
        m_NumberOfPrincipalComponentsRequired );
      break;
    }
  case static_cast<int>(Transform::INVERSE):
    {
      unsigned int theOutputDimension = 0;
      if ( m_GivenTransformationMatrix )
      {
        theOutputDimension = m_TransformationMatrix.Rows() >= m_TransformationMatrix.Cols() ?
          m_TransformationMatrix.Rows() : m_TransformationMatrix.Cols();
      }
      else
      {
        throw itk::ExceptionObject(__FILE__, __LINE__,
          "Mixture matrix is required to know the output size",
          ITK_LOCATION);
      }

      this->GetOutput()->SetNumberOfComponentsPerPixel( theOutputDimension );

      break;
    }
    default:
      throw itk::ExceptionObject(__FILE__, __LINE__,
          "Class should be templeted with FORWARD or INVERSE only...",
          ITK_LOCATION );
  }

  switch ( static_cast<int>(DirectionOfTransformation) )
    {
    case static_cast<int>(Transform::FORWARD):
    {
    ForwardGenerateOutputInformation();
    break;
    }
    case static_cast<int>(Transform::INVERSE):
    {
    ReverseGenerateOutputInformation();
    break;
    }
    }
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::ForwardGenerateOutputInformation()
{
  typename InputImageType::Pointer inputImgPtr
    = const_cast<InputImageType*>( this->GetInput() );

  m_PCAFilter->SetInput( inputImgPtr );
  m_PCAFilter->GetOutput()->UpdateOutputInformation();

  if ( !m_GivenTransformationMatrix )
  {
    GenerateTransformationMatrix();
  }
  else if ( !m_IsTransformationForward )
  {
    // prevent from multiple inversion in the pipelines
    m_IsTransformationForward = true;
    vnl_svd< MatrixElementType > invertor ( m_TransformationMatrix.GetVnlMatrix() );
    m_TransformationMatrix = invertor.pinverse();
  }

  if ( m_TransformationMatrix.GetVnlMatrix().empty() )
  {
    throw itk::ExceptionObject( __FILE__, __LINE__,
          "Empty transformation matrix",
          ITK_LOCATION);
  }

  m_TransformFilter->SetInput( m_PCAFilter->GetOutput() );
  m_TransformFilter->SetMatrix( m_TransformationMatrix.GetVnlMatrix() );
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::ReverseGenerateOutputInformation()
{
 if ( !m_GivenTransformationMatrix )
  {
    throw itk::ExceptionObject( __FILE__, __LINE__,
                                "No Transformation matrix given",
                                ITK_LOCATION );
  }

  if ( m_TransformationMatrix.GetVnlMatrix().empty() )
  {
    throw itk::ExceptionObject( __FILE__, __LINE__,
          "Empty transformation matrix",
          ITK_LOCATION);
  }

  if ( m_IsTransformationForward )
  {
    // prevent from multiple inversion in the pipelines
    m_IsTransformationForward = false;
    vnl_svd< MatrixElementType > invertor ( m_TransformationMatrix.GetVnlMatrix() );
    m_TransformationMatrix = invertor.pinverse();
  }

  m_TransformFilter->SetInput( this->GetInput() );
  m_TransformFilter->SetMatrix( m_TransformationMatrix.GetVnlMatrix() );

  /*
   * PCA filter may throw exception if
   * the mean, stdDev and transformation matrix
   * have not been given at this point
   */
  m_PCAFilter->SetInput( m_TransformFilter->GetOutput() );
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::GenerateData ()
{
  switch ( static_cast<int>(DirectionOfTransformation) )
    {
    case static_cast<int>(Transform::FORWARD):
      return ForwardGenerateData();
    case static_cast<int>(Transform::INVERSE):
      return ReverseGenerateData();
    default:
      throw itk::ExceptionObject(__FILE__, __LINE__,
                                 "Class should be templated with FORWARD or INVERSE only...",
                                 ITK_LOCATION );
    }
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::ForwardGenerateData ()
{
  m_TransformFilter->GraftOutput( this->GetOutput() );
  m_TransformFilter->Update();

  this->GraftOutput( m_TransformFilter->GetOutput() );
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::ReverseGenerateData ()
{
  m_PCAFilter->GraftOutput( this->GetOutput() );
  m_PCAFilter->Update();
  this->GraftOutput( m_PCAFilter->GetOutput() );
}

template < class TInputImage, class TOutputImage,
            Transform::TransformDirection TDirectionOfTransformation >
void
FastICAImageFilter< TInputImage, TOutputImage, TDirectionOfTransformation >
::GenerateTransformationMatrix ()
{
  itk::ProgressReporter reporter ( this, 0, GetNumberOfIterations(), GetNumberOfIterations() );

  double convergence = itk::NumericTraits<double>::max();
  unsigned int iteration = 0;

  const unsigned int size = this->GetNumberOfPrincipalComponentsRequired();

  // transformation matrix
  InternalMatrixType W ( size, size, vnl_matrix_identity );

  while ( iteration++ < GetNumberOfIterations()
          && convergence > GetConvergenceThreshold() )
  {
    InternalMatrixType W_old ( W );

    typename InputImageType::Pointer img = const_cast<InputImageType*>( m_PCAFilter->GetOutput() );
    TransformFilterPointerType transformer = TransformFilterType::New();
    if ( !W.is_identity() )
    {
      transformer->SetInput( GetPCAFilter()->GetOutput() );
      transformer->SetMatrix( W );
      transformer->Update();
      img = const_cast<InputImageType*>( transformer->GetOutput() );
    }

    for ( unsigned int band = 0; band < size; band++ )
    {
      otbMsgDebugMacro( << "Iteration " << iteration << ", bande " << band
                        << ", convergence " << convergence );

      InternalOptimizerPointerType optimizer = InternalOptimizerType::New();
      optimizer->SetInput( 0, m_PCAFilter->GetOutput() );
      optimizer->SetInput( 1, img );
      optimizer->SetW( W );
      optimizer->SetNonLinearity( this->GetNonLinearity(),
                                      this->GetNonLinearityDerivative() );
      optimizer->SetCurrentBandForLoop( band );

      MeanEstimatorFilterPointerType estimator = MeanEstimatorFilterType::New();
      estimator->SetInput( optimizer->GetOutput() );

      // Here we have a pipeline of two persistent filters, we have to manually
      // call Reset() and Synthetize () on the first one (optimizer).
      optimizer->Reset();
      estimator->Update();
      optimizer->Synthetize();

      double norm = 0.;
      for ( unsigned int bd = 0; bd < size; bd++ )
      {
        W(bd, band) -= m_Mu * ( estimator->GetMean()[bd]
                                - optimizer->GetBeta() * W(bd, band) )
                            / optimizer->GetDen();
        norm += std::pow( W(bd, band), 2. );
      }
      for ( unsigned int bd = 0; bd < size; bd++ )
        W(bd, band) /= std::sqrt( norm );
    }

    // Decorrelation of the W vectors
    InternalMatrixType W_tmp = W * W.transpose();
    vnl_svd< MatrixElementType > solver ( W_tmp );
    InternalMatrixType valP = solver.W();
    for ( unsigned int i = 0; i < valP.rows(); ++i )
      valP(i, i) = 1. / std::sqrt( static_cast<double>( valP(i, i) ) ); // Watch for 0 or neg
    InternalMatrixType transf = solver.U();
    W_tmp = transf * valP * transf.transpose();
    W = W_tmp * W;

    // Convergence evaluation
    convergence = 0.;
    for ( unsigned int i = 0; i < W.rows(); ++i )
      for ( unsigned int j = 0; j < W.cols(); ++j )
        convergence += std::abs( W(i, j) - W_old(i, j) );

    reporter.CompletedPixel();
  } // end of while loop

  this->m_TransformationMatrix = W;

  otbMsgDebugMacro( << "Final convergence " << convergence
    << " after " << iteration << " iterations" );
}

} // end of namespace otb

#endif

<|endoftext|>


<commit_before>/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

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

#include "mitkPASpectralUnmixingFilterBase.h"

// Includes for AddEnmemberMatrix
#include "mitkPAPropertyCalculator.h"
#include <eigen3/Eigen/Dense>

// ImageAccessor
#include <mitkImageReadAccessor.h>
#include <mitkImageWriteAccessor.h>

mitk::pa::SpectralUnmixingFilterBase::SpectralUnmixingFilterBase()
{
  m_PropertyCalculatorEigen = mitk::pa::PropertyCalculator::New();
}

mitk::pa::SpectralUnmixingFilterBase::~SpectralUnmixingFilterBase()
{

}

void mitk::pa::SpectralUnmixingFilterBase::AddOutputs(unsigned int outputs)
{
  this->SetNumberOfIndexedOutputs(outputs);
  for (unsigned int i = 0; i<GetNumberOfIndexedOutputs(); i++)
    this->SetNthOutput(i, mitk::Image::New());
}

void mitk::pa::SpectralUnmixingFilterBase::AddWavelength(int wavelength)
{
  m_Wavelength.push_back(wavelength);
}

void mitk::pa::SpectralUnmixingFilterBase::AddChromophore(mitk::pa::PropertyCalculator::ChromophoreType chromophore)
{
  m_Chromophore.push_back(chromophore);
}

void mitk::pa::SpectralUnmixingFilterBase::Verbose(bool verbose)
{
  m_Verbose = verbose;
}

void mitk::pa::SpectralUnmixingFilterBase::RelativeError(bool relativeError)
{
  m_RelativeError = relativeError;
}

void mitk::pa::SpectralUnmixingFilterBase::GenerateData()
{
  MITK_INFO(m_Verbose) << "GENERATING DATA..";

  mitk::Image::Pointer input = GetInput(0);

  unsigned int xDim = input->GetDimensions()[0];
  unsigned int yDim = input->GetDimensions()[1];
  unsigned int numberOfInputImages = input->GetDimensions()[2];

  MITK_INFO(m_Verbose) << "x dimension: " << xDim;
  MITK_INFO(m_Verbose) << "y dimension: " << yDim;
  MITK_INFO(m_Verbose) << "z dimension: " << numberOfInputImages;

  unsigned int sequenceSize = m_Wavelength.size();
  unsigned int totalNumberOfSequences = numberOfInputImages / sequenceSize;
  if (totalNumberOfSequences == 0) //means that more chromophores then wavelengths
    mitkThrow() << "ERROR! REMOVE WAVELENGTHS!";
  MITK_INFO(m_Verbose) << "TotalNumberOfSequences: " << totalNumberOfSequences;

  InitializeOutputs(totalNumberOfSequences);

  auto endmemberMatrix = CalculateEndmemberMatrix(m_Chromophore, m_Wavelength);

  // Copy input image into array
  mitk::ImageReadAccessor readAccess(input);
  const float* inputDataArray = ((const float*)readAccess.GetData());

  CheckPreConditions(numberOfInputImages, inputDataArray);

  // test to see pixel values @ txt file
  myfile.open("SimplexNormalisation.txt");

  unsigned int outputCounter = GetNumberOfIndexedOutputs();
  std::vector<float*> writteBufferVector;
  for (unsigned int i = 0; i < outputCounter; ++i)
  {
    auto output = GetOutput(i);
    mitk::ImageWriteAccessor writeOutput(output);
    float* writeBuffer = (float *)writeOutput.GetData();
    writteBufferVector.push_back(writeBuffer);
  }

  if (m_RelativeError == true)
  {
    // -1 because rel error is output[IndexedOutputs() - 1] and loop over chromophore outputs has to end at [IndexedOutputs() - 2]
    outputCounter -= 1;
  }

  for (unsigned int sequenceCounter = 0; sequenceCounter < totalNumberOfSequences; ++sequenceCounter)
  {
    MITK_INFO(m_Verbose) << "SequenceCounter: " << sequenceCounter;
    //loop over every pixel in XY-plane
    for (unsigned int x = 0; x < xDim; x++)
    {
      for (unsigned int y = 0; y < yDim; y++)
      {
        Eigen::VectorXf inputVector(sequenceSize);
        for (unsigned int z = 0; z < sequenceSize; z++)
        {
          /**
          * 'sequenceCounter*sequenceSize' has to be added to 'z' to ensure that one accesses the
          * correct pixel, because the inputDataArray contains the information of all sequences and
          * not just the one of the current sequence.
          */
          unsigned int pixelNumber = (xDim*yDim*(z+sequenceCounter*sequenceSize)) + x * yDim + y;
          auto pixel = inputDataArray[pixelNumber];

          inputVector[z] = pixel;
        }
        Eigen::VectorXf resultVector = SpectralUnmixingAlgorithm(endmemberMatrix, inputVector);

        unsigned int outputCounter = GetNumberOfIndexedOutputs();
        if (m_RelativeError == true)
        {
          float relativeError = (endmemberMatrix*resultVector - inputVector).norm() / inputVector.norm(); // norm() is L2 norm
          writteBufferVector[outputCounter][(xDim*yDim * sequenceCounter) + x * yDim + y] = relativeError;
        }

        for (unsigned int outputIdx = 0; outputIdx < outputCounter; ++outputIdx)
        {
          writteBufferVector[outputIdx][(xDim*yDim * sequenceCounter) + x * yDim + y] = resultVector[outputIdx];
        }
      }
    }
  }
  MITK_INFO(m_Verbose) << "GENERATING DATA...[DONE]";
  myfile.close();
}

void mitk::pa::SpectralUnmixingFilterBase::CheckPreConditions(unsigned int numberOfInputImages, const float* inputDataArray)
{
  MITK_INFO(m_Verbose) << "CHECK PRECONDITIONS ...";

  if (m_Wavelength.size() < numberOfInputImages)
    MITK_WARN << "NUMBER OF WAVELENGTHS < NUMBER OF INPUT IMAGES";

  if (m_Chromophore.size() > m_Wavelength.size())
    mitkThrow() << "ADD MORE WAVELENGTHS OR REMOVE ENDMEMBERS!";

  if (typeid(inputDataArray[0]).name() != typeid(float).name())
    mitkThrow() << "PIXELTYPE ERROR! FLOAT 32 REQUIRED";

  MITK_INFO(m_Verbose) << "...[DONE]";
}

void mitk::pa::SpectralUnmixingFilterBase::InitializeOutputs(unsigned int totalNumberOfSequences)
{
  MITK_INFO(m_Verbose) << "Initialize Outputs ...";

  unsigned int numberOfInputs = GetNumberOfIndexedInputs();
  unsigned int numberOfOutputs = GetNumberOfIndexedOutputs();
  MITK_INFO(m_Verbose) << "Inputs: " << numberOfInputs << " Outputs: " << numberOfOutputs;

  mitk::PixelType pixelType = mitk::MakeScalarPixelType<float>();
  const int NUMBER_OF_SPATIAL_DIMENSIONS = 3;
  auto* dimensions = new unsigned int[NUMBER_OF_SPATIAL_DIMENSIONS];
  for (unsigned int dimIdx = 0; dimIdx < 2; dimIdx++)
    dimensions[dimIdx] = GetInput()->GetDimensions()[dimIdx];
  dimensions[2] = totalNumberOfSequences;

  for (unsigned int outputIdx = 0; outputIdx < numberOfOutputs; outputIdx++)
    GetOutput(outputIdx)->Initialize(pixelType, NUMBER_OF_SPATIAL_DIMENSIONS, dimensions);
  MITK_INFO(m_Verbose) << "...[DONE]";
}

Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> mitk::pa::SpectralUnmixingFilterBase::CalculateEndmemberMatrix(
  std::vector<mitk::pa::PropertyCalculator::ChromophoreType> m_Chromophore, std::vector<int> m_Wavelength)
{
  unsigned int numberOfChromophores = m_Chromophore.size(); //columns
  unsigned int numberOfWavelengths = m_Wavelength.size(); //rows
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> endmemberMatrixEigen(numberOfWavelengths, numberOfChromophores);

  for (unsigned int j = 0; j < numberOfChromophores; ++j)
  {
    for (unsigned int i = 0; i < numberOfWavelengths; ++i)
      endmemberMatrixEigen(i, j) = PropertyElement(m_Chromophore[j], m_Wavelength[i]);
  }
  MITK_INFO(m_Verbose) << "GENERATING ENMEMBERMATRIX [DONE]";
  return endmemberMatrixEigen;
}

float mitk::pa::SpectralUnmixingFilterBase::PropertyElement(mitk::pa::PropertyCalculator::ChromophoreType chromophore, int wavelength)
{
  if (chromophore == mitk::pa::PropertyCalculator::ChromophoreType::ONEENDMEMBER)
    return 1;
  else
  {
    float value = m_PropertyCalculatorEigen->GetAbsorptionForWavelength(chromophore, wavelength);
    if (value == 0)
      mitkThrow() << "WAVELENGTH " << wavelength << "nm NOT SUPPORTED!";
    else
      return value;
  }
}
<commit_msg>fixed a implementation bug from refacotring the write buffer.<commit_after>/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

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

#include "mitkPASpectralUnmixingFilterBase.h"

// Includes for AddEnmemberMatrix
#include "mitkPAPropertyCalculator.h"
#include <eigen3/Eigen/Dense>

// ImageAccessor
#include <mitkImageReadAccessor.h>
#include <mitkImageWriteAccessor.h>

mitk::pa::SpectralUnmixingFilterBase::SpectralUnmixingFilterBase()
{
  m_PropertyCalculatorEigen = mitk::pa::PropertyCalculator::New();
}

mitk::pa::SpectralUnmixingFilterBase::~SpectralUnmixingFilterBase()
{

}

void mitk::pa::SpectralUnmixingFilterBase::AddOutputs(unsigned int outputs)
{
  this->SetNumberOfIndexedOutputs(outputs);
  for (unsigned int i = 0; i<GetNumberOfIndexedOutputs(); i++)
    this->SetNthOutput(i, mitk::Image::New());
}

void mitk::pa::SpectralUnmixingFilterBase::AddWavelength(int wavelength)
{
  m_Wavelength.push_back(wavelength);
}

void mitk::pa::SpectralUnmixingFilterBase::AddChromophore(mitk::pa::PropertyCalculator::ChromophoreType chromophore)
{
  m_Chromophore.push_back(chromophore);
}

void mitk::pa::SpectralUnmixingFilterBase::Verbose(bool verbose)
{
  m_Verbose = verbose;
}

void mitk::pa::SpectralUnmixingFilterBase::RelativeError(bool relativeError)
{
  m_RelativeError = relativeError;
}

void mitk::pa::SpectralUnmixingFilterBase::GenerateData()
{
  MITK_INFO(m_Verbose) << "GENERATING DATA..";

  mitk::Image::Pointer input = GetInput(0);

  unsigned int xDim = input->GetDimensions()[0];
  unsigned int yDim = input->GetDimensions()[1];
  unsigned int numberOfInputImages = input->GetDimensions()[2];

  MITK_INFO(m_Verbose) << "x dimension: " << xDim;
  MITK_INFO(m_Verbose) << "y dimension: " << yDim;
  MITK_INFO(m_Verbose) << "z dimension: " << numberOfInputImages;

  unsigned int sequenceSize = m_Wavelength.size();
  unsigned int totalNumberOfSequences = numberOfInputImages / sequenceSize;
  if (totalNumberOfSequences == 0) //means that more chromophores then wavelengths
    mitkThrow() << "ERROR! REMOVE WAVELENGTHS!";
  MITK_INFO(m_Verbose) << "TotalNumberOfSequences: " << totalNumberOfSequences;

  InitializeOutputs(totalNumberOfSequences);

  auto endmemberMatrix = CalculateEndmemberMatrix(m_Chromophore, m_Wavelength);

  // Copy input image into array
  mitk::ImageReadAccessor readAccess(input);
  const float* inputDataArray = ((const float*)readAccess.GetData());

  CheckPreConditions(numberOfInputImages, inputDataArray);

  // test to see pixel values @ txt file
  myfile.open("SimplexNormalisation.txt");

  unsigned int outputCounter = GetNumberOfIndexedOutputs();
  std::vector<float*> writteBufferVector;
  for (unsigned int i = 0; i < outputCounter; ++i)
  {
    auto output = GetOutput(i);
    mitk::ImageWriteAccessor writeOutput(output);
    float* writeBuffer = (float *)writeOutput.GetData();
    writteBufferVector.push_back(writeBuffer);
  }

  if (m_RelativeError == true)
  {
    // -1 because rel error is output[IndexedOutputs() - 1] and loop over chromophore outputs has to end at [IndexedOutputs() - 2]
    outputCounter -= 1;
  }

  for (unsigned int sequenceCounter = 0; sequenceCounter < totalNumberOfSequences; ++sequenceCounter)
  {
    MITK_INFO(m_Verbose) << "SequenceCounter: " << sequenceCounter;
    //loop over every pixel in XY-plane
    for (unsigned int x = 0; x < xDim; x++)
    {
      for (unsigned int y = 0; y < yDim; y++)
      {
        Eigen::VectorXf inputVector(sequenceSize);
        for (unsigned int z = 0; z < sequenceSize; z++)
        {
          /**
          * 'sequenceCounter*sequenceSize' has to be added to 'z' to ensure that one accesses the
          * correct pixel, because the inputDataArray contains the information of all sequences and
          * not just the one of the current sequence.
          */
          unsigned int pixelNumber = (xDim*yDim*(z+sequenceCounter*sequenceSize)) + x * yDim + y;
          auto pixel = inputDataArray[pixelNumber];

          inputVector[z] = pixel;
        }
        Eigen::VectorXf resultVector = SpectralUnmixingAlgorithm(endmemberMatrix, inputVector);

        if (m_RelativeError == true)
        {
          float relativeError = (endmemberMatrix*resultVector - inputVector).norm() / inputVector.norm(); // norm() is L2 norm
          writteBufferVector[outputCounter][(xDim*yDim * sequenceCounter) + x * yDim + y] = relativeError;
        }

        for (unsigned int outputIdx = 0; outputIdx < outputCounter; ++outputIdx)
        {
          writteBufferVector[outputIdx][(xDim*yDim * sequenceCounter) + x * yDim + y] = resultVector[outputIdx];
        }
      }
    }
  }
  MITK_INFO(m_Verbose) << "GENERATING DATA...[DONE]";
  myfile.close();
}

void mitk::pa::SpectralUnmixingFilterBase::CheckPreConditions(unsigned int numberOfInputImages, const float* inputDataArray)
{
  MITK_INFO(m_Verbose) << "CHECK PRECONDITIONS ...";

  if (m_Wavelength.size() < numberOfInputImages)
    MITK_WARN << "NUMBER OF WAVELENGTHS < NUMBER OF INPUT IMAGES";

  if (m_Chromophore.size() > m_Wavelength.size())
    mitkThrow() << "ADD MORE WAVELENGTHS OR REMOVE ENDMEMBERS!";

  if (typeid(inputDataArray[0]).name() != typeid(float).name())
    mitkThrow() << "PIXELTYPE ERROR! FLOAT 32 REQUIRED";

  MITK_INFO(m_Verbose) << "...[DONE]";
}

void mitk::pa::SpectralUnmixingFilterBase::InitializeOutputs(unsigned int totalNumberOfSequences)
{
  MITK_INFO(m_Verbose) << "Initialize Outputs ...";

  unsigned int numberOfInputs = GetNumberOfIndexedInputs();
  unsigned int numberOfOutputs = GetNumberOfIndexedOutputs();
  MITK_INFO(m_Verbose) << "Inputs: " << numberOfInputs << " Outputs: " << numberOfOutputs;

  mitk::PixelType pixelType = mitk::MakeScalarPixelType<float>();
  const int NUMBER_OF_SPATIAL_DIMENSIONS = 3;
  auto* dimensions = new unsigned int[NUMBER_OF_SPATIAL_DIMENSIONS];
  for (unsigned int dimIdx = 0; dimIdx < 2; dimIdx++)
    dimensions[dimIdx] = GetInput()->GetDimensions()[dimIdx];
  dimensions[2] = totalNumberOfSequences;

  for (unsigned int outputIdx = 0; outputIdx < numberOfOutputs; outputIdx++)
    GetOutput(outputIdx)->Initialize(pixelType, NUMBER_OF_SPATIAL_DIMENSIONS, dimensions);
  MITK_INFO(m_Verbose) << "...[DONE]";
}

Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> mitk::pa::SpectralUnmixingFilterBase::CalculateEndmemberMatrix(
  std::vector<mitk::pa::PropertyCalculator::ChromophoreType> m_Chromophore, std::vector<int> m_Wavelength)
{
  unsigned int numberOfChromophores = m_Chromophore.size(); //columns
  unsigned int numberOfWavelengths = m_Wavelength.size(); //rows
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> endmemberMatrixEigen(numberOfWavelengths, numberOfChromophores);

  for (unsigned int j = 0; j < numberOfChromophores; ++j)
  {
    for (unsigned int i = 0; i < numberOfWavelengths; ++i)
      endmemberMatrixEigen(i, j) = PropertyElement(m_Chromophore[j], m_Wavelength[i]);
  }
  MITK_INFO(m_Verbose) << "GENERATING ENMEMBERMATRIX [DONE]";
  return endmemberMatrixEigen;
}

float mitk::pa::SpectralUnmixingFilterBase::PropertyElement(mitk::pa::PropertyCalculator::ChromophoreType chromophore, int wavelength)
{
  if (chromophore == mitk::pa::PropertyCalculator::ChromophoreType::ONEENDMEMBER)
    return 1;
  else
  {
    float value = m_PropertyCalculatorEigen->GetAbsorptionForWavelength(chromophore, wavelength);
    if (value == 0)
      mitkThrow() << "WAVELENGTH " << wavelength << "nm NOT SUPPORTED!";
    else
      return value;
  }
}
<|endoftext|>


<commit_before>#pragma once
#include <src/batchringbuffer.h>

#include <time.h>
#include <sys/time.h>

#include <gtest/gtest.h>

#ifdef __MACH__
#include <mach/clock.h>
#include <mach/mach.h>
#endif

#define MILLIS 1000000LL
#define NANOS 1000000000LL

class Bench_4_4_1024_buffer : public testing::Test {
protected:
	brb_buffer * buffer;
};

timespec
diff(timespec start, timespec end) {
	timespec temp;
	if ((end.tv_nsec-start.tv_nsec)<0) {
		temp.tv_sec = end.tv_sec-start.tv_sec-1;
		temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
	} else {
		temp.tv_sec = end.tv_sec-start.tv_sec;
		temp.tv_nsec = end.tv_nsec-start.tv_nsec;
	}
	return temp;
}

void
get_timespec(timespec * ts) {
#ifdef __MACH__ /* OS X does not have clock_gettime, use clock_get_time */
	clock_serv_t cclock;
	mach_timespec_t mts;
	host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
	clock_get_time(cclock, &mts);
	mach_port_deallocate(mach_task_self(), cclock);
	ts->tv_sec = mts.tv_sec;
	ts->tv_nsec = mts.tv_nsec;

#else
	clock_gettime(CLOCK_REALTIME, ts);
#endif
}

double
bench(int n)
{
	timespec time1, time2;

	brb_buffer * buffer;
	brb_batch * batch;
	char cancel;

	brb_init_buffer(&buffer, 4, 4, 1024);
	get_timespec(&time1);
	for (int i = 0; i< n; i++) {

		brb_claim(buffer, &batch, 1, &cancel);
		brb_publish(buffer, batch);
		brb_release(buffer, batch);

	}
	get_timespec(&time2);
	brb_free_buffer(&buffer);
	return (double)diff(time1,time2).tv_nsec / (double)n;
}

TEST_F(Bench_4_4_1024_buffer, _claim_publish_release) {
	double ns_per_op;
	double ops_per_ms;
	double ops_per_s;
	int count;
	/* warmup */
	bench(1000);
	for(count = 1000; count <= 10000000; count *= 10) {
		ns_per_op = bench(count);
		ops_per_ms = MILLIS / ns_per_op;
		ops_per_s = NANOS / ns_per_op;
		printf("[%10d]\t%8.4f ns/op\t\t%12.4f op/ms\t\t%12.4f op/s\n", count, ns_per_op, ops_per_ms, ops_per_s);
	}
}
<commit_msg>Added iostream include for ubuntu<commit_after>#pragma once
#include <src/batchringbuffer.h>

#include <iostream>
#include <time.h>
#include <sys/time.h>

#include <gtest/gtest.h>

#ifdef __MACH__
#include <mach/clock.h>
#include <mach/mach.h>
#endif

#define MILLIS 1000000LL
#define NANOS 1000000000LL

class Bench_4_4_1024_buffer : public testing::Test {
protected:
	brb_buffer * buffer;
};

timespec
diff(timespec start, timespec end) {
	timespec temp;
	if ((end.tv_nsec-start.tv_nsec)<0) {
		temp.tv_sec = end.tv_sec-start.tv_sec-1;
		temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
	} else {
		temp.tv_sec = end.tv_sec-start.tv_sec;
		temp.tv_nsec = end.tv_nsec-start.tv_nsec;
	}
	return temp;
}

void
get_timespec(timespec * ts) {
#ifdef __MACH__ /* OS X does not have clock_gettime, use clock_get_time */
	clock_serv_t cclock;
	mach_timespec_t mts;
	host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
	clock_get_time(cclock, &mts);
	mach_port_deallocate(mach_task_self(), cclock);
	ts->tv_sec = mts.tv_sec;
	ts->tv_nsec = mts.tv_nsec;

#else
	clock_gettime(CLOCK_REALTIME, ts);
#endif
}

double
bench(int n)
{
	timespec time1, time2;

	brb_buffer * buffer;
	brb_batch * batch;
	char cancel;

	brb_init_buffer(&buffer, 4, 4, 1024);
	get_timespec(&time1);
	for (int i = 0; i< n; i++) {

		brb_claim(buffer, &batch, 1, &cancel);
		brb_publish(buffer, batch);
		brb_release(buffer, batch);

	}
	get_timespec(&time2);
	brb_free_buffer(&buffer);
	return (double)diff(time1,time2).tv_nsec / (double)n;
}

TEST_F(Bench_4_4_1024_buffer, _claim_publish_release) {
	double ns_per_op;
	double ops_per_ms;
	double ops_per_s;
	int count;
	/* warmup */
	bench(1000);
	for(count = 1000; count <= 10000000; count *= 10) {
		ns_per_op = bench(count);
		ops_per_ms = MILLIS / ns_per_op;
		ops_per_s = NANOS / ns_per_op;
		printf("[%10d]\t%8.4f ns/op\t\t%12.4f op/ms\t\t%12.4f op/s\n", count, ns_per_op, ops_per_ms, ops_per_s);
	}
}
<|endoftext|>


<commit_before>/*
 *  Copyright (c) 2017, Tier IV, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of Autoware nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64MultiArray.h>
#include <geometry_msgs/TwistStamped.h>
#include <unordered_map>
#include <stdlib.h>
#include <string.h>
#include <vector>
#include <boost/algorithm/string.hpp>
#include <mosquitto.h>
#include <yaml-cpp/yaml.h>
using namespace std;
#include "mqtt_socket/mqtt_setting.hpp"
#include "autoware_msgs/CanInfo.h"

class MqttSender
{
public:
  MqttSender();
  ~MqttSender();
  void canInfoCallback(const autoware_msgs::CanInfoConstPtr &msg);
  static void on_connect(struct mosquitto *mosq, void *obj, int result);
  static void on_disconnect(struct mosquitto *mosq, void *obj, int rc);
  static void on_publish(struct mosquitto *mosq, void *userdata, int mid);
  static void load_config();

private:
  void targetVelocityArrayCallback(const std_msgs::Float64MultiArray &msg);
  void twistCmdCallback(const geometry_msgs::TwistStamped &msg);
  void stateCallback(const std_msgs::String &msg);
  unordered_map<string, ros::Subscriber> Subs;
  ros::NodeHandle node_handle_;

  // MQTT TOPIC
  string mqtt_topic_can_info_;
  string mqtt_topic_state_;
  string mqtt_topic_target_velocity_;
  string mqtt_topic_current_target_velocity_;

  // current behavior/status
  std_msgs::Float64MultiArray current_target_velocity_array_; //kmph
  geometry_msgs::TwistStamped current_twist_cmd_; //mps
  double current_target_velocity_; // mps2kmph(current_twist_cmd_.twist.twist.linear.x);
  std_msgs::String current_state_;

  int callback_counter_ = 0;
};

inline double mps2kmph(double _mpsval)
{
	return (_mpsval * 60 * 60) / 1000; // mps * 60sec * 60 minute / 1000m
}

MqttSender::MqttSender() :
    node_handle_("~")
{
  // ROS Subscriber
  Subs["can_info"] = node_handle_.subscribe("/can_info", 100, &MqttSender::canInfoCallback, this);
  Subs["target_velocity_array"] = node_handle_.subscribe("/target_velocity_array", 1, &MqttSender::targetVelocityArrayCallback, this);
  Subs["twist_cmd"] = node_handle_.subscribe("/twist_cmd", 1, &MqttSender::twistCmdCallback, this);
  Subs["state"] = node_handle_.subscribe("/state", 1, &MqttSender::stateCallback, this);

  // MQTT PARAMS
  mosquitto_lib_init();

  // Load Config
  MqttSender::load_config();

  mqtt_topic_can_info_ = "vehicle/" + to_string(vehicle_id) + "/can_info";
  mqtt_topic_state_ = "vehicle/" + to_string(vehicle_id) + "/state";
  mqtt_topic_target_velocity_ = "vehicle/" + to_string(vehicle_id) + "/target_velocity";
  mqtt_topic_current_target_velocity_ = "vehicle/" + to_string(vehicle_id) + "/current_velocity";

  mqtt_client = mosquitto_new(mqtt_client_id.c_str(), true, NULL);
  mosquitto_connect_callback_set(mqtt_client, &MqttSender::on_connect);
  mosquitto_disconnect_callback_set(mqtt_client, &MqttSender::on_disconnect);

  if(mosquitto_connect_bind(mqtt_client, mqtt_address.c_str(), mqtt_port, mqtt_timeout, NULL)){
    ROS_INFO("Failed to connect broker.\n");
    mosquitto_lib_cleanup();
    exit(EXIT_FAILURE);
  }
}

MqttSender::~MqttSender()
{
  mosquitto_destroy(mqtt_client);
  mosquitto_lib_cleanup();
}

void MqttSender::on_connect(struct mosquitto *mosq, void *obj, int result)
{
    ROS_INFO("on_connect: %s(%d)\n", __FUNCTION__, __LINE__);
}

void MqttSender::on_disconnect(struct mosquitto *mosq, void *obj, int rc)
{
    ROS_INFO("on_disconnect: %s(%d)\n", __FUNCTION__, __LINE__);
}

static void MqttSender::load_config()
{
  string config_file_path = ros::package::getPath(MQTT_NODE_NAME) + MQTT_CONFIG_FILE_NAME;
  ROS_INFO("Load Config File: %s %s(%d)\n", config_file_path.c_str(), __FUNCTION__, __LINE__);

  YAML::Node config = YAML::LoadFile(config_file_path);
  vehicle_id = config["mqtt"]["VEHICLEID"].as<int>();
  mqtt_client_id = "vehicle_" + to_string(vehicle_id) + "_snd";
  mqtt_address = config["mqtt"]["ADDRESS"].as<string>();
  mqtt_port = config["mqtt"]["PORT"].as<int>();
  mqtt_qos = config["mqtt"]["QOS"].as<int>();
  mqtt_timeout = config["mqtt"]["TIMEOUT"].as<int>();

  caninfo_downsample = config["mqtt"]["CANINFO_DOWNSAMPLE"].as<float>();
  gear_d = config["mqtt"]["GEAR_D"].as<int>();
  gear_n = config["mqtt"]["GEAR_N"].as<int>();
  gear_r = config["mqtt"]["GEAR_R"].as<int>();
  gear_p = config["mqtt"]["GEAR_P"].as<int>();

  ROS_INFO("MQTT Sender ADDR: %s:%d, ID: %s\n", mqtt_address.c_str(), mqtt_port,  mqtt_client_id.c_str());

  ROS_INFO(
    "[MQTT Receiver Settings] vehicle_id: %d, \
    caninfo_downsample: %f, gear_d: %d, gear_n: %d, gear_r: %d, \
    gear_p: %d", vehicle_id, caninfo_downsample, gear_d, gear_n, gear_r, gear_p);
}

void MqttSender::targetVelocityArrayCallback(const std_msgs::Float64MultiArray &msg)
{
  ostringstream publish_msg;
  current_target_velocity_array_ = msg;

  for(int i = 0; i < sizeof(msg.data); i++) {
    publish_msg << to_string(msg.data[i]) << ",";
  }
  string publish_msg_str = publish_msg.str();

  int ret = mosquitto_publish(
    mqtt_client,
    NULL,
    mqtt_topic_target_velocity_.c_str(),
    strlen(publish_msg_str.c_str()),
    publish_msg_str.c_str(),
    mqtt_qos,
    false
  );
}

void MqttSender::twistCmdCallback(const geometry_msgs::TwistStamped &msg)
{
  ostringstream publish_msg;
  current_twist_cmd_ = msg;
  current_target_velocity_ = mps2kmph(msg.twist.linear.x);

  publish_msg << to_string(current_target_velocity_);
  string publish_msg_str = publish_msg.str();

  int ret = mosquitto_publish(
    mqtt_client,
    NULL,
    mqtt_topic_current_target_velocity_.c_str(),
    strlen(publish_msg_str.c_str()),
    publish_msg_str.c_str(),
    mqtt_qos,
    false
  );
}

void MqttSender::stateCallback(const std_msgs::String &msg)
{
  ROS_INFO("State: %s\n", msg.data.c_str());

  int ret = mosquitto_publish(
    mqtt_client,
    NULL,
    mqtt_topic_state_.c_str(),
    strlen(msg.data.c_str()),
    msg.data.c_str(),
    mqtt_qos,
    false
  );
}

void MqttSender::canInfoCallback(const autoware_msgs::CanInfoConstPtr &msg)
{

  if(callback_counter_ > caninfo_downsample * 100) {
    ostringstream publish_msg;

    publish_msg << msg->tm << ",";
    publish_msg << to_string(msg->devmode) << ",";
    publish_msg << to_string(msg->drvcontmode) << ",";
    publish_msg << to_string(msg->drvoverridemode) << ",";
    publish_msg << to_string(msg->drvservo) << ",";
    publish_msg << to_string(msg->drivepedal) << ",";
    publish_msg << to_string(msg->targetpedalstr) << ",";
    publish_msg << to_string(msg->inputpedalstr) << ",";
    publish_msg << to_string(msg->targetveloc) << ",";
    publish_msg << to_string(msg->speed) << ",";
    if (msg->driveshift == gear_d)
      publish_msg << "D,";
    else if (msg->driveshift == gear_n)
      publish_msg << "N,";
    else if (msg->driveshift == gear_r)
      publish_msg << "R,";
    else if (msg->driveshift == gear_p)
      publish_msg << "P,";
    else
      publish_msg << "Unkwown,";
    publish_msg << to_string(msg->targetshift) << ",";
    publish_msg << to_string(msg->inputshift) << ",";
    publish_msg << to_string(msg->strmode) << ",";
    publish_msg << to_string(msg->strcontmode) << ",";
    publish_msg << to_string(msg->stroverridemode) << ",";
    publish_msg << to_string(msg->strservo) << ",";
    publish_msg << to_string(msg->targettorque) << ",";
    publish_msg << to_string(msg->torque) << ",";
    publish_msg << to_string(msg->angle) << ",";
    publish_msg << to_string(msg->targetangle) << ",";
    publish_msg << to_string(msg->bbrakepress) << ",";
    publish_msg << to_string(msg->brakepedal) << ",";
    publish_msg << to_string(msg->brtargetpedalstr) << ",";
    publish_msg << to_string(msg->brinputpedalstr) << ",";
    publish_msg << to_string(msg->battery) << ",";
    publish_msg << to_string(msg->voltage) << ",";
    publish_msg << to_string(msg->anp) << ",";
    publish_msg << to_string(msg->battmaxtemparature) << ",";
    publish_msg << to_string(msg->battmintemparature) << ",";
    publish_msg << to_string(msg->maxchgcurrent) << ",";
    publish_msg << to_string(msg->maxdischgcurrent) << ",";
    publish_msg << to_string(msg->sideacc) << ",";
    publish_msg << to_string(msg->accellfromp) << ",";
    publish_msg << to_string(msg->anglefromp) << ",";
    publish_msg << to_string(msg->brakepedalfromp) << ",";
    publish_msg << to_string(msg->speedfr) << ",";
    publish_msg << to_string(msg->speedfl) << ",";
    publish_msg << to_string(msg->speedrr) << ",";
    publish_msg << to_string(msg->speedrl) << ",";
    publish_msg << to_string(msg->velocfromp2) << ",";
    publish_msg << to_string(msg->drvmode) << ",";
    publish_msg << to_string(msg->devpedalstrfromp) << ",";
    publish_msg << to_string(msg->rpm) << ",";
    publish_msg << to_string(msg->velocflfromp) << ",";
    publish_msg << to_string(msg->ev_mode) << ",";
    publish_msg << to_string(msg->temp) << ",";
    publish_msg << to_string(msg->shiftfrmprius) << ",";
    publish_msg << to_string(msg->light) << ",";
    publish_msg << to_string(msg->gaslevel) << ",";
    publish_msg << to_string(msg->door) << ",";
    publish_msg << to_string(msg->cluise);

    // ostringstream publish_msg = create_message(msg);
    string publish_msg_str = publish_msg.str();
    int ret = mosquitto_publish(
      mqtt_client,
      NULL,
      mqtt_topic_can_info_.c_str(),
      strlen(publish_msg_str.c_str()),
      publish_msg_str.c_str(),
      mqtt_qos,
      false
    );

    callback_counter_ = 0;
  }
  else {
    callback_counter_++;
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "mqtt_sender");
  MqttSender node;
  ros::spin();

  return 0;
}
<commit_msg>add current_pose topic to mqtt msg<commit_after>/*
 *  Copyright (c) 2017, Tier IV, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of Autoware nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64MultiArray.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <unordered_map>
#include <stdlib.h>
#include <string.h>
#include <vector>
#include <boost/algorithm/string.hpp>
#include <mosquitto.h>
#include <yaml-cpp/yaml.h>
using namespace std;
#include "mqtt_socket/mqtt_setting.hpp"
#include "autoware_msgs/CanInfo.h"

class MqttSender
{
public:
  MqttSender();
  ~MqttSender();
  void canInfoCallback(const autoware_msgs::CanInfoConstPtr &msg);
  static void on_connect(struct mosquitto *mosq, void *obj, int result);
  static void on_disconnect(struct mosquitto *mosq, void *obj, int rc);
  static void on_publish(struct mosquitto *mosq, void *userdata, int mid);
  static void load_config();

private:
  void targetVelocityArrayCallback(const std_msgs::Float64MultiArray &msg);
  void twistCmdCallback(const geometry_msgs::TwistStamped &msg);
  void stateCallback(const std_msgs::String &msg);
  void currentPoseCallback(const geometry_msgs::PoseStamped& msg);
  unordered_map<string, ros::Subscriber> Subs;
  ros::NodeHandle node_handle_;

  // MQTT TOPIC
  string mqtt_topic_can_info_;
  string mqtt_topic_state_;
  string mqtt_topic_target_velocity_;
  string mqtt_topic_current_target_velocity_;
  string mqtt_topic_current_pose_;

  // current behavior/status
  std_msgs::Float64MultiArray current_target_velocity_array_; //kmph
  geometry_msgs::TwistStamped current_twist_cmd_; //mps
  double current_target_velocity_; // mps2kmph(current_twist_cmd_.twist.twist.linear.x);
  std_msgs::String current_state_;

  int callback_counter_ = 0;
};

inline double mps2kmph(double _mpsval)
{
	return (_mpsval * 60 * 60) / 1000; // mps * 60sec * 60 minute / 1000m
}

MqttSender::MqttSender() :
    node_handle_("~")
{
  // ROS Subscriber
  Subs["can_info"] = node_handle_.subscribe("/can_info", 100, &MqttSender::canInfoCallback, this);
  Subs["target_velocity_array"] = node_handle_.subscribe("/target_velocity_array", 1, &MqttSender::targetVelocityArrayCallback, this);
  Subs["twist_cmd"] = node_handle_.subscribe("/twist_cmd", 1, &MqttSender::twistCmdCallback, this);
  Subs["state"] = node_handle_.subscribe("/state", 1, &MqttSender::stateCallback, this);
  Subs["current_pose"] = node_handle_.subscribe("/current_pose", 1, &MqttSender::currentPoseCallback, this);

  // MQTT PARAMS
  mosquitto_lib_init();

  // Load Config
  MqttSender::load_config();

  mqtt_topic_can_info_ = "vehicle/" + to_string(vehicle_id) + "/can_info";
  mqtt_topic_state_ = "vehicle/" + to_string(vehicle_id) + "/state";
  mqtt_topic_target_velocity_ = "vehicle/" + to_string(vehicle_id) + "/target_velocity";
  mqtt_topic_current_target_velocity_ = "vehicle/" + to_string(vehicle_id) + "/current_velocity";
  mqtt_topic_current_pose_ = "vehicle/" + to_string(vehicle_id) + "/current_pose";

  mqtt_client = mosquitto_new(mqtt_client_id.c_str(), true, NULL);
  mosquitto_connect_callback_set(mqtt_client, &MqttSender::on_connect);
  mosquitto_disconnect_callback_set(mqtt_client, &MqttSender::on_disconnect);

  if(mosquitto_connect_bind(mqtt_client, mqtt_address.c_str(), mqtt_port, mqtt_timeout, NULL)){
    ROS_INFO("Failed to connect broker.\n");
    mosquitto_lib_cleanup();
    exit(EXIT_FAILURE);
  }
}

MqttSender::~MqttSender()
{
  mosquitto_destroy(mqtt_client);
  mosquitto_lib_cleanup();
}

void MqttSender::on_connect(struct mosquitto *mosq, void *obj, int result)
{
    ROS_INFO("on_connect: %s(%d)\n", __FUNCTION__, __LINE__);
}

void MqttSender::on_disconnect(struct mosquitto *mosq, void *obj, int rc)
{
    ROS_INFO("on_disconnect: %s(%d)\n", __FUNCTION__, __LINE__);
}

static void MqttSender::load_config()
{
  string config_file_path = ros::package::getPath(MQTT_NODE_NAME) + MQTT_CONFIG_FILE_NAME;
  ROS_INFO("Load Config File: %s %s(%d)\n", config_file_path.c_str(), __FUNCTION__, __LINE__);

  YAML::Node config = YAML::LoadFile(config_file_path);
  vehicle_id = config["mqtt"]["VEHICLEID"].as<int>();
  mqtt_client_id = "vehicle_" + to_string(vehicle_id) + "_snd";
  mqtt_address = config["mqtt"]["ADDRESS"].as<string>();
  mqtt_port = config["mqtt"]["PORT"].as<int>();
  mqtt_qos = config["mqtt"]["QOS"].as<int>();
  mqtt_timeout = config["mqtt"]["TIMEOUT"].as<int>();

  caninfo_downsample = config["mqtt"]["CANINFO_DOWNSAMPLE"].as<float>();
  gear_d = config["mqtt"]["GEAR_D"].as<int>();
  gear_n = config["mqtt"]["GEAR_N"].as<int>();
  unordered_map<string, ros::Subscriber> Subs;
  gear_r = config["mqtt"]["GEAR_R"].as<int>();
  gear_p = config["mqtt"]["GEAR_P"].as<int>();

  ROS_INFO("MQTT Sender ADDR: %s:%d, ID: %s\n", mqtt_address.c_str(), mqtt_port,  mqtt_client_id.c_str());

  ROS_INFO(
    "[MQTT Receiver Settings] vehicle_id: %d, \
    caninfo_downsample: %f, gear_d: %d, gear_n: %d, gear_r: %d, \
    gear_p: %d", vehicle_id, caninfo_downsample, gear_d, gear_n, gear_r, gear_p);
}

void MqttSender::targetVelocityArrayCallback(const std_msgs::Float64MultiArray &msg)
{
  ostringstream publish_msg;
  current_target_velocity_array_ = msg;

  for(int i = 0; i < sizeof(msg.data); i++) {
    publish_msg << to_string(msg.data[i]) << ",";
  }
  string publish_msg_str = publish_msg.str();

  int ret = mosquitto_publish(
    mqtt_client,
    NULL,
    mqtt_topic_target_velocity_.c_str(),
    strlen(publish_msg_str.c_str()),
    publish_msg_str.c_str(),
    mqtt_qos,
    false
  );
}

void MqttSender::twistCmdCallback(const geometry_msgs::TwistStamped &msg)
{
  ostringstream publish_msg;
  current_twist_cmd_ = msg;
  current_target_velocity_ = mps2kmph(msg.twist.linear.x);

  publish_msg << to_string(current_target_velocity_);
  string publish_msg_str = publish_msg.str();

  int ret = mosquitto_publish(
    mqtt_client,
    NULL,
    mqtt_topic_current_target_velocity_.c_str(),
    strlen(publish_msg_str.c_str()),
    publish_msg_str.c_str(),
    mqtt_qos,
    false
  );
}

void MqttSender::stateCallback(const std_msgs::String &msg)
{
  ROS_INFO("State: %s\n", msg.data.c_str());

  int ret = mosquitto_publish(
    mqtt_client,
    NULL,
    mqtt_topic_state_.c_str(),
    strlen(msg.data.c_str()),
    msg.data.c_str(),
    mqtt_qos,
    false
  );
}

void MqttSender::currentPoseCallback(const geometry_msgs::PoseStamped& msg)
{
  ostringstream publish_msg;

  publish_msg << to_string(msg.pose.position.x) << ",";
  publish_msg << to_string(msg.pose.position.y) << ",";
  publish_msg << to_string(msg.pose.position.z) << ",";
  publish_msg << to_string(msg.pose.orientation.x) << ",";
  publish_msg << to_string(msg.pose.orientation.y) << ",";
  publish_msg << to_string(msg.pose.orientation.z) << ",";
  publish_msg << to_string(msg.pose.orientation.w) << ",";

  string publish_msg_str = publish_msg.str();
  int ret = mosquitto_publish(
    mqtt_client,
    NULL,
    mqtt_topic_current_pose_.c_str(),
    strlen(publish_msg_str.c_str()),
    publish_msg_str.c_str(),
    mqtt_qos,
    false
  );
}

void MqttSender::canInfoCallback(const autoware_msgs::CanInfoConstPtr &msg)
{

  if(callback_counter_ > caninfo_downsample * 100) {
    ostringstream publish_msg;

    publish_msg << msg->tm << ",";
    publish_msg << to_string(msg->devmode) << ",";
    publish_msg << to_string(msg->drvcontmode) << ",";
    publish_msg << to_string(msg->drvoverridemode) << ",";
    publish_msg << to_string(msg->drvservo) << ",";
    publish_msg << to_string(msg->drivepedal) << ",";
    publish_msg << to_string(msg->targetpedalstr) << ",";
    publish_msg << to_string(msg->inputpedalstr) << ",";
    publish_msg << to_string(msg->targetveloc) << ",";
    publish_msg << to_string(msg->speed) << ",";
    if (msg->driveshift == gear_d)
      publish_msg << "D,";
    else if (msg->driveshift == gear_n)
      publish_msg << "N,";
    else if (msg->driveshift == gear_r)
      publish_msg << "R,";
    else if (msg->driveshift == gear_p)
      publish_msg << "P,";
    else
      publish_msg << "Unkwown,";
    publish_msg << to_string(msg->targetshift) << ",";
    publish_msg << to_string(msg->inputshift) << ",";
    publish_msg << to_string(msg->strmode) << ",";
    publish_msg << to_string(msg->strcontmode) << ",";
    publish_msg << to_string(msg->stroverridemode) << ",";
    publish_msg << to_string(msg->strservo) << ",";
    publish_msg << to_string(msg->targettorque) << ",";
    publish_msg << to_string(msg->torque) << ",";
    publish_msg << to_string(msg->angle) << ",";
    publish_msg << to_string(msg->targetangle) << ",";
    publish_msg << to_string(msg->bbrakepress) << ",";
    publish_msg << to_string(msg->brakepedal) << ",";
    publish_msg << to_string(msg->brtargetpedalstr) << ",";
    publish_msg << to_string(msg->brinputpedalstr) << ",";
    publish_msg << to_string(msg->battery) << ",";
    publish_msg << to_string(msg->voltage) << ",";
    publish_msg << to_string(msg->anp) << ",";
    publish_msg << to_string(msg->battmaxtemparature) << ",";
    publish_msg << to_string(msg->battmintemparature) << ",";
    publish_msg << to_string(msg->maxchgcurrent) << ",";
    publish_msg << to_string(msg->maxdischgcurrent) << ",";
    publish_msg << to_string(msg->sideacc) << ",";
    publish_msg << to_string(msg->accellfromp) << ",";
    publish_msg << to_string(msg->anglefromp) << ",";
    publish_msg << to_string(msg->brakepedalfromp) << ",";
    publish_msg << to_string(msg->speedfr) << ",";
    publish_msg << to_string(msg->speedfl) << ",";
    publish_msg << to_string(msg->speedrr) << ",";
    publish_msg << to_string(msg->speedrl) << ",";
    publish_msg << to_string(msg->velocfromp2) << ",";
    publish_msg << to_string(msg->drvmode) << ",";
    publish_msg << to_string(msg->devpedalstrfromp) << ",";
    publish_msg << to_string(msg->rpm) << ",";
    publish_msg << to_string(msg->velocflfromp) << ",";
    publish_msg << to_string(msg->ev_mode) << ",";
    publish_msg << to_string(msg->temp) << ",";
    publish_msg << to_string(msg->shiftfrmprius) << ",";
    publish_msg << to_string(msg->light) << ",";
    publish_msg << to_string(msg->gaslevel) << ",";
    publish_msg << to_string(msg->door) << ",";
    publish_msg << to_string(msg->cluise);

    // ostringstream publish_msg = create_message(msg);
    string publish_msg_str = publish_msg.str();
    int ret = mosquitto_publish(
      mqtt_client,
      NULL,
      mqtt_topic_can_info_.c_str(),
      strlen(publish_msg_str.c_str()),
      publish_msg_str.c_str(),
      mqtt_qos,
      false
    );

    callback_counter_ = 0;
  }
  else {
    callback_counter_++;
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "mqtt_sender");
  MqttSender node;
  ros::spin();

  return 0;
}
<|endoftext|>


<commit_before>/*
###############################################################################
#
#  EGSnrc egs++ shape collection
#  Copyright (C) 2015 National Research Council Canada
#
#  This file is part of EGSnrc.
#
#  EGSnrc is free software: you can redistribute it and/or modify it under
#  the terms of the GNU Affero General Public License as published by the
#  Free Software Foundation, either version 3 of the License, or (at your
#  option) any later version.
#
#  EGSnrc is distributed in the hope that it will be useful, but WITHOUT ANY
#  WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
#  FOR A PARTICULAR PURPOSE.  See the GNU Affero General Public License for
#  more details.
#
#  You should have received a copy of the GNU Affero General Public License
#  along with EGSnrc. If not, see <http://www.gnu.org/licenses/>.
#
###############################################################################
#
#  Author:          Iwan Kawrakow, 2005
#
#  Contributors:
#
###############################################################################
*/

/*! \file egs_shape_collection.cpp
 *  \brief A shape collection
 *  \IK
 */

#include "egs_shape_collection.h"
#include "egs_input.h"
#include "egs_functions.h"

EGS_ShapeCollection::EGS_ShapeCollection(const vector<EGS_BaseShape *> &Shapes,
        const vector<EGS_Float> &Probs, const string &Name, EGS_ObjectFactory *f) :
    EGS_BaseShape(Name,f), nshape(0) {
    int n1 = Shapes.size(), n2 = Probs.size();
    if (n1 < 2 || n2 < 2 || n1 != n2) {
        egsWarning("EGS_ShapeCollection::EGS_ShapeCollection: invalid input\n");
        return;
    }
    nshape = n1;
    shapes = new EGS_BaseShape* [nshape];
    EGS_Float *dum = new EGS_Float [nshape], *p = new EGS_Float [nshape];
    for (int j=0; j<nshape; j++) {
        shapes[j] = Shapes[j];
        shapes[j]->ref();
        p[j] = Probs[j];
        dum[j] = 1;
    }
    table = new EGS_AliasTable(nshape,dum,p,0);
    delete [] dum;
    delete [] p;
}

extern "C" {

    EGS_SHAPE_COLLECTION_EXPORT EGS_BaseShape *createShape(EGS_Input *input,
            EGS_ObjectFactory *f) {
        if (!input) {
            egsWarning("createShape(shape collection): null input?\n");
            return 0;
        }
        vector<EGS_BaseShape *> shapes;
        vector<EGS_Float> probs;
        EGS_Input *ishape;
        while ((ishape = input->takeInputItem("shape",false))) {
            EGS_BaseShape *shape = EGS_BaseShape::createShape(ishape);
            if (!shape) {
                egsWarning("createShape(shape collection): got null shape\n");
            }
            else {
                shapes.push_back(shape);
            }
            delete ishape;
        }
        vector<string> snames;
        int err = input->getInput("shape names",snames);
        if (!err && snames.size() > 0) {
            for (unsigned int j=0; j<snames.size(); j++) {
                EGS_BaseShape *shape = EGS_BaseShape::getShape(snames[j]);
                if (!shape) egsWarning("createShape(shape collection): no shape "
                                           "named %s exists\n",snames[j].c_str());
                else {
                    shapes.push_back(shape);
                }
            }
        }
        err = input->getInput("probabilities",probs);
        bool ok = true;
        if (err) {
            egsWarning("createShape(shape collection): no 'probabilities' input\n");
            ok = false;
        }
        if (shapes.size() < 2) {
            egsWarning("createShape(shape collection): at least 2 shapes are "
                       "needed for a shape collection, you defined %s\n",shapes.size());
            ok = false;
        }
        if (shapes.size() != probs.size()) {
            egsWarning("createShape(shape collection): the number of shapes (%d)"
                       " is not the same as the number of input probabilities (%d)\n");
            ok = false;
        }
        for (unsigned int i=0; i<probs.size(); i++) {
            if (probs[i] < 0) {
                egsWarning("createShape(shape collection): probabilities must not"
                           " be negative, but your input probability %d is %g\n",
                           i,probs[i]);
                ok = false;
            }
        }
        if (!ok) {
            for (unsigned int j=0; j<shapes.size(); j++) {
                EGS_Object::deleteObject(shapes[j]);
            }
            return 0;
        }
        EGS_ShapeCollection *s = new EGS_ShapeCollection(shapes,probs,"",f);
        s->setName(input);
        s->setTransformation(input);
        return s;
    }

}
<commit_msg>Fix format specifier in shape collection warning<commit_after>/*
###############################################################################
#
#  EGSnrc egs++ shape collection
#  Copyright (C) 2015 National Research Council Canada
#
#  This file is part of EGSnrc.
#
#  EGSnrc is free software: you can redistribute it and/or modify it under
#  the terms of the GNU Affero General Public License as published by the
#  Free Software Foundation, either version 3 of the License, or (at your
#  option) any later version.
#
#  EGSnrc is distributed in the hope that it will be useful, but WITHOUT ANY
#  WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
#  FOR A PARTICULAR PURPOSE.  See the GNU Affero General Public License for
#  more details.
#
#  You should have received a copy of the GNU Affero General Public License
#  along with EGSnrc. If not, see <http://www.gnu.org/licenses/>.
#
###############################################################################
#
#  Author:          Iwan Kawrakow, 2005
#
#  Contributors:
#
###############################################################################
*/

/*! \file egs_shape_collection.cpp
 *  \brief A shape collection
 *  \IK
 */

#include "egs_shape_collection.h"
#include "egs_input.h"
#include "egs_functions.h"

EGS_ShapeCollection::EGS_ShapeCollection(const vector<EGS_BaseShape *> &Shapes,
        const vector<EGS_Float> &Probs, const string &Name, EGS_ObjectFactory *f) :
    EGS_BaseShape(Name,f), nshape(0) {
    int n1 = Shapes.size(), n2 = Probs.size();
    if (n1 < 2 || n2 < 2 || n1 != n2) {
        egsWarning("EGS_ShapeCollection::EGS_ShapeCollection: invalid input\n");
        return;
    }
    nshape = n1;
    shapes = new EGS_BaseShape* [nshape];
    EGS_Float *dum = new EGS_Float [nshape], *p = new EGS_Float [nshape];
    for (int j=0; j<nshape; j++) {
        shapes[j] = Shapes[j];
        shapes[j]->ref();
        p[j] = Probs[j];
        dum[j] = 1;
    }
    table = new EGS_AliasTable(nshape,dum,p,0);
    delete [] dum;
    delete [] p;
}

extern "C" {

    EGS_SHAPE_COLLECTION_EXPORT EGS_BaseShape *createShape(EGS_Input *input,
            EGS_ObjectFactory *f) {
        if (!input) {
            egsWarning("createShape(shape collection): null input?\n");
            return 0;
        }
        vector<EGS_BaseShape *> shapes;
        vector<EGS_Float> probs;
        EGS_Input *ishape;
        while ((ishape = input->takeInputItem("shape",false))) {
            EGS_BaseShape *shape = EGS_BaseShape::createShape(ishape);
            if (!shape) {
                egsWarning("createShape(shape collection): got null shape\n");
            }
            else {
                shapes.push_back(shape);
            }
            delete ishape;
        }
        vector<string> snames;
        int err = input->getInput("shape names",snames);
        if (!err && snames.size() > 0) {
            for (unsigned int j=0; j<snames.size(); j++) {
                EGS_BaseShape *shape = EGS_BaseShape::getShape(snames[j]);
                if (!shape) egsWarning("createShape(shape collection): no shape "
                                           "named %s exists\n",snames[j].c_str());
                else {
                    shapes.push_back(shape);
                }
            }
        }
        err = input->getInput("probabilities",probs);
        bool ok = true;
        if (err) {
            egsWarning("createShape(shape collection): no 'probabilities' input\n");
            ok = false;
        }
        if (shapes.size() < 2) {
            egsWarning("createShape(shape collection): at least 2 shapes are "
                       "needed for a shape collection, you defined %d\n",shapes.size());
            ok = false;
        }
        if (shapes.size() != probs.size()) {
            egsWarning("createShape(shape collection): the number of shapes (%d)"
                       " is not the same as the number of input probabilities (%d)\n");
            ok = false;
        }
        for (unsigned int i=0; i<probs.size(); i++) {
            if (probs[i] < 0) {
                egsWarning("createShape(shape collection): probabilities must not"
                           " be negative, but your input probability %d is %g\n",
                           i,probs[i]);
                ok = false;
            }
        }
        if (!ok) {
            for (unsigned int j=0; j<shapes.size(); j++) {
                EGS_Object::deleteObject(shapes[j]);
            }
            return 0;
        }
        EGS_ShapeCollection *s = new EGS_ShapeCollection(shapes,probs,"",f);
        s->setName(input);
        s->setTransformation(input);
        return s;
    }

}
<|endoftext|>