<commit_before>/************************************************************************/
/*                                                                      */
/*               Copyright 1998-2002 by Ullrich Koethe                  */
/*       Cognitive Systems Group, University of Hamburg, Germany        */
/*                                                                      */
/*    This file is part of the VIGRA computer vision library.           */
/*    The VIGRA Website is                                              */
/*        http://kogs-www.informatik.uni-hamburg.de/~koethe/vigra/      */
/*    Please direct questions, bug reports, and contributions to        */
/*        koethe@informatik.uni-hamburg.de          or                  */
/*        vigra@kogs1.informatik.uni-hamburg.de                         */
/*                                                                      */
/*    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 VIGRA_STDIMAGE_HXX
#define VIGRA_STDIMAGE_HXX

#include "vigra/sized_int.hxx"
#include "vigra/tuple.hxx"
#include "vigra/basicimage.hxx"
#include "vigra/iteratortraits.hxx"
#include "vigra/accessor.hxx"
#include "vigra/rgbvalue.hxx"

namespace vigra {

/** \addtogroup StandardImageTypes Standard Image Types

    \brief The most common instantiations of the \ref vigra::BasicImage template
*/
//@{

    /** Byte (8-bit unsigned) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<UInt8> BImage;

    /** Short integer (16-bit signed) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<Int16> SImage;

    /** Integer (32-bit signed) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<Int32> IImage;

    /** Float (float) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<float> FImage;

    /** Double (double) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

       \#include "vigra/stdimage.hxx"

        Namespace: vigra
 */
typedef BasicImage<double> DImage;

    /** Byte (3x 8-bit unsigned) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::UInt8>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<UInt8> > BRGBImage;

    /** Short (3x 16-bit signed) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::Int16>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<Int16> > SRGBImage;

    /** Integer (3x 32-bit signed) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::Int32>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<Int32> > IRGBImage;

    /** Floating-point (3x float) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<float>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<float> > FRGBImage;

    /** Double-precision floating-point (3x double) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<double>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<double> > DRGBImage;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<float, 2>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<float, 2> > FVector2Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<float, 3>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<float, 3> > FVector3Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<float, 4>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<float, 4> > FVector4Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<double, 2>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<double, 2> > DVector2Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<double, 3>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
//typedef BasicImage<TinyVector<double, 3> > DVector3Image;
typedef BasicImage<TinyVector<double, 3> > DVector3Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<double, 4>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<double, 4> > DVector4Image;

//@}

} // namespace vigra

#endif // VIGRA_STDIMAGE_HXX
<commit_msg>added Int8Image, Int8RGBImage etc.<commit_after>/************************************************************************/
/*                                                                      */
/*               Copyright 1998-2002 by Ullrich Koethe                  */
/*       Cognitive Systems Group, University of Hamburg, Germany        */
/*                                                                      */
/*    This file is part of the VIGRA computer vision library.           */
/*    The VIGRA Website is                                              */
/*        http://kogs-www.informatik.uni-hamburg.de/~koethe/vigra/      */
/*    Please direct questions, bug reports, and contributions to        */
/*        koethe@informatik.uni-hamburg.de          or                  */
/*        vigra@kogs1.informatik.uni-hamburg.de                         */
/*                                                                      */
/*    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 VIGRA_STDIMAGE_HXX
#define VIGRA_STDIMAGE_HXX

#include "vigra/sized_int.hxx"
#include "vigra/tuple.hxx"
#include "vigra/basicimage.hxx"
#include "vigra/iteratortraits.hxx"
#include "vigra/accessor.hxx"
#include "vigra/rgbvalue.hxx"

namespace vigra {

/** \addtogroup StandardImageTypes Standard Image Types

    \brief The most common instantiations of the \ref vigra::BasicImage template
*/
//@{

    /** Byte (8-bit unsigned) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<UInt8> BImage;

    /** Byte (8-bit unsigned) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<UInt8> UInt8Image;

    /** Signed byte (8-bit signed) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<Int8> Int8Image;

    /** Short integer (16-bit signed) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<Int16> SImage;

    /** Short integer (16-bit unsigned) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<UInt16> UInt16Image;

    /** Short integer (16-bit signed) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<Int16> Int16Image;

    /** Integer (32-bit signed) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<Int32> IImage;

    /** Integer (32-bit unsigned) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<UInt32> UInt32Image;

    /** Integer (32-bit signed) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<Int32> Int32Image;

    /** Float (float) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<float> FImage;

    /** Double (double) image.
        It uses \ref vigra::BasicImageIterator and \ref vigra::StandardAccessor and
        their const counterparts to access the data.

       \#include "vigra/stdimage.hxx"

        Namespace: vigra
 */
typedef BasicImage<double> DImage;

    /** Byte (3x 8-bit unsigned) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::UInt8>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<UInt8> > BRGBImage;

    /** Byte (3x 8-bit unsigned) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::UInt8>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<UInt8> > UInt8RGBImage;

    /** Byte (3x 8-bit signed) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::UInt8>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<Int8> > Int8RGBImage;

    /** Short (3x 16-bit signed) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::Int16>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<Int16> > SRGBImage;

    /** Short (3x 16-bit unsigned) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::Int16>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<UInt16> > UInt16RGBImage;

    /** Short (3x 16-bit signed) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::Int16>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<Int16> > Int16RGBImage;

    /** Integer (3x 32-bit signed) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::Int32>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<Int32> > IRGBImage;

    /** Integer (3x 32-bit unsigned) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::Int32>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<UInt32> > UInt32RGBImage;

    /** Integer (3x 32-bit signed) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<vigra::Int32>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<Int32> > Int32RGBImage;

    /** Floating-point (3x float) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<float>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<float> > FRGBImage;

    /** Double-precision floating-point (3x double) RGB image.
        The pixel type is \ref vigra::RGBValue "vigra::RGBValue<double>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::RGBAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<RGBValue<double> > DRGBImage;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<float, 2>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<float, 2> > FVector2Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<float, 3>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<float, 3> > FVector3Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<float, 4>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<float, 4> > FVector4Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<double, 2>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<double, 2> > DVector2Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<double, 3>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
//typedef BasicImage<TinyVector<double, 3> > DVector3Image;
typedef BasicImage<TinyVector<double, 3> > DVector3Image;

    /** Floating-point TinyVector image.
        The pixel type is \ref vigra::TinyVector "vigra::TinyVector<double, 4>".
        It uses \ref vigra::BasicImageIterator and \ref vigra::VectorAccessor and
        their const counterparts to access the data.

        \#include "vigra/stdimage.hxx"

        Namespace: vigra
    */
typedef BasicImage<TinyVector<double, 4> > DVector4Image;

//@}

} // namespace vigra

#endif // VIGRA_STDIMAGE_HXX
<|endoftext|>


<commit_before>#ifndef VISION_METHODS_HPP
#define VISION_METHODS_HPP

#include <map>
#include <opencv2/opencv.hpp>

class vision_methods {
    private:
    const int _ksize_x                = 21;
    const int _ksize_y                = 21;
    const int _sigma_x                = 2;
    const int _sigma_y                = 2;
    const unsigned int _MIN_MARKER_ID = 2;
    const unsigned int _MAX_MARKER_ID = 9;
    const int _BLOB_SIZE_RATIO        = 4; // ratio between size and whitespace
    /**
     * extract markers from a map of features
     * @param    points a vector of features
     * @param    markers a map of MarkerID, MarkerLocation
     * @return
     */
    void extract_groups (std::vector<cv::KeyPoint> key_points,
    std::vector<std::vector<cv::KeyPoint>>& potential_markers);

    void extract_markers (std::vector<std::vector<cv::KeyPoint>>& potential_markers,
    std::map<unsigned int, cv::Point2f>& markers);

    public:
    vision_methods ();
    ~vision_methods ();

    /**
     * preprocess image
     * - greyscale
     * - blur to remove noise
     * @param  image_in image in
     * @return image out
     */
    cv::Mat preprocessing (const cv::Mat& image_in);

    /**
     * segment image
     * remove background and transform to binary image.
     * @param  image_in image to be segmented
     * @return          segmented image
     */
    cv::Mat segmentation (const cv::Mat& image_in);

    /**
     * extract features image
     * @param  image_in image to be segmented
     * @param  markers output map for found markers
     * @return          segmented image
     */
    cv::Mat extraction (const cv::Mat& image_in, std::map<unsigned int, cv::Point2f>& markers);
    // structure? feature_extraction(const cv::Mat&image_in);
    // map? classification (const ...);
};

#endif // VISION_METHODS_HPP
<commit_msg>update header documentation<commit_after>#ifndef VISION_METHODS_HPP
#define VISION_METHODS_HPP

#include <map>
#include <opencv2/opencv.hpp>

class vision_methods {
    private:
    const int _ksize_x                = 21;
    const int _ksize_y                = 21;
    const int _sigma_x                = 2;
    const int _sigma_y                = 2;
    const unsigned int _MIN_MARKER_ID = 2;
    const unsigned int _MAX_MARKER_ID = 9;
    const int _BLOB_SIZE_RATIO        = 4; // ratio between size and whitespace

    /**
    * find groups from a vector of keypoints, based on proximity
    * @param    key_points a vector of features
    * @param    potential_markers ouptut containing groups of points
    * @return
    */
    void extract_groups (std::vector<cv::KeyPoint> key_points,
    std::vector<std::vector<cv::KeyPoint>>& potential_markers);

    /**
    * extract markers from a vector of marker candidates
    * @param    potential_markers a vector of groups of keypoints
    * @param    markers output map of MarkerID, MarkerLocation
    * @return
    */
    void extract_markers (std::vector<std::vector<cv::KeyPoint>>& potential_markers,
    std::map<unsigned int, cv::Point2f>& markers);

    public:
    vision_methods ();
    ~vision_methods ();

    /**
     * preprocess image
     * - greyscale
     * - blur to remove noise
     * @param  image_in image in
     * @return image out
     */
    cv::Mat preprocessing (const cv::Mat& image_in);

    /**
     * segment image
     * remove background and transform to binary image.
     * @param  image_in image to be segmented
     * @return          segmented image
     */
    cv::Mat segmentation (const cv::Mat& image_in);

    /**
     * extract features image
     * @param  image_in image to be segmented
     * @param  markers output map for found markers
     * @return          segmented image
     */
    cv::Mat extraction (const cv::Mat& image_in, std::map<unsigned int, cv::Point2f>& markers);
    // structure? feature_extraction(const cv::Mat&image_in);
    // map? classification (const ...);
};

#endif // VISION_METHODS_HPP
<|endoftext|>


<commit_before>/***************************************************************************
* Copyright (c) 2017, Sylvain Corlay and Johan Mabille                     *
*                                                                          *
* Distributed under the terms of the BSD 3-Clause License.                 *
*                                                                          *
* The full license is in the file LICENSE, distributed with this software. *
****************************************************************************/

#ifndef XTL_CONFIG_HPP
#define XTL_CONFIG_HPP

#define XTL_VERSION_MAJOR 0
#define XTL_VERSION_MINOR 2
#define XTL_VERSION_PATCH 2

#endif
<commit_msg>Release 0.2.3<commit_after>/***************************************************************************
* Copyright (c) 2017, Sylvain Corlay and Johan Mabille                     *
*                                                                          *
* Distributed under the terms of the BSD 3-Clause License.                 *
*                                                                          *
* The full license is in the file LICENSE, distributed with this software. *
****************************************************************************/

#ifndef XTL_CONFIG_HPP
#define XTL_CONFIG_HPP

#define XTL_VERSION_MAJOR 0
#define XTL_VERSION_MINOR 2
#define XTL_VERSION_PATCH 3

#endif
<|endoftext|>


<commit_before><commit_msg>Set type declaration postfix<commit_after><|endoftext|>


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

#include <actionlib/server/simple_action_server.h>
#include <moveit_msgs/MoveGroupAction.h>

#include <tf/transform_listener.h>
#include <planning_interface/planning_interface.h>
#include <planning_request_adapter/planning_request_adapter.h>

#include <planning_scene_monitor/planning_scene_monitor.h>
#include <trajectory_execution_ros/trajectory_execution_monitor_ros.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <boost/tokenizer.hpp>

#include <trajectory_processing/iterative_smoother.h>

static const std::string ROBOT_DESCRIPTION = "robot_description";      // name of the robot description (a param name, so it can be changed externally)
static const std::string DISPLAY_PATH_PUB_TOPIC = "display_trajectory";

class MoveGroupAction
{
public:

  enum MoveGroupState
    {
      IDLE,
      PLANNING,
      MONITOR
    };

  MoveGroupAction(planning_scene_monitor::PlanningSceneMonitor &psm) :
    nh_("~"), psm_(psm), state_(IDLE)
  {
    // load the group name
    if (nh_.getParam("group", group_name_))
      ROS_INFO("Starting move_group for group '%s'", group_name_.c_str());
    else
      ROS_FATAL("Group name not specified. Cannot start move_group");
    bool manage_controllers= false;
    nh_.param("manage_controllers", manage_controllers, true);

    trajectory_execution_.reset(new trajectory_execution_ros::TrajectoryExecutionMonitorRos(psm_.getPlanningScene()->getKinematicModel(),
                                                                                manage_controllers));

    // load the planning plugin
    try
    {
      planner_plugin_loader_.reset(new pluginlib::ClassLoader<planning_interface::Planner>("planning_interface", "planning_interface::Planner"));
    }
    catch(pluginlib::PluginlibException& ex)
    {
      ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
    }

    nh_.getParam("planning_plugin", planning_plugin_name_);
    const std::vector<std::string> &classes = planner_plugin_loader_->getDeclaredClasses();
    if (planning_plugin_name_.empty() && classes.size() == 1)
    {
      planning_plugin_name_ = classes[0];
      ROS_INFO("No 'planning_plugin' parameter specified, but only '%s' planning plugin is available. Using that one.", planning_plugin_name_.c_str());
    }
    if (planning_plugin_name_.empty() && classes.size() > 1)
    {
      planning_plugin_name_ = classes[0];
      ROS_INFO("Multiple planning plugins available. You shuold specify the 'planning_plugin' parameter. Using '%s' for now.", planning_plugin_name_.c_str());
    }
    try
    {
      planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planning_plugin_name_));
      planner_instance_->init(psm_.getPlanningScene()->getKinematicModel());
      ROS_INFO_STREAM("Using planning interface '" << planner_instance_->getDescription() << "'");
    }
    catch(pluginlib::PluginlibException& ex)
    {
      std::stringstream ss;
      for (std::size_t i = 0 ; i < classes.size() ; ++i)
        ss << classes[i] << " ";
      ROS_FATAL_STREAM("Exception while loading planner '" << planning_plugin_name_ << "': " << ex.what() << std::endl
                       << "Available plugins: " << ss.str());
    }

    // load the planner request adapters
    std::string adapters;
    if (nh_.getParam("request_adapters", adapters))
    {
      try
      {
        adapter_plugin_loader_.reset(new pluginlib::ClassLoader<planning_request_adapter::PlanningRequestAdapter>("planning_request_adapter", "planning_request_adapter::PlanningRequestAdapter"));
      }
      catch(pluginlib::PluginlibException& ex)
      {
        ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what());
      }
      boost::char_separator<char> sep(" ");
      boost::tokenizer<boost::char_separator<char> > tok(adapters, sep);
      std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> ads;
      for(boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin() ; beg != tok.end(); ++beg)
      {
        planning_request_adapter::PlanningRequestAdapterConstPtr ad;
        try
        {
          ad.reset(adapter_plugin_loader_->createUnmanagedInstance(*beg));
        }
        catch (pluginlib::PluginlibException& ex)
        {
          ROS_ERROR_STREAM("Exception while planning adapter plugin '" << *beg << "': " << ex.what());
        }
        if (ad)
          ads.push_back(ad);
      }
      if (!ads.empty())
      {
        adapter_chain_.reset(new planning_request_adapter::PlanningRequestAdapterChain());
        for (std::size_t i = 0 ; i < ads.size() ; ++i)
        {
          ROS_INFO_STREAM("Using planning request adapter '" << ads[i]->getDescription() << "'");
          adapter_chain_->addAdapter(ads[i]);
        }
      }
    }

    display_path_publisher_ = root_nh_.advertise<moveit_msgs::DisplayTrajectory>("move_" + group_name_ + "/" + DISPLAY_PATH_PUB_TOPIC, 1, true);

    // start the action server
    action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::MoveGroupAction>(root_nh_, "move_" + group_name_, false));
    action_server_->registerGoalCallback(boost::bind(&MoveGroupAction::goalCallback, this));
    action_server_->registerPreemptCallback(boost::bind(&MoveGroupAction::preemptCallback, this));
    action_server_->start();
  }

  void goalCallback(void)
  {
    if (service_goal_thread_)
    {
      terminate_service_thread_ = true;
      service_goal_thread_->join();
      service_goal_thread_.reset();
    }
    goal_ = action_server_->acceptNewGoal();
    if (!goal_)
    {
      ROS_ERROR("Something unexpected happened. No goal found in callback for goal...");
      return;
    }

    if (!goal_->request.group_name.empty() && goal_->request.group_name != group_name_)
    {
      moveit_msgs::MoveGroupResult res;
      res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
      action_server_->setAborted(res, "Cannot accept requests for group '" +
                                 goal_->request.group_name  + "' when the move_group action is loaded for group '" +
                                 group_name_ +  "'");
    }
    else
      service_goal_thread_.reset(new boost::thread(boost::bind(&MoveGroupAction::serviceGoalRequest, this)));
  }

  void preemptCallback(void)
  {
    action_server_->setPreempted();
    terminate_service_thread_ = true;
  }

  void serviceGoalRequest(void)
  {
    setState(PLANNING);

    bool solved = false;
    moveit_msgs::GetMotionPlan::Request req;
    req.motion_plan_request = goal_->request;
    if (req.motion_plan_request.group_name.empty())
      req.motion_plan_request.group_name = group_name_;
    moveit_msgs::GetMotionPlan::Response res;

    const planning_scene::PlanningScenePtr &the_scene =
      planning_scene::PlanningScene::isEmpty(goal_->planning_scene_diff) ? psm_.getPlanningScene() : planning_scene::PlanningScene::diff(psm_.getPlanningScene(), goal_->planning_scene_diff);

    try
    {
      if (adapter_chain_)
        solved = adapter_chain_->adaptAndPlan(planner_instance_, the_scene, req, res);
      else
        solved = planner_instance_->solve(the_scene, req, res);
    }
    catch(std::runtime_error &ex)
    {
      ROS_ERROR("Exception caught: '%s'", ex.what());
    }
    catch(...)
    {
      ROS_ERROR("Unknown exception thrown by planner");
    }

    if (solved)
    {
      trajectory_msgs::JointTrajectory trajectory_out;
      smoother_.smooth(res.trajectory.joint_trajectory, trajectory_out, psm_.getGroupJointLimitsMap().at(group_name_));
      res.trajectory.joint_trajectory = trajectory_out;

      setState(MONITOR);
      execution_complete_ = false;

      // display the trajectory
      moveit_msgs::DisplayTrajectory disp;
      disp.model_id = psm_.getPlanningScene()->getKinematicModel()->getName();
      disp.trajectory_start = res.trajectory_start;
      disp.trajectory = res.trajectory;
      display_path_publisher_.publish(disp);

      trajectory_execution::TrajectoryExecutionRequest ter;
      ter.group_name_ = group_name_;
      ter.trajectory_ = res.trajectory.joint_trajectory; // \TODO This should take in a RobotTrajectory
      if (trajectory_execution_->executeTrajectory(ter, boost::bind(&MoveGroupAction::doneWithTrajectoryExecution, this, _1)))
      {
        ros::WallDuration d(0.01);
        while (nh_.ok() && !execution_complete_ && !terminate_service_thread_)
        {
          /// \TODO Check if the remainder of the path is still valid; If not, replan.
          /// We need a callback in the trajectory monitor for this
          d.sleep();
        }
        moveit_msgs::MoveGroupResult res;
        res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
        action_server_->setSucceeded(res, "Solution was found and executed.");
      }
      else
      {
        moveit_msgs::MoveGroupResult res;
        //        res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
        action_server_->setAborted(res, "Solution was found but the controller failed to execute it.");
      }
    }
    else
    {
      moveit_msgs::MoveGroupResult res;
      res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
      action_server_->setAborted(res, "No motion plan found. No execution attempted.");
    }

    setState(IDLE);
  }

  bool doneWithTrajectoryExecution(trajectory_execution::TrajectoryExecutionDataVector data)
  {
    execution_complete_ = true;
    return true;
  }

  void setState(MoveGroupState state)
  {
    state_ = state;
    switch (state_)
    {
    case IDLE:
      feedback_.state = "IDLE";
      feedback_.time_to_completion = ros::Duration(0.0);
      break;
    case PLANNING:
      feedback_.state = "PLANNING";
      feedback_.time_to_completion = ros::Duration(0.0);
      break;
    case MONITOR:
      feedback_.state = "MONITOR";
      feedback_.time_to_completion = ros::Duration(0.0);
      break;
    }
    action_server_->publishFeedback(feedback_);
  }

  void status(void)
  {
    ROS_INFO("MoveGroup action for group '%s' running using planning plugin '%s'", group_name_.c_str(), planning_plugin_name_.c_str());
  }

private:

  ros::NodeHandle root_nh_;
  ros::NodeHandle nh_;
  planning_scene_monitor::PlanningSceneMonitor &psm_;

  std::string planning_plugin_name_;
  boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::Planner> > planner_plugin_loader_;
  planning_interface::PlannerPtr planner_instance_;
  std::string group_name_;

  boost::scoped_ptr<pluginlib::ClassLoader<planning_request_adapter::PlanningRequestAdapter> > adapter_plugin_loader_;
  boost::scoped_ptr<planning_request_adapter::PlanningRequestAdapterChain> adapter_chain_;

  boost::shared_ptr<actionlib::SimpleActionServer<moveit_msgs::MoveGroupAction> > action_server_;
  moveit_msgs::MoveGroupGoalConstPtr goal_;
  moveit_msgs::MoveGroupFeedback feedback_;

  boost::scoped_ptr<boost::thread> service_goal_thread_;

  boost::shared_ptr<trajectory_execution_ros::TrajectoryExecutionMonitorRos> trajectory_execution_;
  bool terminate_service_thread_;
  bool execution_complete_;
  MoveGroupState state_;
  trajectory_processing::IterativeParabolicSmoother smoother_;

  ros::Publisher display_path_publisher_;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_group", ros::init_options::AnonymousName);

  ros::AsyncSpinner spinner(1);
  spinner.start();

  tf::TransformListener tf;
  planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION, &tf);
  if (psm.getPlanningScene()->isConfigured())
  {
    psm.startWorldGeometryMonitor();
    psm.startSceneMonitor();
    psm.startStateMonitor();

    MoveGroupAction mga(psm);
    mga.status();
    ros::waitForShutdown();
  }
  else
    ROS_ERROR("Planning scene not configured");

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

#include <actionlib/server/simple_action_server.h>
#include <moveit_msgs/MoveGroupAction.h>

#include <tf/transform_listener.h>
#include <planning_interface/planning_interface.h>
#include <planning_request_adapter/planning_request_adapter.h>

#include <planning_scene_monitor/planning_scene_monitor.h>
#include <trajectory_execution_ros/trajectory_execution_monitor_ros.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <boost/tokenizer.hpp>

#include <trajectory_processing/iterative_smoother.h>

static const std::string ROBOT_DESCRIPTION = "robot_description";      // name of the robot description (a param name, so it can be changed externally)
static const std::string DISPLAY_PATH_PUB_TOPIC = "display_trajectory";

class MoveGroupAction
{
public:

  enum MoveGroupState
    {
      IDLE,
      PLANNING,
      MONITOR
    };

  MoveGroupAction(planning_scene_monitor::PlanningSceneMonitor &psm) :
    nh_("~"), psm_(psm), state_(IDLE)
  {
    // load the group name
    if (nh_.getParam("group", group_name_))
      ROS_INFO("Starting move_group for group '%s'", group_name_.c_str());
    else
      ROS_FATAL("Group name not specified. Cannot start move_group");
    bool manage_controllers= false;
    nh_.param("manage_controllers", manage_controllers, true);

    trajectory_execution_.reset(new trajectory_execution_ros::TrajectoryExecutionMonitorRos(psm_.getPlanningScene()->getKinematicModel(),
                                                                                manage_controllers));

    // load the planning plugin
    try
    {
      planner_plugin_loader_.reset(new pluginlib::ClassLoader<planning_interface::Planner>("planning_interface", "planning_interface::Planner"));
    }
    catch(pluginlib::PluginlibException& ex)
    {
      ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
    }

    nh_.getParam("planning_plugin", planning_plugin_name_);
    const std::vector<std::string> &classes = planner_plugin_loader_->getDeclaredClasses();
    if (planning_plugin_name_.empty() && classes.size() == 1)
    {
      planning_plugin_name_ = classes[0];
      ROS_INFO("No 'planning_plugin' parameter specified, but only '%s' planning plugin is available. Using that one.", planning_plugin_name_.c_str());
    }
    if (planning_plugin_name_.empty() && classes.size() > 1)
    {
      planning_plugin_name_ = classes[0];
      ROS_INFO("Multiple planning plugins available. You shuold specify the 'planning_plugin' parameter. Using '%s' for now.", planning_plugin_name_.c_str());
    }
    try
    {
      planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planning_plugin_name_));
      planner_instance_->init(psm_.getPlanningScene()->getKinematicModel());
      ROS_INFO_STREAM("Using planning interface '" << planner_instance_->getDescription() << "'");
    }
    catch(pluginlib::PluginlibException& ex)
    {
      std::stringstream ss;
      for (std::size_t i = 0 ; i < classes.size() ; ++i)
        ss << classes[i] << " ";
      ROS_FATAL_STREAM("Exception while loading planner '" << planning_plugin_name_ << "': " << ex.what() << std::endl
                       << "Available plugins: " << ss.str());
    }

    // load the planner request adapters
    std::string adapters;
    if (nh_.getParam("request_adapters", adapters))
    {
      try
      {
        adapter_plugin_loader_.reset(new pluginlib::ClassLoader<planning_request_adapter::PlanningRequestAdapter>("planning_request_adapter", "planning_request_adapter::PlanningRequestAdapter"));
      }
      catch(pluginlib::PluginlibException& ex)
      {
        ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what());
      }
      boost::char_separator<char> sep(" ");
      boost::tokenizer<boost::char_separator<char> > tok(adapters, sep);
      std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> ads;
      for(boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin() ; beg != tok.end(); ++beg)
      {
        planning_request_adapter::PlanningRequestAdapterConstPtr ad;
        try
        {
          ad.reset(adapter_plugin_loader_->createUnmanagedInstance(*beg));
        }
        catch (pluginlib::PluginlibException& ex)
        {
          ROS_ERROR_STREAM("Exception while planning adapter plugin '" << *beg << "': " << ex.what());
        }
        if (ad)
          ads.push_back(ad);
      }
      if (!ads.empty())
      {
        adapter_chain_.reset(new planning_request_adapter::PlanningRequestAdapterChain());
        for (std::size_t i = 0 ; i < ads.size() ; ++i)
        {
          ROS_INFO_STREAM("Using planning request adapter '" << ads[i]->getDescription() << "'");
          adapter_chain_->addAdapter(ads[i]);
        }
      }
    }

    display_path_publisher_ = root_nh_.advertise<moveit_msgs::DisplayTrajectory>("move_" + group_name_ + "/" + DISPLAY_PATH_PUB_TOPIC, 1, true);

    // start the action server
    action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::MoveGroupAction>(root_nh_, "move_" + group_name_, false));
    action_server_->registerGoalCallback(boost::bind(&MoveGroupAction::goalCallback, this));
    action_server_->registerPreemptCallback(boost::bind(&MoveGroupAction::preemptCallback, this));
    action_server_->start();
  }

  void goalCallback(void)
  {
    if (service_goal_thread_)
    {
      terminate_service_thread_ = true;
      service_goal_thread_->join();
      service_goal_thread_.reset();
    }
    goal_ = action_server_->acceptNewGoal();
    if (!goal_)
    {
      ROS_ERROR("Something unexpected happened. No goal found in callback for goal...");
      return;
    }

    if (!goal_->request.group_name.empty() && goal_->request.group_name != group_name_)
    {
      moveit_msgs::MoveGroupResult res;
      res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
      action_server_->setAborted(res, "Cannot accept requests for group '" +
                                 goal_->request.group_name  + "' when the move_group action is loaded for group '" +
                                 group_name_ +  "'");
    }
    else
      service_goal_thread_.reset(new boost::thread(boost::bind(&MoveGroupAction::serviceGoalRequest, this)));
  }

  void preemptCallback(void)
  {
    action_server_->setPreempted();
    terminate_service_thread_ = true;
  }

  void serviceGoalRequest(void)
  {
    setState(PLANNING);

    bool solved = false;
    moveit_msgs::GetMotionPlan::Request req;
    req.motion_plan_request = goal_->request;
    if (req.motion_plan_request.group_name.empty())
      req.motion_plan_request.group_name = group_name_;
    moveit_msgs::GetMotionPlan::Response res;

    const planning_scene::PlanningScenePtr &the_scene =
      planning_scene::PlanningScene::isEmpty(goal_->planning_scene_diff) ? psm_.getPlanningScene() : planning_scene::PlanningScene::diff(psm_.getPlanningScene(), goal_->planning_scene_diff);

    try
    {
      if (adapter_chain_)
        solved = adapter_chain_->adaptAndPlan(planner_instance_, the_scene, req, res);
      else
        solved = planner_instance_->solve(the_scene, req, res);
    }
    catch(std::runtime_error &ex)
    {
      ROS_ERROR("Exception caught: '%s'", ex.what());
    }
    catch(...)
    {
      ROS_ERROR("Unknown exception thrown by planner");
    }

    if (solved)
    {
      trajectory_msgs::JointTrajectory trajectory_out;
      smoother_.smooth(res.trajectory.joint_trajectory, trajectory_out, psm_.getGroupJointLimitsMap().at(group_name_));
      res.trajectory.joint_trajectory = trajectory_out;

      setState(MONITOR);
      execution_complete_ = false;

      // display the trajectory
      moveit_msgs::DisplayTrajectory disp;
      disp.model_id = psm_.getPlanningScene()->getKinematicModel()->getName();
      disp.trajectory_start = res.trajectory_start;
      disp.trajectory = res.trajectory;
      display_path_publisher_.publish(disp);

      trajectory_execution::TrajectoryExecutionRequest ter;
      ter.group_name_ = group_name_;
      ter.trajectory_ = res.trajectory.joint_trajectory; // \TODO This should take in a RobotTrajectory
      if (trajectory_execution_->executeTrajectory(ter, boost::bind(&MoveGroupAction::doneWithTrajectoryExecution, this, _1)))
      {
        ros::WallDuration d(0.01);
        while (nh_.ok() && !execution_complete_ && !terminate_service_thread_)
        {
          /// \TODO Check if the remainder of the path is still valid; If not, replan.
          /// We need a callback in the trajectory monitor for this
          d.sleep();
        }
        moveit_msgs::MoveGroupResult res;
        res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
        action_server_->setSucceeded(res, "Solution was found and executed.");
      }
      else
      {
        moveit_msgs::MoveGroupResult res;
        //        res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
        action_server_->setAborted(res, "Solution was found but the controller failed to execute it.");
      }
    }
    else
    {
      moveit_msgs::MoveGroupResult res;
      res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
      action_server_->setAborted(res, "No motion plan found. No execution attempted.");
    }

    setState(IDLE);
  }

  bool doneWithTrajectoryExecution(trajectory_execution::TrajectoryExecutionDataVector data)
  {
    execution_complete_ = true;
    return true;
  }

  void setState(MoveGroupState state)
  {
    state_ = state;
    switch (state_)
    {
    case IDLE:
      feedback_.state = "IDLE";
      feedback_.time_to_completion = ros::Duration(0.0);
      break;
    case PLANNING:
      feedback_.state = "PLANNING";
      feedback_.time_to_completion = ros::Duration(0.0);
      break;
    case MONITOR:
      feedback_.state = "MONITOR";
      feedback_.time_to_completion = ros::Duration(0.0);
      break;
    }
    action_server_->publishFeedback(feedback_);
  }

  void status(void)
  {
    ROS_INFO("MoveGroup action for group '%s' running using planning plugin '%s'", group_name_.c_str(), planning_plugin_name_.c_str());
  }

private:

  ros::NodeHandle root_nh_;
  ros::NodeHandle nh_;
  planning_scene_monitor::PlanningSceneMonitor &psm_;

  std::string planning_plugin_name_;
  boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::Planner> > planner_plugin_loader_;
  planning_interface::PlannerPtr planner_instance_;
  std::string group_name_;

  boost::scoped_ptr<pluginlib::ClassLoader<planning_request_adapter::PlanningRequestAdapter> > adapter_plugin_loader_;
  boost::scoped_ptr<planning_request_adapter::PlanningRequestAdapterChain> adapter_chain_;

  boost::shared_ptr<actionlib::SimpleActionServer<moveit_msgs::MoveGroupAction> > action_server_;
  moveit_msgs::MoveGroupGoalConstPtr goal_;
  moveit_msgs::MoveGroupFeedback feedback_;

  boost::scoped_ptr<boost::thread> service_goal_thread_;

  boost::shared_ptr<trajectory_execution_ros::TrajectoryExecutionMonitorRos> trajectory_execution_;
  bool terminate_service_thread_;
  bool execution_complete_;
  MoveGroupState state_;
  trajectory_processing::IterativeParabolicSmoother smoother_;

  ros::Publisher display_path_publisher_;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_group", ros::init_options::AnonymousName);

  ros::AsyncSpinner spinner(1);
  spinner.start();

  boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener());
  planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION, tf);
  if (psm.getPlanningScene()->isConfigured())
  {
    psm.startWorldGeometryMonitor();
    psm.startSceneMonitor();
    psm.startStateMonitor();

    MoveGroupAction mga(psm);
    mga.status();
    ros::waitForShutdown();
  }
  else
    ROS_ERROR("Planning scene not configured");

  return 0;
}
<|endoftext|>


<commit_before>#include "user_mark_container.hpp"

#include "drawer.hpp"
#include "framework.hpp"
#include "anim_phase_chain.hpp"

#include "../graphics/display_list.hpp"
#include "../graphics/screen.hpp"

#include "../geometry/transformations.hpp"

#include "../anim/task.hpp"
#include "../anim/controller.hpp"

#include "../base/macros.hpp"
#include "../base/stl_add.hpp"

#include "../std/scoped_ptr.hpp"
#include "../std/algorithm.hpp"

////////////////////////////////////////////////////////////////////////

namespace
{
  class PinAnimation : public AnimPhaseChain
  {
  public:
    PinAnimation(Framework & f)
      : AnimPhaseChain(f, m_scale)
      , m_scale(0.0)
    {
      InitDefaultPinAnim(this);
    }

    double GetScale() const
    {
      return m_scale;
    }

  private:
    double m_scale;
  };

  class FindMarkFunctor
  {
  public:
    FindMarkFunctor(UserMark ** mark, double & minD, m2::AnyRectD const & rect)
      : m_mark(mark)
      , m_minD(minD)
      , m_rect(rect)
    {
      m_globalCenter = rect.GlobalCenter();
    }

    void operator()(UserMark * mark)
    {
      m2::PointD const & org = mark->GetOrg();
      if (m_rect.IsPointInside(org))
      {
        double minDCandidate = m_globalCenter.SquareLength(org);
        if (minDCandidate < m_minD)
        {
          *m_mark = mark;
          m_minD = minDCandidate;
        }
      }
    }

    UserMark ** m_mark;
    double & m_minD;
    m2::AnyRectD const & m_rect;
    m2::PointD m_globalCenter;
  };

  void DrawUserMarkByPoint(double scale,
                           double visualScale,
                           m2::PointD const & pixelOfsset,
                           PaintOverlayEvent const & event,
                           graphics::DisplayList * dl,
                           m2::PointD const & ptOrg)
  {
    ScreenBase const & modelView = event.GetModelView();
    graphics::Screen * screen = event.GetDrawer()->screen();
    m2::PointD pxPoint = modelView.GtoP(ptOrg);
    pxPoint += (pixelOfsset * visualScale);
    math::Matrix<double, 3, 3> m = math::Shift(math::Scale(math::Identity<double, 3>(),
                                                           scale, scale),
                                               pxPoint.x, pxPoint.y);
    dl->draw(screen, m);
  }

  void DrawUserMarkImpl(double scale,
                        double visualScale,
                        m2::PointD const & pixelOfsset,
                        PaintOverlayEvent const & event,
                        graphics::DisplayList * dl,
                        UserMark const * mark)
  {
    DrawUserMarkByPoint(scale, visualScale, pixelOfsset, event, dl, mark->GetOrg());
  }

  void DrawUserMark(double scale,
                    double visualScale,
                    PaintOverlayEvent const & event,
                    UserMarkDLCache * cache,
                    UserMarkDLCache::Key const & defaultKey,
                    UserMark const * mark)
  {
    if (mark->IsCustomDrawable())
    {
      ICustomDrawable const * drawable = static_cast<ICustomDrawable const *>(mark);
      DrawUserMarkImpl(drawable->GetAnimScaleFactor(), visualScale, drawable->GetPixelOffset(), event, drawable->GetDisplayList(cache), mark);
    }
    else
      DrawUserMarkImpl(scale, visualScale, m2::PointD(0.0, 0.0), event, cache->FindUserMark(defaultKey), mark);
  }
}

UserMarkContainer::UserMarkContainer(double layerDepth, Framework & fm)
  : m_framework(fm)
  , m_controller(this)
  , m_isVisible(true)
  , m_isDrawable(true)
  , m_layerDepth(layerDepth)
{
}

UserMarkContainer::~UserMarkContainer()
{
  Clear();
}

UserMark const * UserMarkContainer::FindMarkInRect(m2::AnyRectD const & rect, double & d) const
{
  UserMark * mark = NULL;
  if (IsVisible())
  {
    FindMarkFunctor f(&mark, d, rect);
    for_each(m_userMarks.begin(), m_userMarks.end(), f);
  }
  return mark;
}

void UserMarkContainer::Draw(PaintOverlayEvent const & e, UserMarkDLCache * cache) const
{
  if (IsVisible() && IsDrawable())
  {
    UserMarkDLCache::Key defaultKey(GetTypeName(), graphics::EPosCenter, m_layerDepth);
    for_each(m_userMarks.begin(), m_userMarks.end(), bind(&DrawUserMark, 1.0, m_framework.GetVisualScale(),
                                                          e, cache, defaultKey, _1));
  }
}

void UserMarkContainer::Clear()
{
  DeleteRange(m_userMarks, DeleteFunctor());
}

namespace
{
  static scoped_ptr<PoiMarkPoint> s_selectionUserMark;
  static scoped_ptr<MyPositionMarkPoint> s_myPosition;
}

void UserMarkContainer::InitStaticMarks(UserMarkContainer * container)
{
  if (s_selectionUserMark == NULL)
    s_selectionUserMark.reset(new PoiMarkPoint(container));

  if (s_myPosition == NULL)
    s_myPosition.reset(new MyPositionMarkPoint(container));
}

PoiMarkPoint * UserMarkContainer::UserMarkForPoi()
{
  ASSERT(s_selectionUserMark != NULL, ());
  return s_selectionUserMark.get();
}

MyPositionMarkPoint * UserMarkContainer::UserMarkForMyPostion()
{
  ASSERT(s_myPosition != NULL, ());
  return s_myPosition.get();
}

UserMark * UserMarkContainer::CreateUserMark(m2::PointD const & ptOrg)
{
  m_userMarks.push_back(AllocateUserMark(ptOrg));
  return m_userMarks.back();
}

size_t UserMarkContainer::GetUserMarkCount() const
{
  return m_userMarks.size();
}

UserMark const * UserMarkContainer::GetUserMark(size_t index) const
{
  ASSERT_LESS(index, m_userMarks.size(), ());
  return m_userMarks[index];
}

UserMark * UserMarkContainer::GetUserMark(size_t index)
{
  ASSERT_LESS(index, m_userMarks.size(), ());
  return m_userMarks[index];
}

namespace
{

template <class T> void DeleteItem(vector<T> & v, size_t i)
{
  if (i < v.size())
  {
    delete v[i];
    v.erase(v.begin() + i);
  }
  else
  {
    LOG(LWARNING, ("Trying to delete non-existing item at index", i));
  }
}

}

void UserMarkContainer::DeleteUserMark(size_t index)
{
  ASSERT_LESS(index, m_userMarks.size(), ());
  DeleteItem(m_userMarks, index);
}

void UserMarkContainer::DeleteUserMark(UserMark const * mark)
{
  vector<UserMark *>::iterator it = find(m_userMarks.begin(), m_userMarks.end(), mark);
  if (it != m_userMarks.end())
    DeleteUserMark(distance(m_userMarks.begin(), it));
}

SearchUserMarkContainer::SearchUserMarkContainer(double layerDepth, Framework & framework)
  : UserMarkContainer(layerDepth, framework)
{
}

string SearchUserMarkContainer::GetTypeName() const
{
  return "search-result";
}

string SearchUserMarkContainer::GetActiveTypeName() const
{
  return "search-result-active";
}

UserMark * SearchUserMarkContainer::AllocateUserMark(const m2::PointD & ptOrg)
{
  return new SearchMarkPoint(ptOrg, this);
}

ApiUserMarkContainer::ApiUserMarkContainer(double layerDepth, Framework & framework)
  : UserMarkContainer(layerDepth, framework)
{
}

string ApiUserMarkContainer::GetTypeName() const
{
  return "api-result";
}

string ApiUserMarkContainer::GetActiveTypeName() const
{
  return "search-result-active";
}

UserMark * ApiUserMarkContainer::AllocateUserMark(const m2::PointD & ptOrg)
{
  return new ApiMarkPoint(ptOrg, this);
}

SelectionContainer::SelectionContainer(Framework & fm)
  : m_container(NULL)
  , m_fm(fm)
{
}

void SelectionContainer::ActivateMark(UserMark const * userMark, bool needAnim)
{
  if (needAnim)
    KillActivationAnim();
  if (userMark != NULL)
  {
    m_ptOrg = userMark->GetOrg();
    m_container = userMark->GetContainer();
    if (needAnim)
      StartActivationAnim();
  }
  else
    m_container = NULL;
}

void SelectionContainer::Draw(const PaintOverlayEvent & e, UserMarkDLCache * cache) const
{
  if (m_container != NULL)
  {
    UserMarkDLCache::Key defaultKey(m_container->GetActiveTypeName(),
                                    graphics::EPosCenter,
                                    m_container->GetDepth());

    DrawUserMarkByPoint(GetActiveMarkScale(),
                        m_fm.GetVisualScale(),
                        m2::PointD(0, 0),
                        e, cache->FindUserMark(defaultKey),
                        m_ptOrg);
  }
}

bool SelectionContainer::IsActive() const
{
  return m_container != NULL;
}

void SelectionContainer::StartActivationAnim()
{
  m_animTask.reset(new PinAnimation(m_fm));
  m_fm.GetAnimController()->AddTask(m_animTask);
  m_fm.Invalidate();
}

void SelectionContainer::KillActivationAnim()
{
  m_animTask.reset();
}

double SelectionContainer::GetActiveMarkScale() const
{
  if (m_animTask != NULL)
  {
    PinAnimation * a = static_cast<PinAnimation *>(m_animTask.get());
    return a->GetScale();
  }

  return 1.0;
}
<commit_msg>[core] draw activation circle lower then user marks<commit_after>#include "user_mark_container.hpp"

#include "drawer.hpp"
#include "framework.hpp"
#include "anim_phase_chain.hpp"

#include "../graphics/display_list.hpp"
#include "../graphics/screen.hpp"
#include "../graphics/depth_constants.hpp"

#include "../geometry/transformations.hpp"

#include "../anim/task.hpp"
#include "../anim/controller.hpp"

#include "../base/macros.hpp"
#include "../base/stl_add.hpp"

#include "../std/scoped_ptr.hpp"
#include "../std/algorithm.hpp"

////////////////////////////////////////////////////////////////////////

namespace
{
  class PinAnimation : public AnimPhaseChain
  {
  public:
    PinAnimation(Framework & f)
      : AnimPhaseChain(f, m_scale)
      , m_scale(0.0)
    {
      InitDefaultPinAnim(this);
    }

    double GetScale() const
    {
      return m_scale;
    }

  private:
    double m_scale;
  };

  class FindMarkFunctor
  {
  public:
    FindMarkFunctor(UserMark ** mark, double & minD, m2::AnyRectD const & rect)
      : m_mark(mark)
      , m_minD(minD)
      , m_rect(rect)
    {
      m_globalCenter = rect.GlobalCenter();
    }

    void operator()(UserMark * mark)
    {
      m2::PointD const & org = mark->GetOrg();
      if (m_rect.IsPointInside(org))
      {
        double minDCandidate = m_globalCenter.SquareLength(org);
        if (minDCandidate < m_minD)
        {
          *m_mark = mark;
          m_minD = minDCandidate;
        }
      }
    }

    UserMark ** m_mark;
    double & m_minD;
    m2::AnyRectD const & m_rect;
    m2::PointD m_globalCenter;
  };

  void DrawUserMarkByPoint(double scale,
                           double visualScale,
                           m2::PointD const & pixelOfsset,
                           PaintOverlayEvent const & event,
                           graphics::DisplayList * dl,
                           m2::PointD const & ptOrg)
  {
    ScreenBase const & modelView = event.GetModelView();
    graphics::Screen * screen = event.GetDrawer()->screen();
    m2::PointD pxPoint = modelView.GtoP(ptOrg);
    pxPoint += (pixelOfsset * visualScale);
    math::Matrix<double, 3, 3> m = math::Shift(math::Scale(math::Identity<double, 3>(),
                                                           scale, scale),
                                               pxPoint.x, pxPoint.y);
    dl->draw(screen, m);
  }

  void DrawUserMarkImpl(double scale,
                        double visualScale,
                        m2::PointD const & pixelOfsset,
                        PaintOverlayEvent const & event,
                        graphics::DisplayList * dl,
                        UserMark const * mark)
  {
    DrawUserMarkByPoint(scale, visualScale, pixelOfsset, event, dl, mark->GetOrg());
  }

  void DrawUserMark(double scale,
                    double visualScale,
                    PaintOverlayEvent const & event,
                    UserMarkDLCache * cache,
                    UserMarkDLCache::Key const & defaultKey,
                    UserMark const * mark)
  {
    if (mark->IsCustomDrawable())
    {
      ICustomDrawable const * drawable = static_cast<ICustomDrawable const *>(mark);
      DrawUserMarkImpl(drawable->GetAnimScaleFactor(), visualScale, drawable->GetPixelOffset(), event, drawable->GetDisplayList(cache), mark);
    }
    else
      DrawUserMarkImpl(scale, visualScale, m2::PointD(0.0, 0.0), event, cache->FindUserMark(defaultKey), mark);
  }
}

UserMarkContainer::UserMarkContainer(double layerDepth, Framework & fm)
  : m_framework(fm)
  , m_controller(this)
  , m_isVisible(true)
  , m_isDrawable(true)
  , m_layerDepth(layerDepth)
{
}

UserMarkContainer::~UserMarkContainer()
{
  Clear();
}

UserMark const * UserMarkContainer::FindMarkInRect(m2::AnyRectD const & rect, double & d) const
{
  UserMark * mark = NULL;
  if (IsVisible())
  {
    FindMarkFunctor f(&mark, d, rect);
    for_each(m_userMarks.begin(), m_userMarks.end(), f);
  }
  return mark;
}

void UserMarkContainer::Draw(PaintOverlayEvent const & e, UserMarkDLCache * cache) const
{
  if (IsVisible() && IsDrawable())
  {
    UserMarkDLCache::Key defaultKey(GetTypeName(), graphics::EPosCenter, m_layerDepth);
    for_each(m_userMarks.begin(), m_userMarks.end(), bind(&DrawUserMark, 1.0, m_framework.GetVisualScale(),
                                                          e, cache, defaultKey, _1));
  }
}

void UserMarkContainer::Clear()
{
  DeleteRange(m_userMarks, DeleteFunctor());
}

namespace
{
  static scoped_ptr<PoiMarkPoint> s_selectionUserMark;
  static scoped_ptr<MyPositionMarkPoint> s_myPosition;
}

void UserMarkContainer::InitStaticMarks(UserMarkContainer * container)
{
  if (s_selectionUserMark == NULL)
    s_selectionUserMark.reset(new PoiMarkPoint(container));

  if (s_myPosition == NULL)
    s_myPosition.reset(new MyPositionMarkPoint(container));
}

PoiMarkPoint * UserMarkContainer::UserMarkForPoi()
{
  ASSERT(s_selectionUserMark != NULL, ());
  return s_selectionUserMark.get();
}

MyPositionMarkPoint * UserMarkContainer::UserMarkForMyPostion()
{
  ASSERT(s_myPosition != NULL, ());
  return s_myPosition.get();
}

UserMark * UserMarkContainer::CreateUserMark(m2::PointD const & ptOrg)
{
  m_userMarks.push_back(AllocateUserMark(ptOrg));
  return m_userMarks.back();
}

size_t UserMarkContainer::GetUserMarkCount() const
{
  return m_userMarks.size();
}

UserMark const * UserMarkContainer::GetUserMark(size_t index) const
{
  ASSERT_LESS(index, m_userMarks.size(), ());
  return m_userMarks[index];
}

UserMark * UserMarkContainer::GetUserMark(size_t index)
{
  ASSERT_LESS(index, m_userMarks.size(), ());
  return m_userMarks[index];
}

namespace
{

template <class T> void DeleteItem(vector<T> & v, size_t i)
{
  if (i < v.size())
  {
    delete v[i];
    v.erase(v.begin() + i);
  }
  else
  {
    LOG(LWARNING, ("Trying to delete non-existing item at index", i));
  }
}

}

void UserMarkContainer::DeleteUserMark(size_t index)
{
  ASSERT_LESS(index, m_userMarks.size(), ());
  DeleteItem(m_userMarks, index);
}

void UserMarkContainer::DeleteUserMark(UserMark const * mark)
{
  vector<UserMark *>::iterator it = find(m_userMarks.begin(), m_userMarks.end(), mark);
  if (it != m_userMarks.end())
    DeleteUserMark(distance(m_userMarks.begin(), it));
}

SearchUserMarkContainer::SearchUserMarkContainer(double layerDepth, Framework & framework)
  : UserMarkContainer(layerDepth, framework)
{
}

string SearchUserMarkContainer::GetTypeName() const
{
  return "search-result";
}

string SearchUserMarkContainer::GetActiveTypeName() const
{
  return "search-result-active";
}

UserMark * SearchUserMarkContainer::AllocateUserMark(const m2::PointD & ptOrg)
{
  return new SearchMarkPoint(ptOrg, this);
}

ApiUserMarkContainer::ApiUserMarkContainer(double layerDepth, Framework & framework)
  : UserMarkContainer(layerDepth, framework)
{
}

string ApiUserMarkContainer::GetTypeName() const
{
  return "api-result";
}

string ApiUserMarkContainer::GetActiveTypeName() const
{
  return "search-result-active";
}

UserMark * ApiUserMarkContainer::AllocateUserMark(const m2::PointD & ptOrg)
{
  return new ApiMarkPoint(ptOrg, this);
}

SelectionContainer::SelectionContainer(Framework & fm)
  : m_container(NULL)
  , m_fm(fm)
{
}

void SelectionContainer::ActivateMark(UserMark const * userMark, bool needAnim)
{
  if (needAnim)
    KillActivationAnim();
  if (userMark != NULL)
  {
    m_ptOrg = userMark->GetOrg();
    m_container = userMark->GetContainer();
    if (needAnim)
      StartActivationAnim();
  }
  else
    m_container = NULL;
}

void SelectionContainer::Draw(const PaintOverlayEvent & e, UserMarkDLCache * cache) const
{
  if (m_container != NULL)
  {
    UserMarkDLCache::Key defaultKey(m_container->GetActiveTypeName(),
                                    graphics::EPosCenter,
                                    graphics::activePinDepth);

    DrawUserMarkByPoint(GetActiveMarkScale(),
                        m_fm.GetVisualScale(),
                        m2::PointD(0, 0),
                        e, cache->FindUserMark(defaultKey),
                        m_ptOrg);
  }
}

bool SelectionContainer::IsActive() const
{
  return m_container != NULL;
}

void SelectionContainer::StartActivationAnim()
{
  m_animTask.reset(new PinAnimation(m_fm));
  m_fm.GetAnimController()->AddTask(m_animTask);
  m_fm.Invalidate();
}

void SelectionContainer::KillActivationAnim()
{
  m_animTask.reset();
}

double SelectionContainer::GetActiveMarkScale() const
{
  if (m_animTask != NULL)
  {
    PinAnimation * a = static_cast<PinAnimation *>(m_animTask.get());
    return a->GetScale();
  }

  return 1.0;
}
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/pm/p9_check_proc_config.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */

#include <stdint.h>

///
/// @file   p9_check_proc_config.C
/// @brief  describes interface for a HWP that generates a bit vector corresponding to P9 chip config.
///
/// *HWP HWP Owner:      Greg Still <stillgs@us.ibm.com>
/// *HWP FW Owner:       Prem S Jha <premjha2@in.ibm.com>
/// *HWP Team:           PM
/// *HWP Level:          3
/// *HWP Consumed by:    Hostboot:Phyp
//

//--------------------------------------------------------------------------
// Includes
//--------------------------------------------------------------------------
#include <p9_check_proc_config.H>
#include <p9_hcd_memmap_base.H>
#include <endian.h>
#include <map>
#include <stdint.h>

#ifndef __HOSTBOOT_MODULE
    std::map< fapi2::TargetType, std::string > g_targetTypeMap;
#endif
/**
 * @brief   misc local constants
 */
enum
{
    INIT_CONFIG_VALUE       =       0x8000000C09800000ull,
    QPMR_PROC_CONFIG_POS    =       0xBFC18,
};

/**
 * @brief   bit position for various chiplets in config vector.
 */
enum
{
    MCS_POS             =   1,
    MBA_POS             =   9,
    MEM_BUF_POS         =   17,
    XBUS_POS            =   25,
    PHB_POS             =   30,
    CAPP_POS            =   37,
    OBUS_POS            =   41,
    NVLINK_POS          =   45,
    OBUS_BRICK_0_POS    =   0,
    OBUS_BRICK_1_POS    =   1,
    OBUS_BRICK_2_POS    =   2,
    OBUS_BRICK_9_POS    =   9,
    OBUS_BRICK_10_POS   =   10,
    OBUS_BRICK_11_POS   =   11,

};

/**
 * @brief   validates the input arguments for the HWP.
 * @param[in]   i_procTgt       fapi2 target for P9 chip.
 * @param[in]   i_pHomerImage   pointer to HOMER base
 */
fapi2::ReturnCode validateInputArgs( CONST_FAPI2_PROC& i_procTgt, void*  i_pHomerImage )
{
    FAPI_INF( ">> validateInputArgs" );

    FAPI_ASSERT( i_procTgt.isFunctional(),
                 fapi2::BAD_PROC_TARGET()
                 .set_TARGET( i_procTgt )
                 .set_INPUT_BUF( i_pHomerImage ),
                 "Bad Proc Target Passed As Input" );

    FAPI_ASSERT( ( i_pHomerImage != NULL ),
                 fapi2::BAD_INPUT_BUFFER()
                 .set_TARGET( i_procTgt )
                 .set_INPUT_BUF( i_pHomerImage ),
                 "Bad Buffer Is Passed As Input" );
fapi_try_exit:

    FAPI_INF( "<< validateInputArgs" );
    return fapi2::current_err;
}

//-----------------------------------------------------------------------------------------

/**
 * @brief   checks Memory buf configuration and updates config vector buffer.
 * @param[in]   i_procTgt       fapi2 target for P9
 * @param[in]   i_configBuf     fapi2 buffer
 * @return      fapi2 return code
 */
fapi2::ReturnCode checkMemConfig( CONST_FAPI2_PROC& i_procTgt, uint64_t& io_configVector )
{
    FAPI_INF( ">> checkMemConfig" );
    auto l_dmiChiplets = i_procTgt.getChildren<fapi2::TARGET_TYPE_DMI>( fapi2::TARGET_STATE_PRESENT );

    uint64_t l_tempVector = 0;

    for( auto l_dmi : l_dmiChiplets )
    {
        auto l_cenChips = l_dmi.getChildren<fapi2::TARGET_TYPE_MEMBUF_CHIP>( fapi2::TARGET_STATE_PRESENT );

        for( auto l_cent : l_cenChips )
        {
            auto l_mbaChiplets = l_cent.getChildren<fapi2::TARGET_TYPE_MBA>( fapi2::TARGET_STATE_PRESENT );
            uint8_t l_memBufPos = 0;
            uint8_t l_memBufBitPos = 0;

            //DMI Pos same as membuf position
            FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_CHIP_UNIT_POS, l_dmi, l_memBufPos ),
                      "Failed to get membuf position ");

            if( l_cent.isFunctional( ) )
            {
                l_tempVector = 0x8000000000000000ull;
                l_memBufBitPos = l_memBufPos + MEM_BUF_POS;
                io_configVector |= ( l_tempVector >> l_memBufBitPos ) ;

                FAPI_INF("Mem Buf Pos 0x%04x Bit Pos 0x%04x",
                         l_memBufPos, l_memBufBitPos );
            }

            for( auto l_mba : l_mbaChiplets )
            {
                if( l_mba.isFunctional( ) )
                {
                    uint8_t l_mbaPos = 0;
                    uint8_t l_mbaBitPos = 0;
                    FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_CHIP_UNIT_POS, l_mba, l_mbaPos ),
                              "Failed to get mba position ");

                    l_mbaBitPos = ( l_mbaChiplets.size() * l_memBufPos ) + l_mbaPos + MBA_POS;
                    l_tempVector = 0x8000000000000000ull;
                    io_configVector |= ( l_tempVector >> l_mbaBitPos );

                    FAPI_INF("Mba Pos 0x%04x Bit Pos 0x%04x",
                             l_mbaPos, l_mbaBitPos );
                }
            }
        }
    }

fapi_try_exit:
    FAPI_INF( ">> checkMemConfig" );
    return fapi2::current_err;
}

/**
 * @brief   checks Memory buf configuration and updates config vector buffer.
 * @param[in]   i_procTgt           fapi2 target for P9
 * @param[in]   io_configVector     Unit Avaialability Vector
 * @param[in]   i_oBusStartPos      start bit position for OBUS
 * @param[in]   i_nvLinkPos         start bit position for NV Link
 * @return      fapi2 return code
 */
fapi2::ReturnCode checkObusChipletHierarchy( CONST_FAPI2_PROC& i_procTgt,
        uint64_t& io_configVector, uint8_t i_oBusStartPos, uint8_t i_nvLinkPos )
{
    auto l_obusList =
        i_procTgt.getChildren < fapi2::TARGET_TYPE_OBUS > ( fapi2::TARGET_STATE_PRESENT );

    uint64_t l_tempVector = 0;

    for( auto itv : l_obusList )
    {
        uint8_t l_oBusPos = 0;

        FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_CHIP_UNIT_POS, itv, l_oBusPos ),
                  "Failed to get chiplet position " );

        if( itv.isFunctional() )
        {
            uint8_t l_configPos = i_oBusStartPos + l_oBusPos;
            uint8_t l_configMode = 0;
            auto l_oBusBrickList = itv.getChildren<fapi2::TARGET_TYPE_OBUS_BRICK>( fapi2::TARGET_STATE_FUNCTIONAL );

            l_tempVector = 0x8000000000000000ull;

            FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_OPTICS_CONFIG_MODE, itv, l_configMode ),
                      "Failed To Read ATTR_OPTICS_CONFIG_MODE" );

            for ( auto l_obusBrick : l_oBusBrickList )
            {
                uint8_t l_brickPos   = 0;
                uint8_t l_brickBitPos = 0;

                if( fapi2::ENUM_ATTR_OPTICS_CONFIG_MODE_NV == l_configMode )
                {
                    FAPI_TRY( FAPI_ATTR_GET(fapi2::ATTR_CHIP_UNIT_POS, l_obusBrick, l_brickPos ),
                              "Error from FAPI_ATTR_GET(ATTR_CHIP_UNIT_POS)" );

                    switch( l_brickPos )
                    {
                        case OBUS_BRICK_0_POS:
                        case OBUS_BRICK_1_POS:
                        case OBUS_BRICK_2_POS:

                            l_brickBitPos   =   l_brickPos + i_nvLinkPos;

                            break;

                        case OBUS_BRICK_9_POS:
                            l_brickBitPos   =   i_nvLinkPos + 3;
                            break;

                        case OBUS_BRICK_10_POS:
                            l_brickBitPos   =   i_nvLinkPos + 4;
                            break;

                        case OBUS_BRICK_11_POS:
                            l_brickBitPos   =   i_nvLinkPos + 5;
                            break;

                        default:

                            FAPI_ASSERT_NOEXIT( ( false ),
                                                fapi2::BAD_OBUS_BRICK_POSITION()
                                                .set_TARGET( l_obusBrick )
                                                .set_OBRICK_POS( l_brickPos ),
                                                "Bad or unexpected Obus brick position" );

                    }

                    io_configVector |=  l_tempVector >> l_brickBitPos;

#ifndef __HOSTBOOT_MODULE

                    FAPI_INF( "OBus Brick Pos %02d Bit Pos 0x%02x (%d) UAV 0x%016lx",
                              l_brickPos, l_brickBitPos, l_brickBitPos, io_configVector );

#endif
                }
            }

            io_configVector |= (l_tempVector >> l_configPos);

            FAPI_INF( "Obus Pos 0x%08x Bit Pos 0x%02x UAV 0x%016lx",
                      l_oBusPos, l_configPos, io_configVector );
        }
    }

fapi_try_exit:

    FAPI_INF( "<< p9_check_proc_config" );
    return fapi2::current_err;
}

//-----------------------------------------------------------------------------------------

fapi2::ReturnCode p9_check_proc_config ( CONST_FAPI2_PROC& i_procTgt, void* i_pHomerImage )
{
    FAPI_INF( ">> p9_check_proc_config" );

    uint64_t l_configVectVal = INIT_CONFIG_VALUE;
    uint8_t* pHomer = (uint8_t*)i_pHomerImage + QPMR_HOMER_OFFSET +
                      QPMR_PROC_CONFIG_POS;

#ifndef __HOSTBOOT_MODULE
    g_targetTypeMap[fapi2::TARGET_TYPE_MCS]         =   "MCS";
    g_targetTypeMap[fapi2::TARGET_TYPE_XBUS]        =   "XBUS";
    g_targetTypeMap[fapi2::TARGET_TYPE_OBUS]        =   "OBUS";
    g_targetTypeMap[fapi2::TARGET_TYPE_CAPP]        =   "CAPP";
    g_targetTypeMap[fapi2::TARGET_TYPE_PHB]         =   "PHB";
    g_targetTypeMap[fapi2::TARGET_TYPE_MEMBUF_CHIP] =   "MEM BUF";
    g_targetTypeMap[fapi2::TARGET_TYPE_MBA]         =   "MBA";
    g_targetTypeMap[fapi2::TARGET_TYPE_OBUS_BRICK]  =   "OBUS_BRICK";
#endif

    FAPI_TRY( validateInputArgs( i_procTgt, i_pHomerImage ),
              "Input Arguments Found Invalid" );

    FAPI_TRY( checkChiplet< fapi2::TARGET_TYPE_MCS >
              ( i_procTgt, fapi2::TARGET_TYPE_MCS, l_configVectVal, MCS_POS ),
              "Failed to get MCS configuration" );

    FAPI_TRY( checkChiplet< fapi2::TARGET_TYPE_XBUS>
              ( i_procTgt, fapi2::TARGET_TYPE_XBUS, l_configVectVal, XBUS_POS ),
              "Failed to get XBUS configuration" );

    FAPI_TRY( checkChiplet<fapi2::TARGET_TYPE_PHB>
              ( i_procTgt, fapi2::TARGET_TYPE_PHB, l_configVectVal, PHB_POS ),
              "Failed to get PHB configuration" );

    FAPI_TRY( checkChiplet<fapi2::TARGET_TYPE_CAPP>
              ( i_procTgt, fapi2::TARGET_TYPE_CAPP, l_configVectVal, CAPP_POS ),
              "Failed to get CAPP configuration" );

    FAPI_TRY(  checkObusChipletHierarchy ( i_procTgt, l_configVectVal, OBUS_POS, NVLINK_POS ),
               "Failed to get OBUS Hierarchy configuration" );

    FAPI_TRY( checkMemConfig( i_procTgt, l_configVectVal ),
              "Failed to get Memory  configuration" );

    FAPI_INF( "Updating Vector in HOMER" );
    *(uint64_t*)pHomer  = htobe64( l_configVectVal );

    FAPI_IMP( "Config Vector is 0x%016lx  ", l_configVectVal );
    FAPI_IMP( "Reading back 0x%016lx  ", htobe64( (*(uint64_t*)pHomer)) );

fapi_try_exit:

    FAPI_INF( "<< p9_check_proc_config" );
    return fapi2::current_err;
}

//-----------------------------------------------------------------------------------------
<commit_msg>24x7: Corrected handling of MCA on a direct attached systems.<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/pm/p9_check_proc_config.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */

#include <stdint.h>

///
/// @file   p9_check_proc_config.C
/// @brief  describes interface for a HWP that generates a bit vector corresponding to P9 chip config.
///
/// *HWP HWP Owner:      Greg Still <stillgs@us.ibm.com>
/// *HWP FW Owner:       Prem S Jha <premjha2@in.ibm.com>
/// *HWP Team:           PM
/// *HWP Level:          3
/// *HWP Consumed by:    Hostboot:Phyp
//

//--------------------------------------------------------------------------
// Includes
//--------------------------------------------------------------------------
#include <p9_check_proc_config.H>
#include <p9_hcd_memmap_base.H>
#include <endian.h>
#include <map>
#include <stdint.h>

#ifndef __HOSTBOOT_MODULE
    std::map< fapi2::TargetType, std::string > g_targetTypeMap;
#endif
/**
 * @brief   misc local constants
 */
enum
{
    INIT_CONFIG_VALUE       =       0x8000000C09800000ull,
    QPMR_PROC_CONFIG_POS    =       0xBFC18,
};

/**
 * @brief   bit position for various chiplets in config vector.
 */
enum
{
    MCS_POS             =   1,
    MBA_POS             =   9,
    MEM_BUF_POS         =   17,
    XBUS_POS            =   25,
    PHB_POS             =   30,
    CAPP_POS            =   37,
    OBUS_POS            =   41,
    NVLINK_POS          =   45,
    OBUS_BRICK_0_POS    =   0,
    OBUS_BRICK_1_POS    =   1,
    OBUS_BRICK_2_POS    =   2,
    OBUS_BRICK_9_POS    =   9,
    OBUS_BRICK_10_POS   =   10,
    OBUS_BRICK_11_POS   =   11,

};

/**
 * @brief   validates the input arguments for the HWP.
 * @param[in]   i_procTgt       fapi2 target for P9 chip.
 * @param[in]   i_pHomerImage   pointer to HOMER base
 */
fapi2::ReturnCode validateInputArgs( CONST_FAPI2_PROC& i_procTgt, void*  i_pHomerImage )
{
    FAPI_INF( ">> validateInputArgs" );

    FAPI_ASSERT( i_procTgt.isFunctional(),
                 fapi2::BAD_PROC_TARGET()
                 .set_TARGET( i_procTgt )
                 .set_INPUT_BUF( i_pHomerImage ),
                 "Bad Proc Target Passed As Input" );

    FAPI_ASSERT( ( i_pHomerImage != NULL ),
                 fapi2::BAD_INPUT_BUFFER()
                 .set_TARGET( i_procTgt )
                 .set_INPUT_BUF( i_pHomerImage ),
                 "Bad Buffer Is Passed As Input" );
fapi_try_exit:

    FAPI_INF( "<< validateInputArgs" );
    return fapi2::current_err;
}

//-----------------------------------------------------------------------------------------

/**
 * @brief   checks Memory buf configuration and updates config vector buffer.
 * @param[in]   i_procTgt       fapi2 target for P9
 * @param[in]   i_configBuf     fapi2 buffer
 * @return      fapi2 return code
 */
fapi2::ReturnCode checkMemConfig( CONST_FAPI2_PROC& i_procTgt, uint64_t& io_configVector )
{
    FAPI_INF( ">> checkMemConfig" );
    auto l_dmiChiplets = i_procTgt.getChildren<fapi2::TARGET_TYPE_DMI>( fapi2::TARGET_STATE_PRESENT );

    uint64_t l_tempVector = 0;

    for( auto l_dmi : l_dmiChiplets )
    {
        auto l_cenChips = l_dmi.getChildren<fapi2::TARGET_TYPE_MEMBUF_CHIP>( fapi2::TARGET_STATE_PRESENT );

        for( auto l_cent : l_cenChips )
        {
            auto l_mbaChiplets = l_cent.getChildren<fapi2::TARGET_TYPE_MBA>( fapi2::TARGET_STATE_PRESENT );
            uint8_t l_memBufPos = 0;
            uint8_t l_memBufBitPos = 0;

            //DMI Pos same as membuf position
            FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_CHIP_UNIT_POS, l_dmi, l_memBufPos ),
                      "Failed to get membuf position ");

            if( l_cent.isFunctional( ) )
            {
                l_tempVector = 0x8000000000000000ull;
                l_memBufBitPos = l_memBufPos + MEM_BUF_POS;
                io_configVector |= ( l_tempVector >> l_memBufBitPos ) ;

                FAPI_INF("Mem Buf Pos 0x%04x Bit Pos 0x%04x",
                         l_memBufPos, l_memBufBitPos );
            }

            for( auto l_mba : l_mbaChiplets )
            {
                if( l_mba.isFunctional( ) )
                {
                    uint8_t l_mbaPos = 0;
                    uint8_t l_mbaBitPos = 0;
                    FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_CHIP_UNIT_POS, l_mba, l_mbaPos ),
                              "Failed to get mba position ");

                    l_mbaBitPos = ( l_mbaChiplets.size() * l_memBufPos ) + l_mbaPos + MBA_POS;
                    l_tempVector = 0x8000000000000000ull;
                    io_configVector |= ( l_tempVector >> l_mbaBitPos );

                    FAPI_INF("Mba Pos 0x%04x Bit Pos 0x%04x",
                             l_mbaPos, l_mbaBitPos );
                }
            }
        }
    }

fapi_try_exit:
    FAPI_INF( ">> checkMemConfig" );
    return fapi2::current_err;
}

/**
 * @brief   checks Memory buf configuration and updates config vector buffer.
 * @param[in]   i_procTgt           fapi2 target for P9
 * @param[in]   io_configVector     Unit Avaialability Vector
 * @param[in]   i_oBusStartPos      start bit position for OBUS
 * @param[in]   i_nvLinkPos         start bit position for NV Link
 * @return      fapi2 return code
 */
fapi2::ReturnCode checkObusChipletHierarchy( CONST_FAPI2_PROC& i_procTgt,
        uint64_t& io_configVector, uint8_t i_oBusStartPos, uint8_t i_nvLinkPos )
{
    auto l_obusList =
        i_procTgt.getChildren < fapi2::TARGET_TYPE_OBUS > ( fapi2::TARGET_STATE_PRESENT );

    uint64_t l_tempVector = 0;

    for( auto itv : l_obusList )
    {
        uint8_t l_oBusPos = 0;

        FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_CHIP_UNIT_POS, itv, l_oBusPos ),
                  "Failed to get chiplet position " );

        if( itv.isFunctional() )
        {
            uint8_t l_configMode = 0;
            auto l_oBusBrickList = itv.getChildren<fapi2::TARGET_TYPE_OBUS_BRICK>( fapi2::TARGET_STATE_FUNCTIONAL );

            l_tempVector = 0x8000000000000000ull;

            FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_OPTICS_CONFIG_MODE, itv, l_configMode ),
                      "Failed To Read ATTR_OPTICS_CONFIG_MODE" );

            for ( auto l_obusBrick : l_oBusBrickList )
            {
                uint8_t l_brickPos   = 0;
                uint8_t l_brickBitPos = 0;

                if( fapi2::ENUM_ATTR_OPTICS_CONFIG_MODE_NV == l_configMode )
                {
                    FAPI_TRY( FAPI_ATTR_GET(fapi2::ATTR_CHIP_UNIT_POS, l_obusBrick, l_brickPos ),
                              "Error from FAPI_ATTR_GET(ATTR_CHIP_UNIT_POS)" );

                    switch( l_brickPos )
                    {
                        case OBUS_BRICK_0_POS:
                        case OBUS_BRICK_1_POS:
                        case OBUS_BRICK_2_POS:

                            l_brickBitPos   =   l_brickPos + i_nvLinkPos;

                            break;

                        case OBUS_BRICK_9_POS:
                            l_brickBitPos   =   i_nvLinkPos + 3;
                            break;

                        case OBUS_BRICK_10_POS:
                            l_brickBitPos   =   i_nvLinkPos + 4;
                            break;

                        case OBUS_BRICK_11_POS:
                            l_brickBitPos   =   i_nvLinkPos + 5;
                            break;

                        default:

                            FAPI_ASSERT_NOEXIT( ( false ),
                                                fapi2::BAD_OBUS_BRICK_POSITION()
                                                .set_TARGET( l_obusBrick )
                                                .set_OBRICK_POS( l_brickPos ),
                                                "Bad or unexpected Obus brick position" );
                            break;

                    }

                    io_configVector |=  l_tempVector >> l_brickBitPos;

#ifndef __HOSTBOOT_MODULE

                    FAPI_INF( "OBus Brick Pos %02d Bit Pos 0x%02x (%d) UAV 0x%016lx",
                              l_brickPos, l_brickBitPos, l_brickBitPos, io_configVector );

#endif
                }
            }
        }
    }

fapi_try_exit:

    FAPI_INF( "<< p9_check_proc_config" );
    return fapi2::current_err;
}

//-----------------------------------------------------------------------------------------

fapi2::ReturnCode p9_check_proc_config ( CONST_FAPI2_PROC& i_procTgt, void* i_pHomerImage )
{
    FAPI_INF( ">> p9_check_proc_config" );

    uint64_t l_configVectVal = INIT_CONFIG_VALUE;
    uint8_t* pHomer = (uint8_t*)i_pHomerImage + QPMR_HOMER_OFFSET +
                      QPMR_PROC_CONFIG_POS;
    uint8_t l_chipName = 0;
#ifndef __HOSTBOOT_MODULE
    g_targetTypeMap[fapi2::TARGET_TYPE_MCS]         =   "MCS";
    g_targetTypeMap[fapi2::TARGET_TYPE_MCA]         =   "MCA";
    g_targetTypeMap[fapi2::TARGET_TYPE_XBUS]        =   "XBUS";
    g_targetTypeMap[fapi2::TARGET_TYPE_OBUS]        =   "OBUS";
    g_targetTypeMap[fapi2::TARGET_TYPE_CAPP]        =   "CAPP";
    g_targetTypeMap[fapi2::TARGET_TYPE_PHB]         =   "PHB";
    g_targetTypeMap[fapi2::TARGET_TYPE_MEMBUF_CHIP] =   "MEM BUF";
    g_targetTypeMap[fapi2::TARGET_TYPE_MBA]         =   "MBA";
    g_targetTypeMap[fapi2::TARGET_TYPE_OBUS_BRICK]  =   "OBUS_BRICK";
#endif

    FAPI_TRY( validateInputArgs( i_procTgt, i_pHomerImage ),
              "Input Arguments Found Invalid" );

    FAPI_TRY( checkChiplet< fapi2::TARGET_TYPE_MCS >
              ( i_procTgt, fapi2::TARGET_TYPE_MCS, l_configVectVal, MCS_POS ),
              "Failed to get MCS configuration" );

    FAPI_TRY( checkChiplet< fapi2::TARGET_TYPE_XBUS>
              ( i_procTgt, fapi2::TARGET_TYPE_XBUS, l_configVectVal, XBUS_POS ),
              "Failed to get XBUS configuration" );

    FAPI_TRY( checkChiplet<fapi2::TARGET_TYPE_PHB>
              ( i_procTgt, fapi2::TARGET_TYPE_PHB, l_configVectVal, PHB_POS ),
              "Failed to get PHB configuration" );

    FAPI_TRY( checkChiplet<fapi2::TARGET_TYPE_CAPP>
              ( i_procTgt, fapi2::TARGET_TYPE_CAPP, l_configVectVal, CAPP_POS ),
              "Failed to get CAPP configuration" );

    FAPI_TRY(  checkObusChipletHierarchy ( i_procTgt, l_configVectVal, OBUS_POS, NVLINK_POS ),
               "Failed to get OBUS Hierarchy configuration" );

    FAPI_ATTR_GET_PRIVILEGED(fapi2::ATTR_NAME, i_procTgt, l_chipName );

    if( fapi2::ENUM_ATTR_NAME_CUMULUS == l_chipName )
    {
        FAPI_TRY( checkMemConfig( i_procTgt, l_configVectVal ),
                  "Failed to get Memory  configuration" );
    }
    else
    {
        //FIXME: RTC 180154 Needs Review for Axone

        FAPI_TRY( checkChiplet< fapi2::TARGET_TYPE_MCA >
                  ( i_procTgt, fapi2::TARGET_TYPE_MCA, l_configVectVal, MBA_POS ),
                  "Failed to get MCA Position" );
    }

    FAPI_INF( "Updating Vector in HOMER" );
    *(uint64_t*)pHomer  = htobe64( l_configVectVal );

    FAPI_IMP( "Config Vector is 0x%016lx  ", l_configVectVal );
    FAPI_IMP( "Reading back 0x%016lx  ", htobe64( (*(uint64_t*)pHomer)) );

fapi_try_exit:

    FAPI_INF( "<< p9_check_proc_config" );
    return fapi2::current_err;
}

//-----------------------------------------------------------------------------------------
<|endoftext|>


<commit_before><commit_msg>coverity#440825 Logically dead code<commit_after><|endoftext|>


<commit_before><commit_msg>Fix debug build without dbgutil<commit_after><|endoftext|>


<commit_before><commit_msg>Changed SetFillColor to SetTextFillColor<commit_after><|endoftext|>


<commit_before>#include <iostream>

int gcd(int a, int b) {
    while (b) {
        int temp = b;
        b = a % b;
        a = temp;
    }
    return a;
}

int lcm(int a, int b) {
    return (a * b) / gcd(a, b);
}

void lcmAndGcd(int* arr, int size) {
    int lcm1 = lcm(arr[0], arr[1]);
    int gcd1 = gcd(arr[0], arr[1]);
    for (int i = 2; i < size; i++) {
        lcm1 = lcm(lcm1, arr[i]);
        gcd1 = gcd(gcd1, arr[i]);
    }
    std::cout << lcm1 << std::endl;
    std::cout << gcd1 << std::endl;
}

int main() {
    int n;
    std::cin >> n;
    int* arr = new int[n];
    for (int i = 0; i < n; i++) {
        std::cin >> arr[i];
    }
    lcmAndGcd(arr, n);
    delete[] arr;
    return 0;
}
<commit_msg>Copyright line<commit_after>/*
 * Copyright (C) 2015 Pavel Dolgov
 *
 * See the LICENSE file for terms of use.
 */

#include <iostream>

int gcd(int a, int b) {
    while (b) {
        int temp = b;
        b = a % b;
        a = temp;
    }
    return a;
}

int lcm(int a, int b) {
    return (a * b) / gcd(a, b);
}

void lcmAndGcd(int* arr, int size) {
    int lcm1 = lcm(arr[0], arr[1]);
    int gcd1 = gcd(arr[0], arr[1]);
    for (int i = 2; i < size; i++) {
        lcm1 = lcm(lcm1, arr[i]);
        gcd1 = gcd(gcd1, arr[i]);
    }
    std::cout << lcm1 << std::endl;
    std::cout << gcd1 << std::endl;
}

int main() {
    int n;
    std::cin >> n;
    int* arr = new int[n];
    for (int i = 0; i < n; i++) {
        std::cin >> arr[i];
    }
    lcmAndGcd(arr, n);
    delete[] arr;
    return 0;
}
<|endoftext|>